diff --git a/.github/workflows/rapier-ci-build.yml b/.github/workflows/rapier-ci-build.yml
index 1b29d15..c38a1a5 100644
--- a/.github/workflows/rapier-ci-build.yml
+++ b/.github/workflows/rapier-ci-build.yml
@@ -13,7 +13,7 @@ jobs:
   check-fmt:
     runs-on: ubuntu-latest
     steps:
-    - uses: actions/checkout@v3
+    - uses: actions/checkout@v4
     - name: Check formatting
       run: cargo fmt -- --check
   build-native:
@@ -21,7 +21,7 @@ jobs:
     env:
       RUSTFLAGS: -D warnings
     steps:
-    - uses: actions/checkout@v3
+    - uses: actions/checkout@v4
     - run: sudo apt-get install -y cmake libxcb-composite0-dev
     - name: Clippy
       run: cargo clippy
@@ -60,7 +60,7 @@ jobs:
     env:
       RUSTFLAGS: -D warnings
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
       - run: rustup target add wasm32-unknown-unknown
       - name: build rapier2d
         run: cd crates/rapier2d && cargo build --verbose --features wasm-bindgen --target wasm32-unknown-unknown;
@@ -71,7 +71,7 @@ jobs:
     env:
       RUSTFLAGS: -D warnings
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
       - run: rustup target add wasm32-unknown-emscripten
       - name: build rapier2d
         run: cd crates/rapier2d && cargo build --verbose --target wasm32-unknown-emscripten;
diff --git a/CHANGELOG.md b/CHANGELOG.md
index 9f57869..f9ec4d1 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,8 +1,96 @@
+## Unreleased
+
+### Added
+
+- Add `Multibody::inverse_kinematics`, `Multibody::inverse_kinematics_delta`,
+  and `::inverse_kinematics_delta_with_jacobian`
+  for running inverse kinematics on a multibody to align one its links pose to the given prescribed pose.
+- Add `InverseKinematicsOption` to customize some behaviors of the inverse-kinematics solver.
+- Add `Multibody::body_jacobian` to get the jacobian of a specific link.
+- Add `Multibody::update_rigid_bodies` to update rigid-bodies based on the multibody links poses.
+- Add `Multibody::forward_kinematics_single_link` to run forward-kinematics to compute the new pose and jacobian of a
+  single link without mutating the multibody. This can take an optional displacement on generalized coordinates that are
+  taken into account during transform propagation.
+
+### Modified
+
+- The contact constraints regularization parameters have been changed from `erp/damping_ratio` to
+  `natural_frequency/damping_ratio`. This helps define them in a timestep-length independent way. The new variables
+  are named `IntegrationParameters::contact_natural_frequency` and `IntegrationParameters::contact_damping_ratio`.
+- The `IntegrationParameters::normalized_max_penetration_correction` has been replaced
+  by `::normalized_max_corrective_velocity`
+  to make the parameter more timestep-length independent. It is now set to a non-infinite value to eliminate aggressive
+  "popping effects".
+- The `Multibody::forward_kinematics` method will no longer automatically update the poses of the `RigidBody` associated
+  to each joint. Instead `Multibody::update_rigid_bodies` has to be called explicitly.
+- The `Multibody::forward_kinematics` method will automatically adjust the multibody’s degrees of freedom if the root
+  rigid-body changed type (between dynamic and non-dynamic). It can also optionally apply the root’s rigid-body pose
+  instead of the root link’s pose (useful for example if you modified the root rigid-body pose externally and wanted
+  to propagate it to the multibody).
+- Remove an internal special-case for contact constraints on fast contacts. The doesn’t seem necessary with the substep
+  solver.
+
+## v0.19.0 (05 May 2024)
+
+### Fix
+
+- Fix crash when simulating a spring joint between two dynamic bodies.
+- Fix kinematic bodies not being affected by gravity after being switched back to dynamic.
+- Fix regression on contact force reporting from contact force events.
+- Fix kinematic character controller getting stuck against vertical walls.
+- Fix joint limits/motors occasionally not being applied properly when one of the attached
+  rigid-bodies is fixed.
+- Fix an issue where contacts would be completely ignored between two convex shapes.
+
+### Added
+
+**Many stability improvements were added as part of this release. To see illustrations of some of these
+changes, see [#625](https://github.com/dimforge/rapier/pull/625).**
+
+- Add `RigidBody::predict_position_using_velocity` to predict the next position of the rigid-body
+  based only on its current velocity.
+- Add `Collider::copy_from` to copy most collider attributes to an existing collider.
+- Add `RigidBody::copy_from` to copy most rigid-body attributes to an existing rigid-body.
+- Add the `BroadPhase` trait and expect an implementor of this trait as input to `PhysicsPipeline::step`.
+- Implement a 2D block-solver as well as warmstarting. Significantly improves stacking capabilities. Generally reduces
+  the "pop" effect that can happen due to penetration corrections.
+- Add `RigidBodyBuilder::soft_ccd_prediction` and `RigidBody::set_soft_ccd_prediction` to enable `soft-ccd`: a form of
+  CCD based on predictive contacts. This is helpful for objects moving moderately fast. This form of CCD is generally
+  cheaper than the normal (time-dropping) CCD implemented so far. It is possible to combine both soft-ccd and
+  time-dropping ccd.
+- Add a `ColliderBuilder::contact_skin`, `Collider::set_contact_skin`, and `Collider::contact_skin`. This forces the
+  solver te maintain a gap between colliders with non-zero contact skin, as if they had a slight margin around them.
+  This helps performance and stability for thin objects (like triangle meshes).
+- Internal edges were reworked to avoid dropping contacts that would help with stability, and improve stability of
+  collisions between two triangle meshes. The `TriMeshFlags::FIX_INTERNAL_EDGES` and
+  `HeightFieldFlags::FIX_INTERNAL_EDGES` flags were added to enable internal edges handling.
+- Add `IntegrationParameters::length_units` to automatically adjust internal thresholds when the user relies on custom
+  length units (e.g. pixels in 2D).
+
+### Modified
+
+**Many shape-casting related functions/structs were renamed. Check out the CHANGELOG for parry 0.15.0 for
+additional details.**
+
+- Renamed `BroadPhase` to `BroadPhaseMultiSap`. The `BroadPhase` is now a trait that can be
+  implemented for providing a custom broad-phase to rapier. Equivalently, the `DefaultBroadPhase` type
+  alias can be used in place of `BroadPhaseMultiSap`.
+- The kinematic character controller autostepping is now disabled by default.
+- Add `KinematicCharacterController::normal_nudge_factor` used to help getting the character unstuck
+  due to rounding errors.
+- Rename `TOI` to `ShapeCastHit`.
+- Rename many fields from `toi` to `time_of_impact`.
+- The `QueryPipeline::cast_shape` method now takes a `ShapeCastOptions` instead of the `max_toi`
+  and `stop_at_penetration` parameters. This allows a couple of extra configurations, including the
+  ability to have the shape-cast target a specific distance instead of actual shape overlap.
+
 ## v0.18.0 (24 Jan. 2024)
+
 The main highlight of this release is the implementation of a new non-linear constraints solver for better stability
 and increased convergence rates. See [#579](https://github.com/dimforge/rapier/pull/579) for additional information.
 
-In order to adjust the number of iterations of the new solver, simply adjust `IntegrationParameters::num_solver_iterations`.
+In order to adjust the number of iterations of the new solver, simply
+adjust `IntegrationParameters::num_solver_iterations`.
 If recovering the old solver behavior is useful to you, call `IntegrationParameters::switch_to_standard_pgs_solver()`.
 
 It is now possible to specify some additional solver iteration for specific rigid-bodies (and everything interacting
@@ -11,11 +99,15 @@ with it directly or indirectly through contacts and joints): `RigidBodyBuilder::
 without affecting performance of the other parts of the simulation.
 
 ### Fix
-- Fix bug causing angular joint limits and motor to sometimes only take into account half of the angles specified by the user.
+
+- Fix bug causing angular joint limits and motor to sometimes only take into account half of the angles specified by the
+  user.
 - Fix bug where collisions would not be re-computed after a collider was re-enabled.
 
 ### Added
-- Add a `SpringJoint` and `SpringJointBuilder` for simulating springs with customizable stiffness and damping coefficients.
+
+- Add a `SpringJoint` and `SpringJointBuilder` for simulating springs with customizable stiffness and damping
+  coefficients.
 - Add `SphericalJoint::local_frame1/2`, `::set_local_frame1/2`, and `SphericalJointBuilder::local_frame1/2` to set both
   the joint’s anchor and reference orientation.
 - Add `EffectiveCharacterMovement::is_sliding_down_slope` to indicate if the character controlled by the kinematic
@@ -27,6 +119,7 @@ without affecting performance of the other parts of the simulation.
 - Fix debug-renderer showing moved kinematic rigid-bodies only at their initial position.
 
 ### Modified
+
 - Make `Wheel::friction_slip` public to customize the front friction applied to the vehicle controller’s wheels.
 - Add the `DebugRenderBackend::filter_object` predicate that can be implemented to apply custom filtering rules
   on the objects being rendered.
@@ -35,19 +128,24 @@ without affecting performance of the other parts of the simulation.
 - Rename `NarrowPhase::intersections_with` to `NarrowPhase::intersection_pairs_with`.
 
 ## v0.17.2 (26 Feb. 2023)
+
 ### Fix
+
 - Fix issue with convex polyhedron jitter due to missing contacts.
 - Fix character controller getting stuck against vertical walls.
 - Fix character controller’s snapping to ground not triggering sometimes.
 - Fix character controller’s horizontal offset being mostly ignored and some instances of vertical offset being ignored.
 
 ## v0.17.1 (22 Jan. 2023)
+
 ### Fix
+
 - Fix bug resulting in dynamic rigid-bodies acting as kinematic bodies after being disabled and then re-enabled.
 
-
 ## v0.17.0 (15 Jan. 2023)
+
 ### Added
+
 - Add `RigidBody::set_enabled`, `RigidBody::is_enabled`, `RigidBodyBuilder::enabled` to enable/disable a rigid-body
   without having to delete it. Disabling a rigid-body attached to a multibody joint isn’t supported yet.
 - Add `Collider::set_enabled`, `Collider::is_enabled`, `ColliderBuilder::enabled` to enable/disable a collider
@@ -61,37 +159,48 @@ without affecting performance of the other parts of the simulation.
 - Add `RigidBody::locked_axes` to get the rigid-body axes that were locked by the user.
 
 ### Modified
+
 - Add the `QueryPipeline` as an optional argument to `PhysicsPipeline::step` and `CollisionPipeline::step`. If this
-  argument is specified, then the query pipeline will be incrementally (i.e. more efficiently) update at the same time as
+  argument is specified, then the query pipeline will be incrementally (i.e. more efficiently) update at the same time
+  as
   these other pipelines. In that case, calling `QueryPipeline::update` a `PhysicsPipeline::step` isn’t needed.
 - `RigidBody::set_body_type` now takes an extra boolean argument indicating if the rigid-body should be woken-up
   (if it becomes dynamic).
 - `RigidBody::mass_properties` now also returns the world-space mass-properties of the rigid-body.
 
 ### Fix
+
 - Fix bug resulting in rigid-bodies being awakened after they are created, even if they are created sleeping.
 
 ## v0.16.1 (10 Nov. 2022)
+
 ### Fix
+
 - Fixed docs build on `docs.rs`.
 
 ## v0.16.0 (30 Oct. 2022)
+
 ### Added
+
 - Implement `Copy` for `CharacterCollision`.
 - Implement conversion (`From` trait) between `Group` and `u32`.
 - Add `ColliderBuilder::trimesh_with_flags` to build a triangle mesh with specific flags controlling
   its initialization.
 
 ### Modified
+
 - Rename `AABB` to `Aabb` to comply with Rust’s style guide.
 - Switch to `parry 0.11`.
 
 ### Fix
+
 - Fix internal edges of 3D triangle meshes or 3D heightfields generating invalid contacts preventing
   balls from moving straight.
 
 ## v0.15.0 (02 Oct. 2022)
+
 ### Added
+
 - Add a **kinematic character** controller implementation. See the `control` module. The character controller currently
   supports the following features:
     - Slide on uneven terrains
@@ -104,8 +213,8 @@ without affecting performance of the other parts of the simulation.
     - Report information on the obstacles it hit on its path.
 - Implement `serde` serialization/deserialization for `CollisionEvents` when the `serde-serialize` feature is enabled
 
-
 ### Modified
+
 - The methods `Collider::set_rotation`, `RigidBody::set_rotation`, and `RigidBody::set_next_kinematic_rotation` now
   take a rotation (`UnitQuaternion` or `UnitComplex`) instead of a vector/angle.
 - The method `QueryFilter::exclude_dynamic` is now a static method (the `self` argument was removed).
@@ -117,7 +226,9 @@ without affecting performance of the other parts of the simulation.
   position.
 
 ## v0.14.0 (09 July 2022)
+
 ### Fixed
+
 - Fix unpredictable broad-phase panic when using small colliders in the simulation.
 - Fix collision events being incorrectly generated for any shape that produces multiple
   contact manifolds (like triangle meshes).
@@ -125,6 +236,7 @@ without affecting performance of the other parts of the simulation.
   to `CollisionPipeline::step`.
 
 ### Modified
+
 - The `RigidBodyBuilder::additional_mass` method will now result in the additional angular inertia
   being automatically computed based on the shapes of the colliders attached to the rigid-body.
 - Remove the deprecated methods `RigidBodyBuilder::mass`, `::principal_angular_inertia`, `::principal_inertia`.
@@ -137,6 +249,7 @@ without affecting performance of the other parts of the simulation.
   `RigidBodyBuilder::enabled_rotations` and `RigidBodyBuilder::enabled_translations`.
 
 ### Added
+
 - Add `RigidBody::recompute_mass_properties_from_colliders` to force the immediate computation
   of a rigid-body’s mass properties (instead of waiting for them to be recomputed during the next
   timestep). This is useful to be able to read immediately the result of a change of a rigid-body
@@ -149,7 +262,8 @@ without affecting performance of the other parts of the simulation.
 - Add `ColliderBuilder::mass` to set the mass of the collider instead of its density. Its angular
   inertia tensor will be automatically computed based on this mass and its shape.
 - Add `Collider::mass` and `Collider::volume` to retrieve the mass or volume of a collider.
-- Add the `QueryFilter` that is now used by all the scene queries instead of the `CollisionGroups` and `Fn(ColliderHandle) -> bool`
+- Add the `QueryFilter` that is now used by all the scene queries instead of the `CollisionGroups`
+  and `Fn(ColliderHandle) -> bool`
   closure. This `QueryFilter` provides easy access to most common filtering strategies (e.g. dynamic bodies only,
   excluding one particular collider, etc.) for scene queries.
 - Add force reporting based on contact force events. The `EventHandler` trait has been modified to include
@@ -161,12 +275,15 @@ without affecting performance of the other parts of the simulation.
   contact force events.
 
 ## v0.13.0 (31 May 2022)
+
 ### Fixed
+
 - Fix incorrect sensor events being generated after collider removal.
 - Fix bug where the CCD thickness wasn’t initialized properly.
 - Fix bug where the contact compliance would result in undesired tunneling, despite CCD being enabled.
 
 ### Modified
+
 - Add a `wake_up: bool` argument to the `ImpulseJointSet::insert` and `MultibodyJointSet::insert` to
   automatically wake-up the rigid-bodies attached to the inserted joint.
 - The methods `ImpulseJointSet::remove/remove_joints_attached_to_rigid_body`,
@@ -180,11 +297,14 @@ without affecting performance of the other parts of the simulation.
   by default.
 
 ### Added
+
 - Debug-renderer: add rendering of contacts, solver contacts, and collider Aabbs
 - Add `MultibodyJointSet::attached_joints` to return all the multibody joints attached to a given rigid-body.
 
 ## v0.12.0 (30 Apr. 2022)
+
 ### Fixed
+
 - Fix the simulation when the `parallel` feature is enabled.
 - Fix bug where damping would not be applied properly to some bodies.
 - Fix panics caused by various situations (contact or joints) involving rigid-bodies with locked translations/rotations.
@@ -194,6 +314,7 @@ without affecting performance of the other parts of the simulation.
 - Fix the broad-phase becoming potentially invalid after a change of collision groups.
 
 ### Modified
+
 - Switch to `nalgebra` 0.31.
 - Switch to `parry` 0.9.
 - Rename `JointHandle` to `ImpulseJointHandle`.
@@ -210,27 +331,30 @@ without affecting performance of the other parts of the simulation.
 - The `ActiveEvents::CONTACT_EVENTS` and `ActiveEvents::INTERSECTION_EVENTS` flags have been replaced by a single
   flag `ActiveEvents::COLLISION_EVENTS`.
 - Joint motors no longer have a `VelocityBased` model. The new choices are `AccelerationBased` and `ForceBased`
-  which are more stable.  
+  which are more stable.
 - Calling the `.build()` function from builders (`RigidBodyBuilder`, `ColliderBuilder`, etc.) is no longer necessary
-  whan adding them to sets. It is automatically called thanks to `Into<_>` implementations.  
+  when adding them to sets. It is automatically called thanks to `Into<_>` implementations.
 - The `ComponentSet` abstractions (and related `_generic` methods like `PhysicsPipeline::step_generic`) have been
   removed. Custom storage for colliders and rigid-bodies are no longer possible: use the built-in `RigidBodySet` and
   `ColliderSet` instead.
 
 ### Semantic modifications
+
 These are changes in the behavior of the physics engine that are not necessarily
 reflected by an API change. See [#304](https://github.com/dimforge/rapier/pull/304) for extensive details.
+
 - `RigidBody::set_linvel` and `RigidBody::set_angvel` no longer modify the velocity of static bodies.
 - `RigidBody::set_body_type` will reset the velocity of a rigid-body to zero if it is static.
 - Don’t automatically clear forces at the end of a timestep.
 - Don’t reset the velocity of kinematic bodies to zero at the end of the timestep.
-- Events `CollisionEvent::Stopped` are now generated after a collider is removed. 
+- Events `CollisionEvent::Stopped` are now generated after a collider is removed.
 
 ### Added
+
 - Significantly improve the API of joints by adding:
-  * Builders based on the builder pattern.
-  * Getters and setters for all joints.
-  * Method to convert a `GenericJoint` to one of the more specific joint type.
+    * Builders based on the builder pattern.
+    * Getters and setters for all joints.
+    * Method to convert a `GenericJoint` to one of the more specific joint type.
 - Improve stability of joint motors.
 - Adds a `bool` argument to `RigidBodySet::remove`. If set to `false`, the colliders attached to the rigid-body
   won’t be automatically deleted (they will only be detached from the deleted rigid-body instead).
@@ -240,12 +364,16 @@ reflected by an API change. See [#304](https://github.com/dimforge/rapier/pull/3
   renderer to debug the state of the physics engine.
 
 ## v0.12.0-alpha.0 (2 Jan. 2022)
+
 ### Fixed
+
 - Fixed `RigidBody::restrict_rotations` to properly take into account the axes to lock.
+
 ### Modified
+
 - All the impulse-based joints have been replaced by a single generic 6-Dofs joint in 3D
   (or 3-Dofs joint in 2D) named `ImpulseJoint`. The `RevoluteJoint, PrismaticJoint, FixedJoint`,
-  and `SphericalJoint` (formely named `BallJoint`) structures still exist but are just convenient
+  and `SphericalJoint` (formerly named `BallJoint`) structures still exist but are just convenient
   ways to initialize the generic `ImpulseJoint`.
 - Our constraints solver has been modified. Before we used one velocity-based resolution followed
   by one position-based resolution. We are now using two velocity-based resolution: the first one
@@ -253,63 +381,81 @@ reflected by an API change. See [#304](https://github.com/dimforge/rapier/pull/3
   code significantly while offering stiffer results.
 
 ### Added
-- Added multibody joints: joints based on the reduced-coordinates modeling. These joints can’t 
+
+- Added multibody joints: joints based on the reduced-coordinates modeling. These joints can’t
   violate their positional constraint.
 - Implement `Default` for most of the struct that supports it.
 
 ## v0.11.1
+
 ### Fixed
+
 - Fix a bug causing large moving colliders to miss some collisions after some time.
 - Fix invalid forces generated by contacts with position-based kinematic bodies.
 - Fix a bug where two colliders without parent would not have their collision computed even if the
   appropriate flags were set.
 
 ## v0.11.0
+
 Check out the user-guide for the JS/Typescript bindings for rapier. It has been fully rewritten and is now exhaustive!
 Check it out on [rapier.rs](https://www.rapier.rs/docs/user_guides/javascript/getting_started_js)
 
 ### Added
+
 - Joint limits are now implemented for all joints that can support them (prismatic, revolute, and ball joints).
 
 ### Modified
+
 - Switch to  `nalgebra 0.29`.
 
 ### Fixed
+
 - Fix the build of Rapier when targeting emscripten.
 
 ## v0.10.1
+
 ### Added
-- Add `Collider::set_translation_wrt_parent` to change the translation of a collider with respect to its parent rigid-body.
+
+- Add `Collider::set_translation_wrt_parent` to change the translation of a collider with respect to its parent
+  rigid-body.
 - Add `Collider::set_rotation_wrt_parent` to change the translation of a collider with respect to its parent rigid-body.
 
-
 ## v0.10.0
+
 ### Added
+
 - Implement `Clone` for `IslandManager`.
 
 ### Modified
+
 - `JointSet::insert` no longer takes the rigid-body set in its arguments.
 - Modify the testbed's plugin system to let plugins interact with the rendering.
 - Implement `PartialEq` for most collider and rigid-body components.
 
 ## v0.9.2
+
 ### Added
+
 - Make the method JointSet::remove_joints_attached_to_rigid_body public so that it can can be called externally for
   letting component-based Rapier integration call it to cleanup joints after a rigid-body removal.
 
 ### Fixed
+
 - Fix a panic that could happen when the same collider is listed twice in the removed_colliders array.
 
-
 ## v0.9.1
+
 ### Added
+
 - Add `rapier::prelude::nalgebra` so that the `vector!` and `point!` macros work out-of-the-box after importing
   the prelude: `use rapier::prelude::*`
 
 ## v0.9.0
+
 The user-guide has been fully rewritten and is now exhaustive! Check it out on [rapier.rs](https://rapier.rs/)
 
 ### Added
+
 - A prelude has been added in order to simplify the most common imports. For example: `use rapier3d::prelude::*`
 - Add `RigidBody::set_translation` and `RigidBody.translation()`.
 - Add `RigidBody::set_rotation` and `RigidBody.rotation()`.
@@ -322,6 +468,7 @@ The user-guide has been fully rewritten and is now exhaustive! Check it out on [
   some SIMD code do generate NaNs which are filtered out by lane-wise selection).
 
 ### Modified
+
 The use of `RigidBodySet, ColliderSet, RigidBody, Collider` is no longer mandatory. Rigid-bodies and colliders have
 been split into multiple components that can be stored in a user-defined set. This is useful for integrating Rapier
 with other engines (for example this allows us to use Bevy's Query as our rigid-body/collider sets).
@@ -330,6 +477,7 @@ The `RigidBodySet, ColliderSet, RigidBody, Collider` are still the best option f
 provide their own component sets.
 
 #### Rigid-bodies
+
 - Renamed `BodyStatus` to `RigidBodyType`.
 - `RigidBodyBuilder::translation` now takes a vector instead of individual components.
 - `RigidBodyBuilder::linvel` now takes a vector instead of individual components.
@@ -337,8 +485,9 @@ provide their own component sets.
   `RigidBodyBuilder::new_kinematic_velocity_based` constructors.
 - The `RigidBodyType::Kinematic` variant has been replaced by two variants: `RigidBodyType::KinematicVelocityBased` and
   `RigidBodyType::KinematicPositionBased`.
-  
+
 #### Colliders
+
 - `Colliderbuilder::translation` now takes a vector instead of individual components.
 - The way `PhysicsHooks` are enabled changed. Now, a physics hooks is executed if any of the two
   colliders involved in the contact/intersection pair contains the related `PhysicsHooksFlag`.
@@ -360,115 +509,139 @@ provide their own component sets.
 - Fixed a bug where collision groups were ignored by CCD.
 
 #### Joints
+
 - The fields `FixedJoint::local_anchor1` and `FixedJoint::local_anchor2` have been renamed to
   `FixedJoint::local_frame1` and `FixedJoint::local_frame2`.
-  
+
 #### Pipelines and others
+
 - The field `ContactPair::pair` (which contained two collider handles) has been replaced by two
   fields: `ContactPair::collider1` and `ContactPair::collider2`.
 - The list of active dynamic bodies is now retrieved with `IslandManager::active_dynamic_bodies`
   instead of `RigidBodySet::iter_active_dynamic`.
 - The list of active kinematic bodies is now retrieved with `IslandManager::active_kinematic_bodies`
   instead of `RigidBodySet::iter_active_kinematic`.
-- `NarrowPhase::contacts_with` now returns an `impl Iterator<Item = &ContactPair>` instead of 
+- `NarrowPhase::contacts_with` now returns an `impl Iterator<Item = &ContactPair>` instead of
   an `Option<impl Iterator<Item = (ColliderHandle, ColliderHandle, &ContactPair)>>`. The colliders
   handles can be read from the contact-pair itself.
 - `NarrowPhase::intersections_with` now returns an iterator directly instead of an `Option<impl Iterator>`.
 - Rename `PhysicsHooksFlags` to `ActiveHooks`.
 - Add the contact pair as an argument to `EventHandler::handle_contact_event`
 
-
 ## v0.8.0
+
 ### Modified
+
 - Switch to nalgebra 0.26.
 
 ## v0.7.2
+
 ### Added
+
 - Implement `Serialize` and `Deserialize` for the `CCDSolver`.
 
 ### Fixed
+
 - Fix a crash that could happen after adding and then removing a collider right away,
-before stepping the simulation.
+  before stepping the simulation.
 
 ## v0.7.1
+
 ### Fixed
+
 - Fixed a bug in the broad-phase that could cause non-determinism after snapshot restoration.
 
 ## v0.7.0
+
 ### Added
+
 - Add the support of **Continuous Collision Detection** (CCD) to
-make sure that some fast-moving objects (chosen by the user) don't miss any contacts.
-This is done by using motion-clamping, i.e., each fast-moving rigid-body with CCD enabled will
-be stopped at the time where their first contact happen. This will result in some "time loss" for that
-rigid-body. This loss of time can be reduced by increasing the maximum number  of CCD substeps executed
-(the default being 1).
+  make sure that some fast-moving objects (chosen by the user) don't miss any contacts.
+  This is done by using motion-clamping, i.e., each fast-moving rigid-body with CCD enabled will
+  be stopped at the time where their first contact happen. This will result in some "time loss" for that
+  rigid-body. This loss of time can be reduced by increasing the maximum number of CCD substeps executed
+  (the default being 1).
 - Add the support of **collider modification**. Now, most of the characteristics of a collider can be
-modified after the collider has been created.
+  modified after the collider has been created.
 - We now use an **implicit friction cone** for handling friction, instead of a pyramidal approximation
-of the friction cone. This means that friction will now  behave in a more isotropic way (i.e. more realistic
-Coulomb friction).
+  of the friction cone. This means that friction will now behave in a more isotropic way (i.e. more realistic
+  Coulomb friction).
 - Add the support of **custom filters** for the `QueryPipeline`. So far, interaction groups (bit masks)
-had to be used to exclude from colliders from a query made with the `QueryPipeline`. Now it is also
-possible to provide a custom closures to apply arbitrary user-defined filters.
+  had to be used to exclude from colliders from a query made with the `QueryPipeline`. Now it is also
+  possible to provide a custom closures to apply arbitrary user-defined filters.
 - It is now possible to solve penetrations using the velocity solver instead of (or alongside) the
-position solver (this is disabled by default, set `IntegrationParameters::velocity_based_erp` to
+  position solver (this is disabled by default, set `IntegrationParameters::velocity_based_erp` to
   a value `> 0.0` to enable.).
 
 Added the methods:
+
 - `ColliderBuilder::halfspace` to create a collider with an unbounded plane shape.
 - `Collider::shape_mut` to get a mutable reference to its shape.
 - `Collider::set_shape`, `::set_restitution_combine_rule`, `::set_position_wrt_parent`, `::set_collision_groups`
-`::set_solver_groups` to change various properties of a collider after its creation.
+  `::set_solver_groups` to change various properties of a collider after its creation.
 - `RigidBodyBuilder::ccd_enabled` to enable CCD for a rigid-body.
 
 ### Modified
+
 - The `target_dist` argument of `QueryPipeline::cast_shape` was removed.
 - `RigidBodyBuilder::mass_properties` has been deprecated, replaced by `::additional_mass_properties`.
 - `RigidBodyBuilder::mass` has been deprecated, replaced by `::additional_mass`.
-- `RigidBodyBuilder::principal_angular_inertia` has been deprecated, replaced by `::additional_principal_angular_inertia`.
-- The field `SolveContact::data` has been replaced by the fields `SolverContact::warmstart_impulse`, 
+- `RigidBodyBuilder::principal_angular_inertia` has been deprecated, replaced
+  by `::additional_principal_angular_inertia`.
+- The field `SolveContact::data` has been replaced by the fields `SolverContact::warmstart_impulse`,
   `SolverContact::warmstart_tangent_impulse`, and `SolverContact::prev_rhs`.
 - All the fields of `IntegrationParameters` that we don't use have been removed.
 - `NarrowPhase::maintain` has been renamed to `NarrowPhase::handle_user_changes`.
 - `BroadPhase::maintain` has been removed. Use ` BroadPhase::update` directly.
 
 ### Fixed
+
 - The Broad-Phase algorithm has been completely reworked to support large colliders properly (until now
-they could result in very large memory and CPU usage).
+  they could result in very large memory and CPU usage).
 
 ## v0.6.1
+
 ### Fixed
+
 - Fix a determinism problem that may happen after snapshot restoration, if a rigid-body is sleeping at
   the time the snapshot is taken.
 
 ## v0.6.0
+
 ### Added
+
 - The support of **dominance groups** have been added. Each rigid-body is part of a dominance group in [-127; 127]
-(the default is 0). If two rigid-body are in contact, the one with the highest dominance will act as if it has
-an infinite mass, making it immune to the forces the other body would apply on it. See [#122](https://github.com/dimforge/rapier/pull/122)
-for further details.
+  (the default is 0). If two rigid-body are in contact, the one with the highest dominance will act as if it has
+  an infinite mass, making it immune to the forces the other body would apply on it.
+  See [#122](https://github.com/dimforge/rapier/pull/122)
+  for further details.
 - The support for **contact modification** has been added. This can bee used to simulate conveyor belts,
-one-way platforms and other non-physical effects. It can also be used to simulate materials with
-variable friction and restitution coefficient on a single collider. See [#120](https://github.com/dimforge/rapier/pull/120)
-for further details.
+  one-way platforms and other non-physical effects. It can also be used to simulate materials with
+  variable friction and restitution coefficient on a single collider.
+  See [#120](https://github.com/dimforge/rapier/pull/120)
+  for further details.
 - The support for **joint motors** have been added. This can be used to control the position and/or
-velocity of a joint based on a spring-like equation. See [#119](https://github.com/dimforge/rapier/pull/119)
-for further details.
+  velocity of a joint based on a spring-like equation. See [#119](https://github.com/dimforge/rapier/pull/119)
+  for further details.
 
 ### Removed
+
 - The `ContactPairFilter` and `IntersectionPairFilter` traits have been removed. They are both
   combined in a single new trait: `PhysicsHooks`.
 
 ## v0.5.0
+
 In this release we are dropping `ncollide` and use our new crate [`parry`](https://parry.rs)
 instead! This comes with a lot of new features, as well as two new crates: `rapier2d-f64` and
 `rapier3d-f64` for physics simulation with 64-bits floats.
 
 ### Added
+
 - Added a `RAPIER.version()` function at the root of the package to retrieve the version of Rapier
   as a string.
 
 Several geometric queries have been added to the `QueryPipeline`:
+
 - `QueryPipeline::intersections_with_ray`: get all colliders intersecting a ray.
 - `QueryPipeline::intersection_with_shape`: get one collider intersecting a shape.
 - `QueryPipeline::project_point`: get the projection of a point on the closest collider.
@@ -478,66 +651,83 @@ Several geometric queries have been added to the `QueryPipeline`:
 - `QueryPipeline::intersections_with_shape`: get all the colliders intersecting a shape.
 
 Several new shape types are now supported:
+
 - `RoundCuboid`, `Segment`, `Triangle`, `RoundTriangle`, `Polyline`, `ConvexPolygon` (2D only),
   `RoundConvexPolygon` (2D only), `ConvexPolyhedron` (3D only), `RoundConvexPolyhedron` (3D only),
   `RoundCone` (3D only).
 
 It is possible to build `ColliderDesc` using these new shapes:
+
 - `ColliderBuilder::round_cuboid`, `ColliderBuilder::segment`, `ColliderBuilder::triangle`, `ColliderBuilder::round_triangle`,
   `ColliderBuilder::convex_hull`, `ColliderBuilder::round_convex_hull`, `ColliderBuilder::polyline`,
   `ColliderBuilder::convex_decomposition`, `ColliderBuilder::round_convex_decomposition`,
   `ColliderBuilder::convex_polyline` (2D only), `ColliderBuilder::round_convex_polyline` (2D only),
-  `ColliderBuilder::convex_mesh` (3D only),`ColliderBuilder::round_convex_mesh` (3D only), `ColliderBuilder::round_cone` (3D only).
+  `ColliderBuilder::convex_mesh` (3D only),`ColliderBuilder::round_convex_mesh` (3D
+  only), `ColliderBuilder::round_cone` (3D only).
 
 It is possible to specify different rules for combining friction and restitution coefficients
 of the two colliders involved in a contact with:
+
 - `ColliderDesc::friction_combine_rule`, and `ColliderDesc::restitution_combine_rule`.
 
 Various RigidBody-related getter and setters have been added:
+
 - `RigidBodyBuilder::gravity_scale`, `RigidBody::gravity_scale`, `RigidBody::set_gravity_scale` to get/set the scale
   factor applied to the gravity affecting a rigid-body. Setting this to 0.0 will make the rigid-body ignore gravity.
 - `RigidBody::set_linear_damping` and `RigidBody::set_angular_damping` to set the linear and angular damping of
   the rigid-body.
 - `RigidBodyBuilder::restrict_rotations` to prevent rotations along specific coordinate axes. This replaces the three
   boolean arguments previously passed to `.set_principal_angular_inertia`.
-  
+
 ### Breaking changes
+
 Breaking changes related to contacts:
-- The way contacts are represented changed. Refer to the documentation of `parry::query::ContactManifold`, `parry::query::TrackedContact`
+
+- The way contacts are represented changed. Refer to the documentation
+  of `parry::query::ContactManifold`, `parry::query::TrackedContact`
   and `rapier::geometry::ContactManifoldData` and `rapier::geometry::ContactData` for details.
 
 Breaking changes related to rigid-bodies:
-- The `RigidBodyDesc.setMass` takes only one argument now. Use `RigidBodyDesc.lockTranslations` to lock the translational
+
+- The `RigidBodyDesc.setMass` takes only one argument now. Use `RigidBodyDesc.lockTranslations` to lock the
+  translational
   motion of the rigid-body.
 - The `RigidBodyDesc.setPrincipalAngularInertia` no longer have boolean parameters to lock rotations.
-  Use `RigidBodyDesc.lockRotations` or `RigidBodyDesc.restrictRotations` to lock the rotational motion of the rigid-body.
+  Use `RigidBodyDesc.lockRotations` or `RigidBodyDesc.restrictRotations` to lock the rotational motion of the
+  rigid-body.
 
 Breaking changes related to colliders:
+
 - The collider shape type has been renamed from `ColliderShape` to `SharedShape` (now part of the Parry crate).
 - The `Polygon` shape no longer exists. For a 2D convex polygon, use a `ConvexPolygon` instead.
 - All occurrences of `Trimesh` have been replaced by `TriMesh` (note the change in case).
 
 Breaking changes related to events:
+
 - Rename all occurrences of `Proximity` to `Intersection`.
 - The `Proximity` enum has been removed, it's replaced by a boolean.
 
 ## v0.4.2
+
 - Fix a bug in angular inertia tensor computation that could cause rotations not to
   work properly.
 - Add `RigidBody::set_mass_properties` to set the mass properties of an already-constructed
   rigid-body.
 
 ## v0.4.1
+
 - The `RigidBodyBuilder::principal_inertia` method has been deprecated and renamed to
   `principal_angular_inertia` for clarity.
 
 ## v0.4.0
+
 - The rigid-body `linvel`, `angvel`, and `position` fields are no longer public. Access using
   their corresponding getters/setters. For example: `rb.linvel()`, `rb.set_linvel(vel, true)`.
 - Add `RigidBodyBuilder::sleeping(true)` to allow the creation of a rigid-body that is asleep
   at initialization-time.
 
 #### Locking translation and rotations of a rigid-body
+
 - Add `RigidBodyBuilder::lock_rotations` to prevent a rigid-body from rotating because of forces.
 - Add `RigidBodyBuilder::lock_translations` to prevent a rigid-body from translating because of forces.
 - Add `RigidBodyBuilder::principal_inertia` for setting the principal inertia of a rigid-body, and/or
@@ -546,6 +736,7 @@ Breaking changes related to events:
   contributions should be taken into account in the future too.
 
 #### Reading contact and proximity information
+
 - Add `NarrowPhase::contacts_with` and `NarrowPhase::proximities_with` to retrieve all the contact
   pairs and proximity pairs involving a specific collider.
 - Add `NarrowPhase::contact_pair` and `NarrowPhase::proximity_pair` to retrieve one specific contact
@@ -554,6 +745,7 @@ Breaking changes related to events:
   proximity pairs detected by the narrow-phase.
 
 ## v0.3.2
+
 - Add linear and angular damping. The damping factor can be set with `RigidBodyBuilder::linear_damping` and
   `RigidBodyBuilder::angular_damping`.
 - Implement `Clone` for almost everything that can be worth cloning.
@@ -562,34 +754,43 @@ Breaking changes related to events:
 - The restitution coefficient of colliders is now taken into account by the physics solver.
 
 ## v0.3.1
+
 - Fix non-determinism problem when using triangle-meshes, cone, cylinders, or capsules.
 - Add `JointSet::remove(...)` to remove a joint from the `JointSet`.
 
 ## v0.3.0
+
 - Collider shapes are now trait-objects instead of a `Shape` enum.
 - Add a user-defined `u128` to each colliders and rigid-bodies for storing user data.
 - Add the support for `Cylinder`, `RoundCylinder`, and `Cone` shapes.
 - Added the support for collision filtering based on bit masks (often known as collision groups, collision masks, or
-  collision layers in other physics engines). Each collider has two groups. Their `collision_groups` is used for filtering
-  what pair of colliders should have their contacts computed by the narrow-phase. Their `solver_groups` is used for filtering
+  collision layers in other physics engines). Each collider has two groups. Their `collision_groups` is used for
+  filtering
+  what pair of colliders should have their contacts computed by the narrow-phase. Their `solver_groups` is used for
+  filtering
   what pair of colliders should have their contact forces computed by the constraints solver.
-- Collision groups can also be used to filter what collider should be hit by a ray-cast performed by the `QueryPipeline`.
+- Collision groups can also be used to filter what collider should be hit by a ray-cast performed by
+  the `QueryPipeline`.
 - Added collision filters based on user-defined trait-objects. This adds two traits `ContactPairFilter` and
   `ProximityPairFilter` that allows user-defined logic for determining if two colliders/sensors are allowed to interact.
-- The `PhysicsPipeline::step` method now takes two additional arguments: the optional `&ContactPairFilter` and `&ProximityPairFilter`
-for filtering contact and proximity pairs.
+- The `PhysicsPipeline::step` method now takes two additional arguments: the optional `&ContactPairFilter`
+  and `&ProximityPairFilter`
+  for filtering contact and proximity pairs.
 
 ## v0.2.1
+
 - Fix panic in TriMesh construction and QueryPipeline update caused by a stack overflow or a subtraction underflow.
 
 ## v0.2.0
+
 The most significant change on this version is the addition of the `QueryPipeline` responsible for performing
 scene-wide queries. So far only ray-casting has been implemented.
 
 - Add `ColliderSet::remove(...)` to remove a collider from the `ColliderSet`.
 - Replace `PhysicsPipeline::remove_rigid_body` by `RigidBodySet::remove`.
 - The `JointSet.iter()` now returns an iterator yielding `(JointHandle, &Joint)` instead of just `&Joint`.
-- Add `ColliderDesc::translation(...)` to set the translation of a collider relative to the rigid-body it is attached to.
+- Add `ColliderDesc::translation(...)` to set the translation of a collider relative to the rigid-body it is attached
+  to.
 - Add `ColliderDesc::rotation(...)` to set the rotation of a collider relative to the rigid-body it is attached to.
 - Add `ColliderDesc::position(...)` to set the position of a collider relative to the rigid-body it is attached to.
 - Add `Collider::position_wrt_parent()` to get the position of a collider relative to the rigid-body it is attached to.
@@ -597,8 +798,8 @@ scene-wide queries. So far only ray-casting has been implemented.
 - Deprecate `Collider::delta()` in favor of the new `Collider::position_wrt_parent()`.
 - Fix multiple issues occurring when having colliders resulting in a non-zero center-of-mass.
 - Fix a crash happening when removing a rigid-body with a collider, stepping the simulation, adding another rigid-body
- with a collider, and stepping the simulation again.
+  with a collider, and stepping the simulation again.
 - Fix NaN when detection contacts between two polygonal faces where one has a normal perfectly perpendicular to the
-separating vector.
+  separating vector.
 - Fix bug collision detection between trimeshes and other shapes. The bug appeared depending on whether the trimesh
-collider was added before the other shape's collider or after.
+  collider was added before the other shape's collider or after.
diff --git a/Cargo.toml b/Cargo.toml
index 023432e..b9d767c 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -1,6 +1,6 @@
 [workspace]
-members = [ "crates/rapier2d", "crates/rapier2d-f64", "crates/rapier_testbed2d", "crates/rapier_testbed2d-f64", "examples2d", "benchmarks2d",
-            "crates/rapier3d", "crates/rapier3d-f64", "crates/rapier_testbed3d", "crates/rapier_testbed3d-f64", "examples3d", "examples3d-f64", "benchmarks3d" ]
+members = ["crates/rapier2d", "crates/rapier2d-f64", "crates/rapier_testbed2d", "crates/rapier_testbed2d-f64", "examples2d", "benchmarks2d",
+    "crates/rapier3d", "crates/rapier3d-f64", "crates/rapier_testbed3d", "crates/rapier_testbed3d-f64", "examples3d", "examples3d-f64", "benchmarks3d"]
 resolver = "2"
 
 [patch.crates-io]
diff --git a/README.md b/README.md
index 8e576fc..1174a1c 100644
--- a/README.md
+++ b/README.md
@@ -35,24 +35,28 @@
 
 ## What is Rapier?
 
-Rapier is a set of 2D and 3D physics engines for games, animation, and robotics. These crates 
+Rapier is a set of 2D and 3D physics engines for games, animation, and robotics. These crates
 are `rapier2d`, `rapier3d`, `rapier2d-f64`, and `rapier3d-f64`. They are written with the Rust
 programming language, by the [Dimforge](https://dimforge.com) organization. It is forever free
 and open-source!
 
 ## Roadmap
+
 We update our roadmap at the beginning of each year. Our 2021 roadmap can be seen
 [there](https://www.dimforge.com/blog/2021/01/01/physics-simulation-with-rapier-2021-roadmap/#rapier-roadmap-for-2021).
 We regularly give updates about our progress on [our blog](https://www.dimforge.com/blog).
 
 ## Getting started
+
 The easiest way to get started with Rapier is to:
+
 1. Read the [user-guides](https://www.rapier.rs/docs/).
 2. Play with the examples: `cargo run --release --bin all_examples2` and `cargo run --release --bin all_examples3`.
    Their source code are available on the `examples2d/` and `examples3d/` directory.
 3. Don't hesitate to ask for help on [Discord](https://discord.gg/vt9DJSW), or by opening an issue on GitHub.
-  
+
 ## Resources and discussions
+
 - [Dimforge](https://dimforge.com): See all the open-source projects we are working on! Follow our announcements
   on our [blog](https://www.dimforge.com/blog).
 - [User guide](https://www.rapier.rs/docs/): Learn to use Rapier in your project by reading the official User Guides.
@@ -63,23 +67,3 @@ The easiest way to get started with Rapier is to:
 Please make sure to familiarize yourself with our [Code of Conduct](CODE_OF_CONDUCT.md)
 and our [Contribution Guidelines](CONTRIBUTING.md) before contributing or participating in
 discussions with the community.
-
-
-## Acknowledgements
-Rapier is supported by our **platinum** sponsors:
-<p>
-  <a href="https://embark-studios.com">
-    <img src="https://www.embark.dev/img/logo_black.png" width="301px">
-  </a>
-</p>
-
-And our gold sponsors:
-
-<p>
-  <a href="https://fragcolor.com">
-    <img src="https://dimforge.com/img/fragcolor_logo2_color_black.svg" width="300px">
-  </a>
-  <a href="https://resolutiongames.com/">
-    <img src="https://dimforge.com/img/logo_resolution_games.png" width="300px" />
-  </a>
-</p>
\ No newline at end of file
diff --git a/benchmarks2d/all_benchmarks2.rs b/benchmarks2d/all_benchmarks2.rs
index fcc7b95..3f98fb4 100644
--- a/benchmarks2d/all_benchmarks2.rs
+++ b/benchmarks2d/all_benchmarks2.rs
@@ -17,6 +17,7 @@ mod joint_ball2;
 mod joint_fixed2;
 mod joint_prismatic2;
 mod pyramid2;
+mod vertical_stacks2;
 
 fn demo_name_from_command_line() -> Option<String> {
     let mut args = std::env::args();
@@ -57,6 +58,7 @@ pub fn main() {
         ("Convex polygons", convex_polygons2::init_world),
         ("Heightfield", heightfield2::init_world),
         ("Pyramid", pyramid2::init_world),
+        ("Verticals stacks", vertical_stacks2::init_world),
         ("(Stress test) joint ball", joint_ball2::init_world),
         ("(Stress test) joint fixed", joint_fixed2::init_world),
         (
@@ -66,7 +68,7 @@ pub fn main() {
     ];
 
     // 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(')')) {
+    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,
diff --git a/benchmarks2d/vertical_stacks2.rs b/benchmarks2d/vertical_stacks2.rs
new file mode 100644
index 0000000..15552ba
--- /dev/null
+++ b/benchmarks2d/vertical_stacks2.rs
@@ -0,0 +1,61 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    let num = 80;
+    let rad = 0.5;
+
+    /*
+     * Ground
+     */
+    let ground_size = num as f32 * rad * 10.0;
+    let ground_thickness = 1.0;
+
+    let rigid_body = RigidBodyBuilder::fixed();
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_thickness);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+
+    let shiftx_centerx = [
+        (rad * 2.0 + 0.0002, -(num as f32) * rad * 2.0 * 1.5),
+        (rad * 2.0 + rad, num as f32 * rad * 2.0 * 1.5),
+    ];
+
+    for (shiftx, centerx) in shiftx_centerx {
+        let shifty = rad * 2.0;
+        let centery = shifty / 2.0 + ground_thickness;
+
+        for i in 0..num {
+            for j in 0usize..1 + i * 2 {
+                let fj = j as f32;
+                let fi = i as f32;
+                let x = (fj - fi) * shiftx + centerx;
+                let y = (num as f32 - fi - 1.0) * shifty + centery;
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y]);
+                let handle = bodies.insert(rigid_body);
+                let collider = ColliderBuilder::cuboid(rad, rad);
+                colliders.insert_with_parent(collider, handle, &mut bodies);
+            }
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 5.0);
+}
diff --git a/benchmarks3d/all_benchmarks3.rs b/benchmarks3d/all_benchmarks3.rs
index 591886e..c1e0da2 100644
--- a/benchmarks3d/all_benchmarks3.rs
+++ b/benchmarks3d/all_benchmarks3.rs
@@ -20,6 +20,8 @@ mod joint_fixed3;
 mod joint_prismatic3;
 mod joint_revolute3;
 mod keva3;
+mod many_pyramids3;
+mod many_sleep3;
 mod many_static3;
 mod pyramid3;
 mod stacks3;
@@ -56,6 +58,7 @@ pub fn main() {
         ("Compound", compound3::init_world),
         ("Convex polyhedron", convex_polyhedron3::init_world),
         ("Many static", many_static3::init_world),
+        ("Many sleep", many_sleep3::init_world),
         ("Heightfield", heightfield3::init_world),
         ("Stacks", stacks3::init_world),
         ("Pyramid", pyramid3::init_world),
@@ -64,6 +67,7 @@ pub fn main() {
         ("ImpulseJoint fixed", joint_fixed3::init_world),
         ("ImpulseJoint revolute", joint_revolute3::init_world),
         ("ImpulseJoint prismatic", joint_prismatic3::init_world),
+        ("Many pyramids", many_pyramids3::init_world),
         ("Keva tower", keva3::init_world),
     ];
 
diff --git a/benchmarks3d/ccd3.rs b/benchmarks3d/ccd3.rs
index f642739..4f7b2c1 100644
--- a/benchmarks3d/ccd3.rs
+++ b/benchmarks3d/ccd3.rs
@@ -56,8 +56,8 @@ pub fn init_world(testbed: &mut Testbed) {
                     1 => ColliderBuilder::ball(rad),
                     // Rounded cylinders are much more efficient that cylinder, even if the
                     // rounding margin is small.
-                    // 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0),
-                    // 3 => ColliderBuilder::cone(rad, rad),
+                    2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0),
+                    3 => ColliderBuilder::cone(rad, rad),
                     _ => ColliderBuilder::capsule_y(rad, rad),
                 };
 
diff --git a/benchmarks3d/many_pyramids3.rs b/benchmarks3d/many_pyramids3.rs
new file mode 100644
index 0000000..1562aec
--- /dev/null
+++ b/benchmarks3d/many_pyramids3.rs
@@ -0,0 +1,80 @@
+use rapier3d::prelude::*;
+use rapier_testbed3d::Testbed;
+
+fn create_pyramid(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    offset: Vector<f32>,
+    stack_height: usize,
+    rad: f32,
+) {
+    let shift = rad * 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 = (fi * shift / 2.0) + (fj - fi) * shift;
+            let y = fi * shift;
+
+            // Build the rigid body.
+            let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y, 0.0] + offset);
+            let handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::cuboid(rad, rad, rad);
+            colliders.insert_with_parent(collider, handle, bodies);
+        }
+    }
+}
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    let rad = 0.5;
+    let pyramid_count = 40;
+    let spacing = 4.0;
+
+    /*
+     * Ground
+     */
+    let ground_size = 50.0;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]);
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(
+        ground_size,
+        ground_height,
+        pyramid_count as f32 * spacing / 2.0 + ground_size,
+    );
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    for pyramid_index in 0..pyramid_count {
+        let bottomy = rad;
+        create_pyramid(
+            &mut bodies,
+            &mut colliders,
+            vector![
+                0.0,
+                bottomy,
+                (pyramid_index as f32 - pyramid_count as f32 / 2.0) * spacing
+            ],
+            20,
+            rad,
+        );
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
+}
diff --git a/benchmarks3d/many_sleep3.rs b/benchmarks3d/many_sleep3.rs
new file mode 100644
index 0000000..70f85cc
--- /dev/null
+++ b/benchmarks3d/many_sleep3.rs
@@ -0,0 +1,54 @@
+use rapier3d::prelude::*;
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Create the balls
+     */
+    let num = 50;
+    let rad = 1.0;
+
+    let shift = rad * 2.0 + 1.0;
+    let centerx = shift * (num as f32) / 2.0;
+    let centery = shift / 2.0;
+    let centerz = shift * (num as f32) / 2.0;
+
+    for i in 0..num {
+        for j in 0usize..num {
+            for k in 0..num {
+                let x = i as f32 * shift - centerx;
+                let y = j as f32 * shift + centery;
+                let z = k as f32 * shift - centerz;
+
+                let status = if j == 0 {
+                    RigidBodyType::Fixed
+                } else {
+                    RigidBodyType::Dynamic
+                };
+                let density = 0.477;
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::new(status)
+                    .translation(vector![x, y, z])
+                    .sleeping(true); // j < num - 1);
+                let handle = bodies.insert(rigid_body);
+                let collider = ColliderBuilder::ball(rad).density(density);
+                colliders.insert_with_parent(collider, handle, &mut bodies);
+            }
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
+}
diff --git a/crates/rapier2d-f64/Cargo.toml b/crates/rapier2d-f64/Cargo.toml
index 90ee6b5..f80aa03 100644
--- a/crates/rapier2d-f64/Cargo.toml
+++ b/crates/rapier2d-f64/Cargo.toml
@@ -1,14 +1,14 @@
 [package]
-name    = "rapier2d-f64"
-version = "0.18.0"
-authors = [ "Sébastien Crozet <developer@crozet.re>" ]
+name = "rapier2d-f64"
+version = "0.19.0"
+authors = ["Sébastien Crozet <developer@crozet.re>"]
 description = "2-dimensional physics engine in Rust."
 documentation = "https://docs.rs/rapier2d"
 homepage = "https://rapier.rs"
 repository = "https://github.com/dimforge/rapier"
 readme = "README.md"
-categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
-keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
+categories = ["science", "game-development", "mathematics", "simulation", "wasm"]
+keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
 license = "Apache-2.0"
 edition = "2021"
 
@@ -16,23 +16,23 @@ edition = "2021"
 maintenance = { status = "actively-developed" }
 
 [features]
-default = [ "dim2", "f64" ]
-dim2    = [ ]
-f64     = [ ]
-parallel = [ "rayon" ]
-simd-stable = [ "simba/wide", "simd-is-enabled" ]
-simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ]
+default = ["dim2", "f64"]
+dim2 = []
+f64 = []
+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 = [ "vec_map" ]
-wasm-bindgen = [ "instant/wasm-bindgen" ]
-serde-serialize = [ "nalgebra/serde-serialize", "parry2d-f64/serde-serialize", "serde", "bit-vec/serde", "arrayvec/serde" ]
-enhanced-determinism = [ "simba/libm_force", "parry2d-f64/enhanced-determinism" ]
-debug-render = [ ]
-profiler = [ "instant" ] # Enables the internal profiler.
+simd-is-enabled = ["vec_map"]
+wasm-bindgen = ["instant/wasm-bindgen"]
+serde-serialize = ["nalgebra/serde-serialize", "parry2d-f64/serde-serialize", "serde", "bit-vec/serde", "arrayvec/serde"]
+enhanced-determinism = ["simba/libm_force", "parry2d-f64/enhanced-determinism"]
+debug-render = []
+profiler = ["instant"] # Enables the internal profiler.
 
 # Feature used for debugging only.
-debug-disable-legitimate-fe-exceptions = [ ]
+debug-disable-legitimate-fe-exceptions = []
 
 # Feature used for development and debugging only.
 # Do not enable this unless you are working on the engine internals.
@@ -44,15 +44,15 @@ features = ["parallel", "simd-stable", "serde-serialize", "debug-render"]
 [lib]
 name = "rapier2d_f64"
 path = "../../src/lib.rs"
-required-features = [ "dim2", "f64" ]
+required-features = ["dim2", "f64"]
 
 
 [dependencies]
 vec_map = { version = "0.8", optional = true }
-instant = { version = "0.1", features = [ "now" ], optional = true }
+instant = { version = "0.1", features = ["now"], optional = true }
 num-traits = "0.2"
 nalgebra = "0.32"
-parry2d-f64 = "0.13.1"
+parry2d-f64 = "0.15.0"
 simba = "0.8"
 approx = "0.5"
 rayon = { version = "1", optional = true }
@@ -60,11 +60,13 @@ crossbeam = "0.8"
 arrayvec = "0.7"
 bit-vec = "0.6"
 rustc-hash = "1"
-serde = { version = "1", features = [ "derive" ], optional = true }
+serde = { version = "1", features = ["derive"], optional = true }
 downcast-rs = "1.2"
 num-derive = "0.4"
 bitflags = "1"
+log = "0.4"
+ordered-float = "4"
 
 [dev-dependencies]
 bincode = "1"
-serde = { version = "1", features = [ "derive" ] }
+serde = { version = "1", features = ["derive"] }
diff --git a/crates/rapier2d/Cargo.toml b/crates/rapier2d/Cargo.toml
index 0d8bc24..0b4882f 100644
--- a/crates/rapier2d/Cargo.toml
+++ b/crates/rapier2d/Cargo.toml
@@ -1,14 +1,14 @@
 [package]
-name    = "rapier2d"
-version = "0.18.0"
-authors = [ "Sébastien Crozet <developer@crozet.re>" ]
+name = "rapier2d"
+version = "0.19.0"
+authors = ["Sébastien Crozet <developer@crozet.re>"]
 description = "2-dimensional physics engine in Rust."
 documentation = "https://docs.rs/rapier2d"
 homepage = "https://rapier.rs"
 repository = "https://github.com/dimforge/rapier"
 readme = "README.md"
-categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
-keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
+categories = ["science", "game-development", "mathematics", "simulation", "wasm"]
+keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
 license = "Apache-2.0"
 edition = "2021"
 
@@ -16,23 +16,23 @@ edition = "2021"
 maintenance = { status = "actively-developed" }
 
 [features]
-default = [ "dim2", "f32" ]
-dim2    = [ ]
-f32     = [ ]
-parallel = [ "rayon" ]
-simd-stable = [ "simba/wide", "simd-is-enabled" ]
-simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ]
+default = ["dim2", "f32"]
+dim2 = []
+f32 = []
+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 = [ "vec_map" ]
-wasm-bindgen = [ "instant/wasm-bindgen" ]
-serde-serialize = [ "nalgebra/serde-serialize", "parry2d/serde-serialize", "serde", "bit-vec/serde", "arrayvec/serde" ]
-enhanced-determinism = [ "simba/libm_force", "parry2d/enhanced-determinism" ]
-debug-render = [ ]
-profiler = [ "instant" ] # Enables the internal profiler.
+simd-is-enabled = ["vec_map"]
+wasm-bindgen = ["instant/wasm-bindgen"]
+serde-serialize = ["nalgebra/serde-serialize", "parry2d/serde-serialize", "serde", "bit-vec/serde", "arrayvec/serde"]
+enhanced-determinism = ["simba/libm_force", "parry2d/enhanced-determinism"]
+debug-render = []
+profiler = ["instant"] # Enables the internal profiler.
 
 # Feature used for debugging only.
-debug-disable-legitimate-fe-exceptions = [ ]
+debug-disable-legitimate-fe-exceptions = []
 
 # Feature used for development and debugging only.
 # Do not enable this unless you are working on the engine internals.
@@ -44,15 +44,15 @@ features = ["parallel", "simd-stable", "serde-serialize", "debug-render"]
 [lib]
 name = "rapier2d"
 path = "../../src/lib.rs"
-required-features = [ "dim2", "f32" ]
+required-features = ["dim2", "f32"]
 
 
 [dependencies]
 vec_map = { version = "0.8", optional = true }
-instant = { version = "0.1", features = [ "now" ], optional = true }
+instant = { version = "0.1", features = ["now"], optional = true }
 num-traits = "0.2"
 nalgebra = "0.32"
-parry2d = "0.13.1"
+parry2d = "0.15.0"
 simba = "0.8"
 approx = "0.5"
 rayon = { version = "1", optional = true }
@@ -60,11 +60,13 @@ crossbeam = "0.8"
 arrayvec = "0.7"
 bit-vec = "0.6"
 rustc-hash = "1"
-serde = { version = "1", features = [ "derive" ], optional = true }
+serde = { version = "1", features = ["derive"], optional = true }
 downcast-rs = "1.2"
 num-derive = "0.4"
 bitflags = "1"
+log = "0.4"
+ordered-float = "4"
 
 [dev-dependencies]
 bincode = "1"
-serde = { version = "1", features = [ "derive" ] }
+serde = { version = "1", features = ["derive"] }
diff --git a/crates/rapier3d-f64/Cargo.toml b/crates/rapier3d-f64/Cargo.toml
index 12c71b7..28f282c 100644
--- a/crates/rapier3d-f64/Cargo.toml
+++ b/crates/rapier3d-f64/Cargo.toml
@@ -1,14 +1,14 @@
 [package]
-name    = "rapier3d-f64"
-version = "0.18.0"
-authors = [ "Sébastien Crozet <developer@crozet.re>" ]
+name = "rapier3d-f64"
+version = "0.19.0"
+authors = ["Sébastien Crozet <developer@crozet.re>"]
 description = "3-dimensional physics engine in Rust."
 documentation = "https://docs.rs/rapier3d"
 homepage = "https://rapier.rs"
 repository = "https://github.com/dimforge/rapier"
 readme = "README.md"
-categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
-keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
+categories = ["science", "game-development", "mathematics", "simulation", "wasm"]
+keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
 license = "Apache-2.0"
 edition = "2021"
 
@@ -16,23 +16,23 @@ edition = "2021"
 maintenance = { status = "actively-developed" }
 
 [features]
-default = [ "dim3", "f64" ]
-dim3    = [ ]
-f64     = [ ]
-parallel = [ "rayon" ]
-simd-stable = [ "parry3d-f64/simd-stable", "simba/wide", "simd-is-enabled" ]
-simd-nightly = [ "parry3d-f64/simd-nightly", "simba/packed_simd", "simd-is-enabled" ]
+default = ["dim3", "f64"]
+dim3 = []
+f64 = []
+parallel = ["rayon"]
+simd-stable = ["parry3d-f64/simd-stable", "simba/wide", "simd-is-enabled"]
+simd-nightly = ["parry3d-f64/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 = [ "vec_map" ]
-wasm-bindgen = [ "instant/wasm-bindgen" ]
-serde-serialize = [ "nalgebra/serde-serialize", "parry3d-f64/serde-serialize", "serde", "bit-vec/serde" ]
-enhanced-determinism = [ "simba/libm_force", "parry3d-f64/enhanced-determinism" ]
+simd-is-enabled = ["vec_map"]
+wasm-bindgen = ["instant/wasm-bindgen"]
+serde-serialize = ["nalgebra/serde-serialize", "parry3d-f64/serde-serialize", "serde", "bit-vec/serde"]
+enhanced-determinism = ["simba/libm_force", "parry3d-f64/enhanced-determinism"]
 debug-render = []
-profiler = [ "instant" ] # Enables the internal profiler.
+profiler = ["instant"] # Enables the internal profiler.
 
 # Feature used for debugging only.
-debug-disable-legitimate-fe-exceptions = [ ]
+debug-disable-legitimate-fe-exceptions = []
 
 # Feature used for development and debugging only.
 # Do not enable this unless you are working on the engine internals.
@@ -44,15 +44,15 @@ features = ["parallel", "simd-stable", "serde-serialize", "debug-render"]
 [lib]
 name = "rapier3d_f64"
 path = "../../src/lib.rs"
-required-features = [ "dim3", "f64" ]
+required-features = ["dim3", "f64"]
 
 
 [dependencies]
 vec_map = { version = "0.8", optional = true }
-instant = { version = "0.1", features = [ "now" ], optional = true }
+instant = { version = "0.1", features = ["now"], optional = true }
 num-traits = "0.2"
 nalgebra = "0.32"
-parry3d-f64 = "0.13.1"
+parry3d-f64 = "0.15.0"
 simba = "0.8"
 approx = "0.5"
 rayon = { version = "1", optional = true }
@@ -60,11 +60,13 @@ crossbeam = "0.8"
 arrayvec = "0.7"
 bit-vec = "0.6"
 rustc-hash = "1"
-serde = { version = "1", features = [ "derive" ], optional = true }
+serde = { version = "1", features = ["derive"], optional = true }
 downcast-rs = "1.2"
 num-derive = "0.4"
 bitflags = "1"
+log = "0.4"
+ordered-float = "4"
 
 [dev-dependencies]
 bincode = "1"
-serde = { version = "1", features = [ "derive" ] }
+serde = { version = "1", features = ["derive"] }
diff --git a/crates/rapier3d/Cargo.toml b/crates/rapier3d/Cargo.toml
index 1d3605b..10c5fe1 100644
--- a/crates/rapier3d/Cargo.toml
+++ b/crates/rapier3d/Cargo.toml
@@ -1,14 +1,14 @@
 [package]
-name    = "rapier3d"
-version = "0.18.0"
-authors = [ "Sébastien Crozet <developer@crozet.re>" ]
+name = "rapier3d"
+version = "0.19.0"
+authors = ["Sébastien Crozet <developer@crozet.re>"]
 description = "3-dimensional physics engine in Rust."
 documentation = "https://docs.rs/rapier3d"
 homepage = "https://rapier.rs"
 repository = "https://github.com/dimforge/rapier"
 readme = "README.md"
-categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
-keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
+categories = ["science", "game-development", "mathematics", "simulation", "wasm"]
+keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
 license = "Apache-2.0"
 edition = "2021"
 
@@ -16,23 +16,23 @@ edition = "2021"
 maintenance = { status = "actively-developed" }
 
 [features]
-default = [ "dim3", "f32" ]
-dim3    = [ ]
-f32     = [ ]
-parallel = [ "rayon" ]
-simd-stable = [ "parry3d/simd-stable", "simba/wide", "simd-is-enabled" ]
-simd-nightly = [ "parry3d/simd-nightly", "simba/packed_simd", "simd-is-enabled" ]
+default = ["dim3", "f32"]
+dim3 = []
+f32 = []
+parallel = ["rayon"]
+simd-stable = ["parry3d/simd-stable", "simba/wide", "simd-is-enabled"]
+simd-nightly = ["parry3d/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 = [ "vec_map" ]
-wasm-bindgen = [ "instant/wasm-bindgen" ]
-serde-serialize = [ "nalgebra/serde-serialize", "parry3d/serde-serialize", "serde", "bit-vec/serde" ]
-enhanced-determinism = [ "simba/libm_force", "parry3d/enhanced-determinism" ]
-debug-render = [ ]
-profiler = [ "instant" ] # Enables the internal profiler.
+simd-is-enabled = ["vec_map"]
+wasm-bindgen = ["instant/wasm-bindgen"]
+serde-serialize = ["nalgebra/serde-serialize", "parry3d/serde-serialize", "serde", "bit-vec/serde"]
+enhanced-determinism = ["simba/libm_force", "parry3d/enhanced-determinism"]
+debug-render = []
+profiler = ["instant"] # Enables the internal profiler.
 
 # Feature used for debugging only.
-debug-disable-legitimate-fe-exceptions = [ ]
+debug-disable-legitimate-fe-exceptions = []
 
 # Feature used for development and debugging only.
 # Do not enable this unless you are working on the engine internals.
@@ -44,15 +44,15 @@ features = ["parallel", "simd-stable", "serde-serialize", "debug-render"]
 [lib]
 name = "rapier3d"
 path = "../../src/lib.rs"
-required-features = [ "dim3", "f32" ]
+required-features = ["dim3", "f32"]
 
 
 [dependencies]
 vec_map = { version = "0.8", optional = true }
-instant = { version = "0.1", features = [ "now" ], optional = true }
+instant = { version = "0.1", features = ["now"], optional = true }
 num-traits = "0.2"
 nalgebra = "0.32"
-parry3d = "0.13.1"
+parry3d = "0.15.0"
 simba = "0.8"
 approx = "0.5"
 rayon = { version = "1", optional = true }
@@ -60,11 +60,13 @@ crossbeam = "0.8"
 arrayvec = "0.7"
 bit-vec = "0.6"
 rustc-hash = "1"
-serde = { version = "1", features = [ "derive" ], optional = true }
+serde = { version = "1", features = ["derive"], optional = true }
 downcast-rs = "1.2"
 num-derive = "0.4"
 bitflags = "1"
+log = "0.4"
+ordered-float = "4"
 
 [dev-dependencies]
 bincode = "1"
-serde = { version = "1", features = [ "derive" ] }
+serde = { version = "1", features = ["derive"] }
diff --git a/crates/rapier_testbed2d-f64/Cargo.toml b/crates/rapier_testbed2d-f64/Cargo.toml
index 2212a03..800ac49 100644
--- a/crates/rapier_testbed2d-f64/Cargo.toml
+++ b/crates/rapier_testbed2d-f64/Cargo.toml
@@ -1,12 +1,12 @@
 [package]
-name    = "rapier_testbed2d-f64"
-version = "0.18.0"
-authors = [ "Sébastien Crozet <developer@crozet.re>" ]
+name = "rapier_testbed2d-f64"
+version = "0.19.0"
+authors = ["Sébastien Crozet <developer@crozet.re>"]
 description = "Testbed for the Rapier 2-dimensional physics engine in Rust."
 homepage = "http://rapier.org"
 repository = "https://github.com/dimforge/rapier"
-categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
-keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
+categories = ["science", "game-development", "mathematics", "simulation", "wasm"]
+keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
 license = "Apache-2.0"
 edition = "2021"
 
@@ -16,48 +16,48 @@ maintenance = { status = "actively-developed" }
 [lib]
 name = "rapier_testbed2d"
 path = "../../src_testbed/lib.rs"
-required-features = [ "dim2" ]
+required-features = ["dim2"]
 
 [features]
-default = [ "dim2" ]
-dim2 = [ ]
-parallel = [ "rapier/parallel", "num_cpus" ]
-other-backends = [ "wrapped2d" ]
+default = ["dim2"]
+dim2 = []
+parallel = ["rapier/parallel", "num_cpus"]
+other-backends = ["wrapped2d"]
 
 [package.metadata.docs.rs]
 features = ["parallel", "other-backends"]
 
 [dependencies]
-nalgebra   = { version = "0.32", features = [ "rand" ] }
-rand       = "0.8"
-rand_pcg   = "0.3"
-instant    = { version = "0.1", features = [ "web-sys", "now" ]}
-bitflags   = "1"
-num_cpus   = { version = "1", optional = true }
-wrapped2d  = { version = "0.4", optional = true }
+nalgebra = { version = "0.32", features = ["rand", "glam025"] }
+rand = "0.8"
+rand_pcg = "0.3"
+instant = { version = "0.1", features = ["web-sys", "now"] }
+bitflags = "1"
+num_cpus = { version = "1", optional = true }
+wrapped2d = { version = "0.4", optional = true }
 crossbeam = "0.8"
 bincode = "1"
-Inflector  = "0.11"
+Inflector = "0.11"
 md5 = "0.7"
 
-bevy_egui = "0.23"
-bevy_ecs = "0.12"
-bevy_core_pipeline = "0.12"
-bevy_pbr = "0.12"
-bevy_sprite = "0.12"
+bevy_egui = "0.26"
+bevy_ecs = "0.13"
+bevy_core_pipeline = "0.13"
+bevy_pbr = "0.13"
+bevy_sprite = "0.13"
 #bevy_prototype_debug_lines = "0.7"
 
 # Dependencies for native only.
 [target.'cfg(not(target_arch = "wasm32"))'.dependencies]
-bevy = {version = "0.12", default-features = false, features = ["bevy_asset", "bevy_winit", "x11", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"]}
+bevy = { version = "0.13", default-features = false, features = ["bevy_asset", "bevy_winit", "x11", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"] }
 
 # Dependencies for WASM only.
 [target.'cfg(target_arch = "wasm32")'.dependencies]
-bevy = {version = "0.12", default-features = false, features = ["bevy_asset", "bevy_winit", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"]}
+bevy = { version = "0.13", default-features = false, features = ["bevy_asset", "bevy_winit", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"] }
 #bevy_webgl2 = "0.5"
 
 [dependencies.rapier]
 package = "rapier2d-f64"
 path = "../rapier2d-f64"
-version = "0.18.0"
-features = [ "serde-serialize", "debug-render", "profiler" ]
+version = "0.19.0"
+features = ["serde-serialize", "debug-render", "profiler"]
diff --git a/crates/rapier_testbed2d/Cargo.toml b/crates/rapier_testbed2d/Cargo.toml
index 06c32e3..3935fcb 100644
--- a/crates/rapier_testbed2d/Cargo.toml
+++ b/crates/rapier_testbed2d/Cargo.toml
@@ -1,12 +1,12 @@
 [package]
-name    = "rapier_testbed2d"
-version = "0.18.0"
-authors = [ "Sébastien Crozet <developer@crozet.re>" ]
+name = "rapier_testbed2d"
+version = "0.19.0"
+authors = ["Sébastien Crozet <developer@crozet.re>"]
 description = "Testbed for the Rapier 2-dimensional physics engine in Rust."
 homepage = "http://rapier.org"
 repository = "https://github.com/dimforge/rapier"
-categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
-keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
+categories = ["science", "game-development", "mathematics", "simulation", "wasm"]
+keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
 license = "Apache-2.0"
 edition = "2021"
 
@@ -16,48 +16,48 @@ maintenance = { status = "actively-developed" }
 [lib]
 name = "rapier_testbed2d"
 path = "../../src_testbed/lib.rs"
-required-features = [ "dim2" ]
+required-features = ["dim2"]
 
 [features]
-default = [ "dim2" ]
-dim2 = [ ]
-parallel = [ "rapier/parallel", "num_cpus" ]
-other-backends = [ "wrapped2d" ]
+default = ["dim2"]
+dim2 = []
+parallel = ["rapier/parallel", "num_cpus"]
+other-backends = ["wrapped2d"]
 
 [package.metadata.docs.rs]
 features = ["parallel", "other-backends"]
 
 [dependencies]
-nalgebra   = { version = "0.32", features = [ "rand" ] }
-rand       = "0.8"
-rand_pcg   = "0.3"
-instant    = { version = "0.1", features = [ "web-sys", "now" ]}
-bitflags   = "1"
-num_cpus   = { version = "1", optional = true }
-wrapped2d  = { version = "0.4", optional = true }
+nalgebra = { version = "0.32", features = ["rand", "glam025"] }
+rand = "0.8"
+rand_pcg = "0.3"
+instant = { version = "0.1", features = ["web-sys", "now"] }
+bitflags = "1"
+num_cpus = { version = "1", optional = true }
+wrapped2d = { version = "0.4", optional = true }
 crossbeam = "0.8"
 bincode = "1"
-Inflector  = "0.11"
+Inflector = "0.11"
 md5 = "0.7"
 
-bevy_egui = "0.23"
-bevy_ecs = "0.12"
-bevy_core_pipeline = "0.12"
-bevy_pbr = "0.12"
-bevy_sprite = "0.12"
+bevy_egui = "0.26"
+bevy_ecs = "0.13"
+bevy_core_pipeline = "0.13"
+bevy_pbr = "0.13"
+bevy_sprite = "0.13"
 #bevy_prototype_debug_lines = "0.7"
 
 # Dependencies for native only.
 [target.'cfg(not(target_arch = "wasm32"))'.dependencies]
-bevy = {version = "0.12", default-features = false, features = ["bevy_sprite", "bevy_winit", "x11", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"]}
+bevy = { version = "0.13", default-features = false, features = ["bevy_sprite", "bevy_winit", "x11", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"] }
 
 # Dependencies for WASM only.
 [target.'cfg(target_arch = "wasm32")'.dependencies]
-bevy = {version = "0.12", default-features = false, features = ["bevy_sprite", "bevy_winit", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"]}
+bevy = { version = "0.13", default-features = false, features = ["bevy_sprite", "bevy_winit", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"] }
 #bevy_webgl2 = "0.5"
 
 [dependencies.rapier]
 package = "rapier2d"
 path = "../rapier2d"
-version = "0.18.0"
-features = [ "serde-serialize", "debug-render", "profiler" ]
+version = "0.19.0"
+features = ["serde-serialize", "debug-render", "profiler"]
diff --git a/crates/rapier_testbed3d-f64/Cargo.toml b/crates/rapier_testbed3d-f64/Cargo.toml
index ca3c95c..8fd5628 100644
--- a/crates/rapier_testbed3d-f64/Cargo.toml
+++ b/crates/rapier_testbed3d-f64/Cargo.toml
@@ -1,12 +1,12 @@
 [package]
-name    = "rapier_testbed3d-f64"
-version = "0.18.0"
-authors = [ "Sébastien Crozet <developer@crozet.re>" ]
+name = "rapier_testbed3d-f64"
+version = "0.19.0"
+authors = ["Sébastien Crozet <developer@crozet.re>"]
 description = "Testbed for the Rapier 3-dimensional physics engine in Rust."
 homepage = "http://rapier.org"
 repository = "https://github.com/dimforge/rapier"
-categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
-keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
+categories = ["science", "game-development", "mathematics", "simulation", "wasm"]
+keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
 license = "Apache-2.0"
 edition = "2021"
 
@@ -16,47 +16,47 @@ maintenance = { status = "actively-developed" }
 [lib]
 name = "rapier_testbed3d"
 path = "../../src_testbed/lib.rs"
-required-features = [ "dim3" ]
+required-features = ["dim3"]
 
 [features]
-default = [ "dim3" ]
-dim3 = [ ]
-parallel = [ "rapier/parallel", "num_cpus" ]
+default = ["dim3"]
+dim3 = []
+parallel = ["rapier/parallel", "num_cpus"]
 
 [package.metadata.docs.rs]
 features = ["parallel"]
 
 [dependencies]
-nalgebra   = { version = "0.32", features = [ "rand" ] }
-rand       = "0.8"
-rand_pcg   = "0.3"
-instant    = { version = "0.1", features = [ "web-sys", "now" ]}
-bitflags   = "1"
-num_cpus   = { version = "1", optional = true }
+nalgebra = { version = "0.32", features = ["rand", "glam025"] }
+rand = "0.8"
+rand_pcg = "0.3"
+instant = { version = "0.1", features = ["web-sys", "now"] }
+bitflags = "1"
+num_cpus = { version = "1", optional = true }
 crossbeam = "0.8"
 bincode = "1"
 md5 = "0.7"
-Inflector  = "0.11"
-serde = { version = "1", features = [ "derive" ] }
+Inflector = "0.11"
+serde = { version = "1", features = ["derive"] }
 
-bevy_egui = "0.23"
-bevy_ecs = "0.12"
-bevy_core_pipeline = "0.12"
-bevy_pbr = "0.12"
-bevy_sprite = "0.12"
+bevy_egui = "0.26"
+bevy_ecs = "0.13"
+bevy_core_pipeline = "0.13"
+bevy_pbr = "0.13"
+bevy_sprite = "0.13"
 #bevy_prototype_debug_lines = { version = "0.7", features = [ "3d" ] }
 
 # Dependencies for native only.
 [target.'cfg(not(target_arch = "wasm32"))'.dependencies]
-bevy = {version = "0.12", default-features = false, features = ["bevy_winit", "x11", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"]}
+bevy = { version = "0.13", default-features = false, features = ["bevy_winit", "x11", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"] }
 
 # Dependencies for WASM only.
 [target.'cfg(target_arch = "wasm32")'.dependencies]
-bevy = {version = "0.12", default-features = false, features = ["bevy_winit", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"]}
+bevy = { version = "0.13", default-features = false, features = ["bevy_winit", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"] }
 #bevy_webgl2 = "0.5"
 
 [dependencies.rapier]
 package = "rapier3d-f64"
 path = "../rapier3d-f64"
-version = "0.18.0"
-features = [ "serde-serialize", "debug-render", "profiler" ]
+version = "0.19.0"
+features = ["serde-serialize", "debug-render", "profiler"]
diff --git a/crates/rapier_testbed3d/Cargo.toml b/crates/rapier_testbed3d/Cargo.toml
index 3b3bdeb..a3f3c5d 100644
--- a/crates/rapier_testbed3d/Cargo.toml
+++ b/crates/rapier_testbed3d/Cargo.toml
@@ -1,12 +1,12 @@
 [package]
-name    = "rapier_testbed3d"
-version = "0.18.0"
-authors = [ "Sébastien Crozet <developer@crozet.re>" ]
+name = "rapier_testbed3d"
+version = "0.19.0"
+authors = ["Sébastien Crozet <developer@crozet.re>"]
 description = "Testbed for the Rapier 3-dimensional physics engine in Rust."
 homepage = "http://rapier.org"
 repository = "https://github.com/dimforge/rapier"
-categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
-keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
+categories = ["science", "game-development", "mathematics", "simulation", "wasm"]
+keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
 license = "Apache-2.0"
 edition = "2021"
 
@@ -16,51 +16,51 @@ maintenance = { status = "actively-developed" }
 [lib]
 name = "rapier_testbed3d"
 path = "../../src_testbed/lib.rs"
-required-features = [ "dim3" ]
+required-features = ["dim3"]
 
 [features]
-default = [ "dim3" ]
-dim3 = [ ]
-parallel = [ "rapier/parallel", "num_cpus" ]
-other-backends = [ "physx", "physx-sys", "glam" ]
+default = ["dim3"]
+dim3 = []
+parallel = ["rapier/parallel", "num_cpus"]
+other-backends = ["physx", "physx-sys", "glam"]
 
 [package.metadata.docs.rs]
 features = ["parallel", "other-backends"]
 
 [dependencies]
-nalgebra   = { version = "0.32", features = [ "rand" ] }
-rand       = "0.8"
-rand_pcg   = "0.3"
-instant    = { version = "0.1", features = [ "web-sys", "now" ]}
-bitflags   = "1"
-glam       = { version = "0.24", optional = true } # For Physx
-num_cpus   = { version = "1", optional = true }
-physx      = { version = "0.19", features = [ "glam" ], optional = true }
+nalgebra = { version = "0.32", features = ["rand", "glam025"] }
+rand = "0.8"
+rand_pcg = "0.3"
+instant = { version = "0.1", features = ["web-sys", "now"] }
+bitflags = "1"
+glam = { version = "0.24", optional = true } # For Physx
+num_cpus = { version = "1", optional = true }
+physx = { version = "0.19", features = ["glam"], optional = true }
 physx-sys = { version = "0.11", optional = true }
 crossbeam = "0.8"
 bincode = "1"
 md5 = "0.7"
-Inflector  = "0.11"
-serde = { version = "1", features = [ "derive" ] }
+Inflector = "0.11"
+serde = { version = "1", features = ["derive"] }
 
-bevy_egui = "0.23"
-bevy_ecs = "0.12"
-bevy_core_pipeline = "0.12"
-bevy_pbr = "0.12"
-bevy_sprite = "0.12"
+bevy_egui = "0.26"
+bevy_ecs = "0.13"
+bevy_core_pipeline = "0.13"
+bevy_pbr = "0.13"
+bevy_sprite = "0.13"
 #bevy_prototype_debug_lines = { version = "0.7", features = [ "3d" ] }
 
 # Dependencies for native only.
 [target.'cfg(not(target_arch = "wasm32"))'.dependencies]
-bevy = {version = "0.12", default-features = false, features = ["bevy_winit", "x11", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"]}
+bevy = { version = "0.13", default-features = false, features = ["bevy_winit", "x11", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"] }
 
 # Dependencies for WASM only.
 [target.'cfg(target_arch = "wasm32")'.dependencies]
-bevy = {version = "0.12", default-features = false, features = ["bevy_winit", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"]}
+bevy = { version = "0.13", default-features = false, features = ["bevy_winit", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"] }
 #bevy_webgl2 = "0.5"
 
 [dependencies.rapier]
 package = "rapier3d"
 path = "../rapier3d"
-version = "0.18.0"
-features = [ "serde-serialize", "debug-render", "profiler" ]
+version = "0.19.0"
+features = ["serde-serialize", "debug-render", "profiler"]
diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs
index 4cdbf98..39d85f6 100644
--- a/examples2d/all_examples2.rs
+++ b/examples2d/all_examples2.rs
@@ -15,8 +15,12 @@ mod collision_groups2;
 mod convex_polygons2;
 mod damping2;
 mod debug_box_ball2;
+mod debug_compression2;
+mod debug_total_overlap2;
+mod debug_vertical_column2;
 mod drum2;
 mod heightfield2;
+mod inverse_kinematics2;
 mod joint_motor_position2;
 mod joints2;
 mod locked_rotations2;
@@ -26,6 +30,17 @@ mod polyline2;
 mod pyramid2;
 mod restitution2;
 mod rope_joints2;
+mod s2d_arch;
+mod s2d_ball_and_chain;
+mod s2d_bridge;
+mod s2d_card_house;
+mod s2d_confined;
+mod s2d_far_pyramid;
+mod s2d_high_mass_ratio_1;
+mod s2d_high_mass_ratio_2;
+mod s2d_high_mass_ratio_3;
+mod s2d_joint_grid;
+mod s2d_pyramid;
 mod sensor2;
 mod trimesh2;
 
@@ -70,6 +85,7 @@ pub fn main() {
         ("Damping", damping2::init_world),
         ("Drum", drum2::init_world),
         ("Heightfield", heightfield2::init_world),
+        ("Inverse kinematics", inverse_kinematics2::init_world),
         ("Joints", joints2::init_world),
         ("Locked rotations", locked_rotations2::init_world),
         ("One-way platforms", one_way_platforms2::init_world),
@@ -82,6 +98,23 @@ pub fn main() {
         ("Trimesh", trimesh2::init_world),
         ("Joint motor position", joint_motor_position2::init_world),
         ("(Debug) box ball", debug_box_ball2::init_world),
+        ("(Debug) compression", debug_compression2::init_world),
+        ("(Debug) total overlap", debug_total_overlap2::init_world),
+        (
+            "(Debug) vertical column",
+            debug_vertical_column2::init_world,
+        ),
+        ("(s2d) high mass ratio 1", s2d_high_mass_ratio_1::init_world),
+        ("(s2d) high mass ratio 2", s2d_high_mass_ratio_2::init_world),
+        ("(s2d) high mass ratio 3", s2d_high_mass_ratio_3::init_world),
+        ("(s2d) confined", s2d_confined::init_world),
+        ("(s2d) pyramid", s2d_pyramid::init_world),
+        ("(s2d) card house", s2d_card_house::init_world),
+        ("(s2d) arch", s2d_arch::init_world),
+        ("(s2d) bridge", s2d_bridge::init_world),
+        ("(s2d) ball and chain", s2d_ball_and_chain::init_world),
+        ("(s2d) joint grid", s2d_joint_grid::init_world),
+        ("(s2d) far pyramid", s2d_far_pyramid::init_world),
     ];
 
     // Lexicographic sort, with stress tests moved at the end of the list.
diff --git a/examples2d/character_controller2.rs b/examples2d/character_controller2.rs
index 8ecd23c..2325101 100644
--- a/examples2d/character_controller2.rs
+++ b/examples2d/character_controller2.rs
@@ -27,7 +27,7 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let rigid_body = RigidBodyBuilder::kinematic_position_based().translation(vector![-3.0, 5.0]);
     let character_handle = bodies.insert(rigid_body);
-    let collider = ColliderBuilder::cuboid(0.15, 0.3);
+    let collider = ColliderBuilder::capsule_y(0.3, 0.15);
     colliders.insert_with_parent(collider, character_handle, &mut bodies);
 
     /*
@@ -94,14 +94,18 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let wall_angle = PI / 2.;
     let wall_size = 2.0;
+    let wall_pos = vector![
+        ground_size + slope_size * 2.0 + impossible_slope_size + 0.35,
+        -ground_height + 2.5 * 2.3
+    ];
     let collider = ColliderBuilder::cuboid(wall_size, ground_height)
-        .translation(vector![
-            ground_size + slope_size * 2.0 + impossible_slope_size + 0.35,
-            -ground_height + 2.5 * 2.3
-        ])
+        .translation(wall_pos)
         .rotation(wall_angle);
     colliders.insert(collider);
 
+    let collider = ColliderBuilder::cuboid(wall_size, ground_height).translation(wall_pos);
+    colliders.insert(collider);
+
     /*
      * Create a moving platform.
      */
diff --git a/examples2d/debug_compression2.rs b/examples2d/debug_compression2.rs
new file mode 100644
index 0000000..e0052d6
--- /dev/null
+++ b/examples2d/debug_compression2.rs
@@ -0,0 +1,75 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Ground
+     */
+    let width = 75.0;
+    let thickness = 2.0;
+    let ys = [-30.0 - thickness, 30.0 + thickness];
+
+    for y in ys {
+        let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, y]);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(width, thickness);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+    }
+
+    // Build two compression boxes rigid body.
+    let half_height = (ys[1] - ys[0]) / 2.0 - thickness;
+    let xs = [-width + thickness, width - thickness];
+    let mut handles = [RigidBodyHandle::invalid(); 2];
+
+    for i in 0..2 {
+        let rigid_body = RigidBodyBuilder::dynamic().translation(vector![xs[i], 0.0]);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(thickness, half_height);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+        handles[i] = handle;
+    }
+
+    // Build the balls.
+    let num = 8;
+    let rad = half_height / (num as f32);
+    for i in 0..num {
+        for j in 0..num {
+            let x = i as f32 * rad * 2.0 - num as f32 * rad;
+            let y = j as f32 * rad * 2.0 - num as f32 * rad + rad;
+            let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y]);
+            let handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::ball(rad);
+            colliders.insert_with_parent(collider, handle, &mut bodies);
+        }
+    }
+
+    let mut force = vector![0.0, 0.0];
+
+    testbed.add_callback(move |_, physics, _, _| {
+        let left_plank = &mut physics.bodies[handles[0]];
+        left_plank.reset_forces(true);
+        left_plank.add_force(force, true);
+
+        let right_plank = &mut physics.bodies[handles[1]];
+        right_plank.reset_forces(true);
+        right_plank.add_force(-force, true);
+
+        force.x += 10000.0;
+
+        println!("force: {}", force.x);
+    });
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 0.0], 50.0);
+}
diff --git a/examples2d/debug_total_overlap2.rs b/examples2d/debug_total_overlap2.rs
new file mode 100644
index 0000000..b61ad67
--- /dev/null
+++ b/examples2d/debug_total_overlap2.rs
@@ -0,0 +1,28 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    // Build many balls, all spawned at the same point.
+    let rad = 0.5;
+
+    for _ in 0..100 {
+        let rigid_body = RigidBodyBuilder::dynamic();
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(rad, rad);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 0.0], 50.0);
+}
diff --git a/examples2d/debug_vertical_column2.rs b/examples2d/debug_vertical_column2.rs
new file mode 100644
index 0000000..4ca50db
--- /dev/null
+++ b/examples2d/debug_vertical_column2.rs
@@ -0,0 +1,47 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    let num = 80;
+    let rad = 0.5;
+
+    /*
+     * Ground
+     */
+    let ground_size = 1.0;
+    let ground_thickness = 1.0;
+
+    let rigid_body = RigidBodyBuilder::fixed();
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_thickness).friction(0.3);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+
+    for i in 0..num {
+        let y = i as f32 * rad * 2.0 + ground_thickness + rad;
+
+        // Build the rigid body.
+        let rigid_body = RigidBodyBuilder::dynamic().translation(vector![0.0, y]);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(rad, rad).friction(0.3);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    // testbed.harness_mut().physics.gravity.y = -981.0;
+    testbed.look_at(point![0.0, 2.5], 5.0);
+}
diff --git a/examples2d/inverse_kinematics2.rs b/examples2d/inverse_kinematics2.rs
new file mode 100644
index 0000000..985bdb2
--- /dev/null
+++ b/examples2d/inverse_kinematics2.rs
@@ -0,0 +1,96 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let mut multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 1.0;
+    let ground_height = 0.01;
+
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height]);
+    let floor_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height);
+    colliders.insert_with_parent(collider, floor_handle, &mut bodies);
+
+    /*
+     * Setup groups
+     */
+    let num_segments = 10;
+    let body = RigidBodyBuilder::fixed();
+    let mut last_body = bodies.insert(body);
+    let mut last_link = MultibodyJointHandle::invalid();
+
+    for i in 0..num_segments {
+        let size = 1.0 / num_segments as f32;
+        let body = RigidBodyBuilder::dynamic().can_sleep(false);
+        let new_body = bodies.insert(body);
+        // NOTE: we add a sensor collider just to make the destbed draw a rectangle to make
+        //       the demo look nicer. IK could be used without cuboid.
+        let collider = ColliderBuilder::cuboid(size / 8.0, size / 2.0)
+            .density(0.0)
+            .sensor(true);
+        colliders.insert_with_parent(collider, new_body, &mut bodies);
+
+        let link_ab = RevoluteJointBuilder::new()
+            .local_anchor1(point![0.0, size / 2.0 * (i != 0) as usize as f32])
+            .local_anchor2(point![0.0, -size / 2.0])
+            .build()
+            .data;
+
+        last_link = multibody_joints
+            .insert(last_body, new_body, link_ab, true)
+            .unwrap();
+
+        last_body = new_body;
+    }
+
+    let mut displacements = DVector::zeros(0);
+
+    testbed.add_callback(move |graphics, physics, _, _| {
+        let Some(graphics) = graphics else { return };
+        if let Some((multibody, link_id)) = physics.multibody_joints.get_mut(last_link) {
+            // Ensure our displacement vector has the right number of elements.
+            if displacements.nrows() < multibody.ndofs() {
+                displacements = DVector::zeros(multibody.ndofs());
+            } else {
+                displacements.fill(0.0);
+            }
+
+            let Some(mouse_point) = graphics.mouse().point else {
+                return;
+            };
+
+            // We will have the endpoint track the mouse position.
+            let target_point = mouse_point.coords;
+
+            let options = InverseKinematicsOption {
+                constrained_axes: JointAxesMask::LIN_AXES,
+                ..Default::default()
+            };
+
+            multibody.inverse_kinematics(
+                &physics.bodies,
+                link_id,
+                &options,
+                &Isometry::from(target_point),
+                &mut displacements,
+            );
+            multibody.apply_displacements(displacements.as_slice());
+        }
+    });
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 0.0], 300.0);
+}
diff --git a/examples2d/s2d_arch.rs b/examples2d/s2d_arch.rs
new file mode 100644
index 0000000..5b935e9
--- /dev/null
+++ b/examples2d/s2d_arch.rs
@@ -0,0 +1,109 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    #[allow(clippy::excessive_precision)]
+    let mut ps1 = [
+        Point::new(16.0, 0.0),
+        Point::new(14.93803712795643, 5.133601056842984),
+        Point::new(13.79871746027416, 10.24928069555078),
+        Point::new(12.56252963284711, 15.34107019122473),
+        Point::new(11.20040987372525, 20.39856541571217),
+        Point::new(9.66521217819836, 25.40369899225096),
+        Point::new(7.87179930638133, 30.3179337000085),
+        Point::new(5.635199558196225, 35.03820717801641),
+        Point::new(2.405937953536585, 39.09554102558315),
+    ];
+
+    #[allow(clippy::excessive_precision)]
+    let mut ps2 = [
+        Point::new(24.0, 0.0),
+        Point::new(22.33619528222415, 6.02299846205841),
+        Point::new(20.54936888969905, 12.00964361211476),
+        Point::new(18.60854610798073, 17.9470321677465),
+        Point::new(16.46769273811807, 23.81367936585418),
+        Point::new(14.05325025774858, 29.57079353071012),
+        Point::new(11.23551045834022, 35.13775818285372),
+        Point::new(7.752568160730571, 40.30450679009583),
+        Point::new(3.016931552701656, 44.28891593799322),
+    ];
+
+    let scale = 0.25;
+    let friction = 0.6;
+
+    for i in 0..9 {
+        ps1[i] *= scale;
+        ps2[i] *= scale;
+    }
+
+    /*
+     * Ground
+     */
+    let collider = ColliderBuilder::segment(point![-100.0, 0.0], point![100.0, 0.0]).friction(0.6);
+    colliders.insert(collider);
+
+    /*
+     * Create the arch
+     */
+    for i in 0..8 {
+        let ps = [ps1[i], ps2[i], ps2[i + 1], ps1[i + 1]];
+        let rigid_body = RigidBodyBuilder::dynamic();
+        let ground_handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::convex_hull(&ps)
+            .unwrap()
+            .friction(friction);
+        colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+    }
+
+    for i in 0..8 {
+        let ps = [
+            point![-ps2[i].x, ps2[i].y],
+            point![-ps1[i].x, ps1[i].y],
+            point![-ps1[i + 1].x, ps1[i + 1].y],
+            point![-ps2[i + 1].x, ps2[i + 1].y],
+        ];
+        let rigid_body = RigidBodyBuilder::dynamic();
+        let ground_handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::convex_hull(&ps)
+            .unwrap()
+            .friction(friction);
+        colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+    }
+
+    {
+        let ps = [
+            ps1[8],
+            ps2[8],
+            point![-ps1[8].x, ps1[8].y],
+            point![-ps2[8].x, ps2[8].y],
+        ];
+        let rigid_body = RigidBodyBuilder::dynamic();
+        let ground_handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::convex_hull(&ps)
+            .unwrap()
+            .friction(friction);
+        colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+    }
+
+    for i in 0..4 {
+        let rigid_body =
+            RigidBodyBuilder::dynamic().translation(vector![0.0, 0.5 + ps2[8].y + 1.0 * i as f32]);
+        let ground_handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(2.0, 0.5).friction(friction);
+        colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 20.0);
+}
diff --git a/examples2d/s2d_ball_and_chain.rs b/examples2d/s2d_ball_and_chain.rs
new file mode 100644
index 0000000..29e0d87
--- /dev/null
+++ b/examples2d/s2d_ball_and_chain.rs
@@ -0,0 +1,73 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground = bodies.insert(RigidBodyBuilder::fixed());
+
+    /*
+     * Create the bridge.
+     */
+    let count = 40;
+    let hx = 0.5;
+    let density = 20.0;
+    let friction = 0.6;
+    let capsule = ColliderBuilder::capsule_x(hx, 0.125)
+        .friction(friction)
+        .density(density);
+
+    let mut prev = ground;
+    for i in 0..count {
+        let rigid_body = RigidBodyBuilder::dynamic()
+            .linear_damping(0.1)
+            .angular_damping(0.1)
+            .translation(vector![(1.0 + 2.0 * i as f32) * hx, count as f32 * hx]);
+        let handle = bodies.insert(rigid_body);
+        colliders.insert_with_parent(capsule.clone(), handle, &mut bodies);
+
+        let pivot = point![(2.0 * i as f32) * hx, count as f32 * hx];
+        let joint = RevoluteJointBuilder::new()
+            .local_anchor1(bodies[prev].position().inverse_transform_point(&pivot))
+            .local_anchor2(bodies[handle].position().inverse_transform_point(&pivot))
+            .contacts_enabled(false);
+        impulse_joints.insert(prev, handle, joint, true);
+        prev = handle;
+    }
+
+    let radius = 8.0;
+    let rigid_body = RigidBodyBuilder::dynamic()
+        .linear_damping(0.1)
+        .angular_damping(0.1)
+        .translation(vector![
+            (1.0 + 2.0 * count as f32) * hx + radius - hx,
+            count as f32 * hx
+        ]);
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::ball(radius)
+        .friction(friction)
+        .density(density);
+    colliders.insert_with_parent(collider, handle, &mut bodies);
+
+    let pivot = point![(2.0 * count as f32) * hx, count as f32 * hx];
+    let joint = RevoluteJointBuilder::new()
+        .local_anchor1(bodies[prev].position().inverse_transform_point(&pivot))
+        .local_anchor2(bodies[handle].position().inverse_transform_point(&pivot))
+        .contacts_enabled(false);
+    impulse_joints.insert(prev, handle, joint, true);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 20.0);
+}
diff --git a/examples2d/s2d_bridge.rs b/examples2d/s2d_bridge.rs
new file mode 100644
index 0000000..ab9c96f
--- /dev/null
+++ b/examples2d/s2d_bridge.rs
@@ -0,0 +1,56 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground = bodies.insert(RigidBodyBuilder::fixed());
+
+    /*
+     * Create the bridge.
+     */
+    let density = 20.0;
+    let x_base = -80.0;
+    let count = 160;
+    let mut prev = ground;
+
+    for i in 0..count {
+        let rigid_body = RigidBodyBuilder::dynamic()
+            .linear_damping(0.1)
+            .angular_damping(0.1)
+            .translation(vector![x_base + 0.5 + 1.0 * i as f32, 20.0]);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(0.5, 0.125).density(density);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+
+        let pivot = point![x_base + 1.0 * i as f32, 20.0];
+        let joint = RevoluteJointBuilder::new()
+            .local_anchor1(bodies[prev].position().inverse_transform_point(&pivot))
+            .local_anchor2(bodies[handle].position().inverse_transform_point(&pivot))
+            .contacts_enabled(false);
+        impulse_joints.insert(prev, handle, joint, true);
+        prev = handle;
+    }
+
+    let pivot = point![x_base + 1.0 * count as f32, 20.0];
+    let joint = RevoluteJointBuilder::new()
+        .local_anchor1(bodies[prev].position().inverse_transform_point(&pivot))
+        .local_anchor2(bodies[ground].position().inverse_transform_point(&pivot))
+        .contacts_enabled(false);
+    impulse_joints.insert(prev, ground, joint, true);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 20.0);
+}
diff --git a/examples2d/s2d_card_house.rs b/examples2d/s2d_card_house.rs
new file mode 100644
index 0000000..5e2c5dc
--- /dev/null
+++ b/examples2d/s2d_card_house.rs
@@ -0,0 +1,78 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    let friction = 0.7;
+
+    /*
+     * Ground
+     */
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -2.0]);
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(40.0, 2.0).friction(friction);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let scale = 10.0;
+    let card_height = 0.2 * scale;
+    let card_thickness = 0.001 * scale;
+    let angle0 = 25.0 * std::f32::consts::PI / 180.0;
+    let angle1 = -25.0 * std::f32::consts::PI / 180.0;
+    let angle2 = 0.5 * std::f32::consts::PI;
+
+    let card_box = ColliderBuilder::cuboid(card_thickness, card_height).friction(friction);
+
+    let mut nb = 5;
+    let mut z0 = 0.0;
+    let mut y = card_height - 0.02 * scale;
+
+    while nb != 0 {
+        let mut z = z0;
+
+        for i in 0..nb {
+            if i != nb - 1 {
+                let rigid_body = RigidBodyBuilder::dynamic()
+                    .translation(vector![z + 0.25 * scale, y + card_height - 0.015 * scale])
+                    .rotation(angle2);
+                let ground_handle = bodies.insert(rigid_body);
+                colliders.insert_with_parent(card_box.clone(), ground_handle, &mut bodies);
+            }
+
+            let rigid_body = RigidBodyBuilder::dynamic()
+                .translation(vector![z, y])
+                .rotation(angle1);
+            let ground_handle = bodies.insert(rigid_body);
+            colliders.insert_with_parent(card_box.clone(), ground_handle, &mut bodies);
+
+            z += 0.175 * scale;
+
+            let rigid_body = RigidBodyBuilder::dynamic()
+                .translation(vector![z, y])
+                .rotation(angle0);
+            let ground_handle = bodies.insert(rigid_body);
+            colliders.insert_with_parent(card_box.clone(), ground_handle, &mut bodies);
+
+            z += 0.175 * scale;
+        }
+
+        y += card_height * 2.0 - 0.03 * scale;
+        z0 += 0.175 * scale;
+        nb -= 1;
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 20.0);
+}
diff --git a/examples2d/s2d_confined.rs b/examples2d/s2d_confined.rs
new file mode 100644
index 0000000..ee54408
--- /dev/null
+++ b/examples2d/s2d_confined.rs
@@ -0,0 +1,69 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    let radius = 0.5;
+    let grid_count = 25;
+    let friction = 0.6;
+    let max_count = grid_count * grid_count;
+
+    /*
+     * Ground
+     */
+    let collider =
+        ColliderBuilder::capsule_from_endpoints(point![-10.5, 0.0], point![10.5, 0.0], radius)
+            .friction(friction);
+    colliders.insert(collider);
+    let collider =
+        ColliderBuilder::capsule_from_endpoints(point![-10.5, 0.0], point![-10.5, 20.5], radius)
+            .friction(friction);
+    colliders.insert(collider);
+    let collider =
+        ColliderBuilder::capsule_from_endpoints(point![10.5, 0.0], point![10.5, 20.5], radius)
+            .friction(friction);
+    colliders.insert(collider);
+    let collider =
+        ColliderBuilder::capsule_from_endpoints(point![-10.5, 20.5], point![10.5, 20.5], radius)
+            .friction(friction);
+    colliders.insert(collider);
+
+    /*
+     * Create the spheres
+     */
+    let mut row;
+    let mut count = 0;
+    let mut column = 0;
+
+    while count < max_count {
+        row = 0;
+        for _ in 0..grid_count {
+            let x = -8.75 + column as f32 * 18.0 / (grid_count as f32);
+            let y = 1.5 + row as f32 * 18.0 / (grid_count as f32);
+            let body = RigidBodyBuilder::dynamic()
+                .translation(vector![x, y])
+                .gravity_scale(0.0);
+            let body_handle = bodies.insert(body);
+            let ball = ColliderBuilder::ball(radius).friction(friction);
+            colliders.insert_with_parent(ball, body_handle, &mut bodies);
+
+            count += 1;
+            row += 1;
+        }
+
+        column += 1;
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 20.0);
+}
diff --git a/examples2d/s2d_far_pyramid.rs b/examples2d/s2d_far_pyramid.rs
new file mode 100644
index 0000000..7948731
--- /dev/null
+++ b/examples2d/s2d_far_pyramid.rs
@@ -0,0 +1,50 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    let origin = vector![100_000.0, -80_000.0];
+    let friction = 0.6;
+
+    /*
+     * Ground
+     */
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -1.0] + origin);
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(100.0, 1.0).friction(friction);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let base_count = 10;
+
+    let h = 0.5;
+    let shift = 1.25 * h;
+
+    for i in 0..base_count {
+        let y = (2.0 * i as f32 + 1.0) * shift + 0.5;
+
+        for j in i..base_count {
+            let x = (i as f32 + 1.0) * shift + 2.0 * (j as f32 - i as f32) * shift
+                - h * base_count as f32;
+            let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y] + origin);
+            let ground_handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::cuboid(h, h).friction(friction);
+            colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5] + origin, 20.0);
+}
diff --git a/examples2d/s2d_high_mass_ratio_1.rs b/examples2d/s2d_high_mass_ratio_1.rs
new file mode 100644
index 0000000..b211a5e
--- /dev/null
+++ b/examples2d/s2d_high_mass_ratio_1.rs
@@ -0,0 +1,66 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    let extent = 1.0;
+    let friction = 0.5;
+
+    /*
+     * Ground
+     */
+    let ground_width = 66.0 * extent;
+
+    let rigid_body = RigidBodyBuilder::fixed();
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::segment(
+        point![-0.5 * 2.0 * ground_width, 0.0],
+        point![0.5 * 2.0 * ground_width, 0.0],
+    )
+    .friction(friction);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+
+    for j in 0..3 {
+        let mut count = 10;
+        let offset = -20.0 * extent + 2.0 * (count as f32 + 1.0) * extent * j as f32;
+        let mut y = extent;
+
+        while count > 0 {
+            for i in 0..count {
+                let coeff = i as f32 - 0.5 * count as f32;
+                let yy = if count == 1 { y + 2.0 } else { y };
+                let position = vector![2.0 * coeff * extent + offset, yy];
+                let rigid_body = RigidBodyBuilder::dynamic().translation(position);
+                let parent = bodies.insert(rigid_body);
+
+                let collider = ColliderBuilder::cuboid(extent, extent)
+                    .density(if count == 1 {
+                        (j as f32 + 1.0) * 100.0
+                    } else {
+                        1.0
+                    })
+                    .friction(friction);
+                colliders.insert_with_parent(collider, parent, &mut bodies);
+            }
+
+            count -= 1;
+            y += 2.0 * extent;
+        }
+    }
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 20.0);
+}
diff --git a/examples2d/s2d_high_mass_ratio_2.rs b/examples2d/s2d_high_mass_ratio_2.rs
new file mode 100644
index 0000000..518df95
--- /dev/null
+++ b/examples2d/s2d_high_mass_ratio_2.rs
@@ -0,0 +1,53 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    let extent = 1.0;
+    let friction = 0.6;
+
+    /*
+     * Ground
+     */
+    let ground_width = 66.0 * extent;
+
+    let rigid_body = RigidBodyBuilder::fixed();
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::segment(
+        point![-0.5 * 2.0 * ground_width, 0.0],
+        point![0.5 * 2.0 * ground_width, 0.0],
+    )
+    .friction(friction);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let rigid_body = RigidBodyBuilder::dynamic().translation(vector![-9.0 * extent, 0.5 * extent]);
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(0.5 * extent, 0.5 * extent).friction(friction);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::dynamic().translation(vector![9.0 * extent, 0.5 * extent]);
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(0.5 * extent, 0.5 * extent).friction(friction);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::dynamic().translation(vector![0.0, (10.0 + 16.0) * extent]);
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(10.0 * extent, 10.0 * extent).friction(friction);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 20.0);
+}
diff --git a/examples2d/s2d_high_mass_ratio_3.rs b/examples2d/s2d_high_mass_ratio_3.rs
new file mode 100644
index 0000000..b1f6340
--- /dev/null
+++ b/examples2d/s2d_high_mass_ratio_3.rs
@@ -0,0 +1,47 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    let extent = 1.0;
+    let friction = 0.6;
+
+    /*
+     * Ground
+     */
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -2.0]);
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(40.0, 2.0).friction(friction);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let rigid_body = RigidBodyBuilder::dynamic().translation(vector![-9.0 * extent, 0.5 * extent]);
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(0.5 * extent, 0.5 * extent).friction(friction);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::dynamic().translation(vector![9.0 * extent, 0.5 * extent]);
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(0.5 * extent, 0.5 * extent).friction(friction);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::dynamic().translation(vector![0.0, (10.0 + 16.0) * extent]);
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(10.0 * extent, 10.0 * extent).friction(friction);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 20.0);
+}
diff --git a/examples2d/s2d_joint_grid.rs b/examples2d/s2d_joint_grid.rs
new file mode 100644
index 0000000..0033c70
--- /dev/null
+++ b/examples2d/s2d_joint_grid.rs
@@ -0,0 +1,63 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Create the joint grid.
+     */
+    let rad = 0.4;
+    let numi = 100;
+    let numk = 100;
+    let shift = 1.0;
+    let mut index = 0;
+    let mut handles = vec![RigidBodyHandle::invalid(); numi * numk];
+
+    for k in 0..numk {
+        for i in 0..numi {
+            let body_type = if k >= numk / 2 - 3 && k <= numk / 2 + 3 && i == 0 {
+                RigidBodyType::Fixed
+            } else {
+                RigidBodyType::Dynamic
+            };
+
+            let rigid_body = RigidBodyBuilder::new(body_type)
+                .translation(vector![k as f32 * shift, -(i as f32) * shift]);
+            let handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::ball(rad);
+            colliders.insert_with_parent(collider, handle, &mut bodies);
+
+            if i > 0 {
+                let joint = RevoluteJointBuilder::new()
+                    .local_anchor1(point![0.0, -0.5 * shift])
+                    .local_anchor2(point![0.0, 0.5 * shift])
+                    .contacts_enabled(false);
+                impulse_joints.insert(handles[index - 1], handle, joint, true);
+            }
+
+            if k > 0 {
+                let joint = RevoluteJointBuilder::new()
+                    .local_anchor1(point![0.5 * shift, 0.0])
+                    .local_anchor2(point![-0.5 * shift, 0.0])
+                    .contacts_enabled(false);
+                impulse_joints.insert(handles[index - numi], handle, joint, true);
+            }
+
+            handles[index] = handle;
+            index += 1;
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 20.0);
+}
diff --git a/examples2d/s2d_pyramid.rs b/examples2d/s2d_pyramid.rs
new file mode 100644
index 0000000..c35fe63
--- /dev/null
+++ b/examples2d/s2d_pyramid.rs
@@ -0,0 +1,47 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Ground
+     */
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -1.0]);
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(100.0, 1.0).friction(0.6);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let base_count = 100;
+
+    let h = 0.5;
+    let shift = 1.0 * h;
+
+    for i in 0..base_count {
+        let y = (2.0 * i as f32 + 1.0) * shift;
+
+        for j in i..base_count {
+            let x = (i as f32 + 1.0) * shift + 2.0 * (j as f32 - i as f32) * shift
+                - h * base_count as f32;
+            let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y]);
+            let ground_handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::cuboid(h, h).friction(0.6);
+            colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 20.0);
+}
diff --git a/examples2d/trimesh2.rs b/examples2d/trimesh2.rs
index 2cb8410..a295db5 100644
--- a/examples2d/trimesh2.rs
+++ b/examples2d/trimesh2.rs
@@ -41,7 +41,7 @@ pub fn init_world(testbed: &mut Testbed) {
     colliders.insert_with_parent(collider, handle, &mut bodies);
 
     /*
-     * Create the trimeshes from a tesselated SVG.
+     * Create the trimeshes from a tessellated SVG.
      */
     let mut fill_tess = FillTessellator::new();
     let opt = usvg::Options::default();
@@ -67,7 +67,7 @@ pub fn init_world(testbed: &mut Testbed) {
                         &FillOptions::tolerance(0.01),
                         &mut BuffersBuilder::new(&mut mesh, VertexCtor { prim_id: 0 }),
                     )
-                    .expect("Tesselation failed.");
+                    .expect("Tessellation failed.");
 
                 let angle = transform.get_rotate() as f32;
 
@@ -84,7 +84,8 @@ pub fn init_world(testbed: &mut Testbed) {
                     .collect();
 
                 for k in 0..5 {
-                    let collider = ColliderBuilder::trimesh(vertices.clone(), indices.clone());
+                    let collider = ColliderBuilder::trimesh(vertices.clone(), indices.clone())
+                        .contact_skin(0.2);
                     let rigid_body = RigidBodyBuilder::dynamic()
                         .translation(vector![ith as f32 * 8.0 - 20.0, 20.0 + k as f32 * 11.0])
                         .rotation(angle);
diff --git a/examples3d-f64/debug_serialized3.rs b/examples3d-f64/debug_serialized3.rs
index e2fa747..3801d37 100644
--- a/examples3d-f64/debug_serialized3.rs
+++ b/examples3d-f64/debug_serialized3.rs
@@ -4,7 +4,7 @@ use rapier_testbed3d::Testbed;
 #[derive(serde::Deserialize)]
 struct State {
     pub islands: IslandManager,
-    pub broad_phase: BroadPhase,
+    pub broad_phase: DefaultBroadPhase,
     pub narrow_phase: NarrowPhase,
     pub bodies: RigidBodySet,
     pub colliders: ColliderSet,
diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs
index bac826e..3e6398f 100644
--- a/examples3d/all_examples3.rs
+++ b/examples3d/all_examples3.rs
@@ -23,12 +23,15 @@ mod debug_deserialize3;
 mod debug_dynamic_collider_add3;
 mod debug_friction3;
 mod debug_infinite_fall3;
+mod debug_pop3;
 mod debug_prismatic3;
 mod debug_rollback3;
 mod debug_shape_modification3;
+mod debug_thin_cube_on_mesh3;
 mod debug_triangle3;
 mod debug_trimesh3;
 mod domino3;
+mod dynamic_trimesh3;
 mod fountain3;
 mod heightfield3;
 mod joints3;
@@ -39,6 +42,7 @@ mod debug_cube_high_mass_ratio3;
 mod debug_internal_edges3;
 mod debug_long_chain3;
 mod debug_multibody_ang_motor_pos3;
+mod inverse_kinematics3;
 mod joint_motor_position3;
 mod keva3;
 mod locked_rotations3;
@@ -102,8 +106,10 @@ pub fn main() {
         ("Convex polyhedron", convex_polyhedron3::init_world),
         ("Damping", damping3::init_world),
         ("Domino", domino3::init_world),
+        ("Dynamic trimeshes", dynamic_trimesh3::init_world),
         ("Heightfield", heightfield3::init_world),
         ("Impulse Joints", joints3::init_world_with_joints),
+        ("Inverse kinematics", inverse_kinematics3::init_world),
         ("Joint Motor Position", joint_motor_position3::init_world),
         ("Locked rotations", locked_rotations3::init_world),
         ("One-way platforms", one_way_platforms3::init_world),
@@ -124,6 +130,7 @@ pub fn main() {
         ),
         ("(Debug) big colliders", debug_big_colliders3::init_world),
         ("(Debug) boxes", debug_boxes3::init_world),
+        ("(Debug) pop", debug_pop3::init_world),
         (
             "(Debug) dyn. coll. add",
             debug_dynamic_collider_add3::init_world,
@@ -141,6 +148,7 @@ pub fn main() {
         ),
         ("(Debug) triangle", debug_triangle3::init_world),
         ("(Debug) trimesh", debug_trimesh3::init_world),
+        ("(Debug) thin cube", debug_thin_cube_on_mesh3::init_world),
         ("(Debug) cylinder", debug_cylinder3::init_world),
         ("(Debug) infinite fall", debug_infinite_fall3::init_world),
         ("(Debug) prismatic", debug_prismatic3::init_world),
diff --git a/examples3d/character_controller3.rs b/examples3d/character_controller3.rs
index 939cfd1..f031f0a 100644
--- a/examples3d/character_controller3.rs
+++ b/examples3d/character_controller3.rs
@@ -13,21 +13,37 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Ground
      */
+    let scale = 1.0; // Set to a larger value to check if it still works with larger units.
     let ground_size = 5.0;
     let ground_height = 0.1;
 
-    let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]);
+    let rigid_body =
+        RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0] * scale);
     let floor_handle = bodies.insert(rigid_body);
-    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
+    let collider = ColliderBuilder::cuboid(
+        ground_size * scale,
+        ground_height * scale,
+        ground_size * scale,
+    );
+    colliders.insert_with_parent(collider, floor_handle, &mut bodies);
+
+    let rigid_body =
+        RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, -ground_size] * scale); //.rotation(vector![-0.1, 0.0, 0.0]);
+    let floor_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(
+        ground_size * scale,
+        ground_size * scale,
+        ground_height * scale,
+    );
     colliders.insert_with_parent(collider, floor_handle, &mut bodies);
 
     /*
      * Character we will control manually.
      */
     let rigid_body =
-        RigidBodyBuilder::kinematic_position_based().translation(vector![-3.0, 5.0, 0.0]);
+        RigidBodyBuilder::kinematic_position_based().translation(vector![-3.0, 5.0, 0.0] * scale);
     let character_handle = bodies.insert(rigid_body);
-    let collider = ColliderBuilder::capsule_y(0.3, 0.15); // 0.15, 0.3, 0.15);
+    let collider = ColliderBuilder::capsule_y(0.3 * scale, 0.15 * scale); // 0.15, 0.3, 0.15);
     colliders.insert_with_parent(collider, character_handle, &mut bodies);
 
     /*
@@ -47,9 +63,9 @@ pub fn init_world(testbed: &mut Testbed) {
                 let y = j as f32 * shift + centery;
                 let z = k as f32 * shift + centerx;
 
-                let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y, z]);
+                let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y, z] * scale);
                 let handle = bodies.insert(rigid_body);
-                let collider = ColliderBuilder::cuboid(rad, rad, rad);
+                let collider = ColliderBuilder::cuboid(rad * scale, rad * scale, rad * scale);
                 colliders.insert_with_parent(collider, handle, &mut bodies);
             }
         }
@@ -64,8 +80,12 @@ pub fn init_world(testbed: &mut Testbed) {
         let x = i as f32 * stair_width / 2.0;
         let y = i as f32 * stair_height * 1.5 + 3.0;
 
-        let collider = ColliderBuilder::cuboid(stair_width / 2.0, stair_height / 2.0, stair_width)
-            .translation(vector![x, y, 0.0]);
+        let collider = ColliderBuilder::cuboid(
+            stair_width / 2.0 * scale,
+            stair_height / 2.0 * scale,
+            stair_width * scale,
+        )
+        .translation(vector![x * scale, y * scale, 0.0]);
         colliders.insert(collider);
     }
 
@@ -74,9 +94,13 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let slope_angle = 0.2;
     let slope_size = 2.0;
-    let collider = ColliderBuilder::cuboid(slope_size, ground_height, slope_size)
-        .translation(vector![ground_size + slope_size, -ground_height + 0.4, 0.0])
-        .rotation(Vector::z() * slope_angle);
+    let collider = ColliderBuilder::cuboid(
+        slope_size * scale,
+        ground_height * scale,
+        slope_size * scale,
+    )
+    .translation(vector![ground_size + slope_size, -ground_height + 0.4, 0.0] * scale)
+    .rotation(Vector::z() * slope_angle);
     colliders.insert(collider);
 
     /*
@@ -84,22 +108,29 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let impossible_slope_angle = 0.9;
     let impossible_slope_size = 2.0;
-    let collider = ColliderBuilder::cuboid(slope_size, ground_height, ground_size)
-        .translation(vector![
+    let collider = ColliderBuilder::cuboid(
+        slope_size * scale,
+        ground_height * scale,
+        ground_size * scale,
+    )
+    .translation(
+        vector![
             ground_size + slope_size * 2.0 + impossible_slope_size - 0.9,
             -ground_height + 2.3,
             0.0
-        ])
-        .rotation(Vector::z() * impossible_slope_angle);
+        ] * scale,
+    )
+    .rotation(Vector::z() * impossible_slope_angle);
     colliders.insert(collider);
 
     /*
      * Create a moving platform.
      */
-    let body = RigidBodyBuilder::kinematic_velocity_based().translation(vector![-8.0, 1.5, 0.0]);
+    let body =
+        RigidBodyBuilder::kinematic_velocity_based().translation(vector![-8.0, 1.5, 0.0] * scale);
     // .rotation(-0.3);
     let platform_handle = bodies.insert(body);
-    let collider = ColliderBuilder::cuboid(2.0, ground_height, 2.0);
+    let collider = ColliderBuilder::cuboid(2.0 * scale, ground_height * scale, 2.0 * scale);
     colliders.insert_with_parent(collider, platform_handle, &mut bodies);
 
     /*
@@ -113,18 +144,18 @@ pub fn init_world(testbed: &mut Testbed) {
             + (j as f32 * ground_size.z / (nsubdivs as f32) / 2.0).cos()
     });
 
-    let collider =
-        ColliderBuilder::heightfield(heights, ground_size).translation(vector![-8.0, 5.0, 0.0]);
+    let collider = ColliderBuilder::heightfield(heights, ground_size * scale)
+        .translation(vector![-8.0, 5.0, 0.0] * scale);
     colliders.insert(collider);
 
     /*
      * A tilting dynamic body with a limited joint.
      */
-    let ground = RigidBodyBuilder::fixed().translation(vector![0.0, 5.0, 0.0]);
+    let ground = RigidBodyBuilder::fixed().translation(vector![0.0, 5.0, 0.0] * scale);
     let ground_handle = bodies.insert(ground);
-    let body = RigidBodyBuilder::dynamic().translation(vector![0.0, 5.0, 0.0]);
+    let body = RigidBodyBuilder::dynamic().translation(vector![0.0, 5.0, 0.0] * scale);
     let handle = bodies.insert(body);
-    let collider = ColliderBuilder::cuboid(1.0, 0.1, 2.0);
+    let collider = ColliderBuilder::cuboid(1.0 * scale, 0.1 * scale, 2.0 * scale);
     colliders.insert_with_parent(collider, handle, &mut bodies);
     let joint = RevoluteJointBuilder::new(Vector::z_axis()).limits([-0.3, 0.3]);
     impulse_joints.insert(ground_handle, handle, joint, true);
@@ -137,7 +168,7 @@ pub fn init_world(testbed: &mut Testbed) {
             (run_state.time * 2.0).sin() * 2.0,
             (run_state.time * 5.0).sin() * 1.5,
             0.0
-        ];
+        ] * scale;
         // let angvel = run_state.time.sin() * 0.5;
 
         // Update the velocity-based kinematic body by setting its velocity.
diff --git a/examples3d/convex_decomposition3.rs b/examples3d/convex_decomposition3.rs
index 0863430..eeaa682 100644
--- a/examples3d/convex_decomposition3.rs
+++ b/examples3d/convex_decomposition3.rs
@@ -1,128 +1,5 @@
-use obj::raw::object::Polygon;
-use rapier3d::parry::bounding_volume;
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
-use std::fs::File;
-use std::io::BufReader;
 
-/*
- * NOTE: The `r` macro is only here to convert from f64 to the `N` scalar type.
- * This simplifies experimentation with various scalar types (f32, fixed-point numbers, etc.)
- */
 pub fn init_world(testbed: &mut Testbed) {
-    /*
-     * World
-     */
-    let mut bodies = RigidBodySet::new();
-    let mut colliders = ColliderSet::new();
-    let impulse_joints = ImpulseJointSet::new();
-    let multibody_joints = MultibodyJointSet::new();
-
-    /*
-     * Ground
-     */
-    let ground_size = 50.0;
-    let ground_height = 0.1;
-
-    let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]);
-    let handle = bodies.insert(rigid_body);
-    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
-    colliders.insert_with_parent(collider, handle, &mut bodies);
-
-    /*
-     * Create the convex decompositions.
-     */
-    let geoms = models();
-    let ngeoms = geoms.len();
-    let width = (ngeoms as f32).sqrt() as usize;
-    let num_duplications = 4;
-    let shift = 5.0f32;
-
-    for (igeom, obj_path) in geoms.into_iter().enumerate() {
-        let deltas = Isometry::identity();
-
-        let mut shapes = Vec::new();
-        println!("Parsing and decomposing: {}", obj_path);
-        let input = BufReader::new(File::open(obj_path).unwrap());
-
-        if let Ok(model) = obj::raw::parse_obj(input) {
-            let mut vertices: Vec<_> = model
-                .positions
-                .iter()
-                .map(|v| point![v.0, v.1, v.2])
-                .collect();
-            let indices: Vec<_> = model
-                .polygons
-                .into_iter()
-                .flat_map(|p| match p {
-                    Polygon::P(idx) => idx.into_iter(),
-                    Polygon::PT(idx) => Vec::from_iter(idx.into_iter().map(|i| i.0)).into_iter(),
-                    Polygon::PN(idx) => Vec::from_iter(idx.into_iter().map(|i| i.0)).into_iter(),
-                    Polygon::PTN(idx) => Vec::from_iter(idx.into_iter().map(|i| i.0)).into_iter(),
-                })
-                .collect();
-
-            // Compute the size of the model, to scale it and have similar size for everything.
-            let aabb = bounding_volume::details::point_cloud_aabb(&deltas, &vertices);
-            let center = aabb.center();
-            let diag = (aabb.maxs - aabb.mins).norm();
-
-            vertices
-                .iter_mut()
-                .for_each(|p| *p = (*p - center.coords) * 6.0 / diag);
-
-            let indices: Vec<_> = indices
-                .chunks(3)
-                .map(|idx| [idx[0] as u32, idx[1] as u32, idx[2] as u32])
-                .collect();
-
-            let decomposed_shape = SharedShape::convex_decomposition(&vertices, &indices);
-            shapes.push(decomposed_shape);
-
-            // let compound = SharedShape::compound(compound_parts);
-
-            for k in 1..num_duplications + 1 {
-                let x = (igeom % width) as f32 * shift;
-                let y = (igeom / width) as f32 * shift + 4.0;
-                let z = k as f32 * shift;
-
-                let body = RigidBodyBuilder::dynamic().translation(vector![x, y, z]);
-                let handle = bodies.insert(body);
-
-                for shape in &shapes {
-                    let collider = ColliderBuilder::new(shape.clone());
-                    colliders.insert_with_parent(collider, handle, &mut bodies);
-                }
-            }
-        }
-    }
-
-    /*
-     * Set up the testbed.
-     */
-    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
-    testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
-}
-
-fn models() -> Vec<String> {
-    vec![
-        "assets/3d/camel_decimated.obj".to_string(),
-        "assets/3d/chair.obj".to_string(),
-        "assets/3d/cup_decimated.obj".to_string(),
-        "assets/3d/dilo_decimated.obj".to_string(),
-        "assets/3d/feline_decimated.obj".to_string(),
-        "assets/3d/genus3_decimated.obj".to_string(),
-        "assets/3d/hand2_decimated.obj".to_string(),
-        "assets/3d/hand_decimated.obj".to_string(),
-        "assets/3d/hornbug.obj".to_string(),
-        "assets/3d/octopus_decimated.obj".to_string(),
-        "assets/3d/rabbit_decimated.obj".to_string(),
-        // "assets/3d/rust_logo.obj".to_string(),
-        "assets/3d/rust_logo_simplified.obj".to_string(),
-        "assets/3d/screwdriver_decimated.obj".to_string(),
-        "assets/3d/table.obj".to_string(),
-        "assets/3d/tstTorusModel.obj".to_string(),
-        // "assets/3d/tstTorusModel2.obj".to_string(),
-        // "assets/3d/tstTorusModel3.obj".to_string(),
-    ]
+    crate::dynamic_trimesh3::do_init_world(testbed, true);
 }
diff --git a/examples3d/debug_deserialize3.rs b/examples3d/debug_deserialize3.rs
index b8b79fb..0762b4d 100644
--- a/examples3d/debug_deserialize3.rs
+++ b/examples3d/debug_deserialize3.rs
@@ -6,7 +6,7 @@ struct PhysicsState {
     pub gravity: Vector<f32>,
     pub integration_parameters: IntegrationParameters,
     pub islands: IslandManager,
-    pub broad_phase: BroadPhase,
+    pub broad_phase: DefaultBroadPhase,
     pub narrow_phase: NarrowPhase,
     pub bodies: RigidBodySet,
     pub colliders: ColliderSet,
diff --git a/examples3d/debug_internal_edges3.rs b/examples3d/debug_internal_edges3.rs
index 7112fb0..86de1bd 100644
--- a/examples3d/debug_internal_edges3.rs
+++ b/examples3d/debug_internal_edges3.rs
@@ -11,7 +11,8 @@ pub fn init_world(testbed: &mut Testbed) {
     let multibody_joints = MultibodyJointSet::new();
 
     let heights = DMatrix::zeros(100, 100);
-    let heightfield = HeightField::new(heights, vector![60.0, 1.0, 60.0]);
+    let heightfield =
+        HeightField::with_flags(heights, vector![60.0, 1.0, 60.0], HeightFieldFlags::all());
     let rotation = vector![0.0, 0.0, 0.0]; // vector![-0.1, 0.0, 0.0];
     colliders
         .insert(ColliderBuilder::new(SharedShape::new(heightfield.clone())).rotation(rotation));
diff --git a/examples3d/debug_pop3.rs b/examples3d/debug_pop3.rs
new file mode 100644
index 0000000..93d58e0
--- /dev/null
+++ b/examples3d/debug_pop3.rs
@@ -0,0 +1,42 @@
+use rapier3d::prelude::*;
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 10.0;
+    let ground_height = 10.0;
+
+    for _ in 0..1 {
+        let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+    }
+
+    // Build the dynamic box rigid body.
+    for _ in 0..1 {
+        let rigid_body = RigidBodyBuilder::dynamic()
+            // .translation(vector![0.0, 0.1, 0.0])
+            // .rotation(vector![0.8, 0.2, 0.1])
+            .can_sleep(false);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(1.0, 1.0, 1.0);
+        colliders.insert_with_parent(collider.clone(), handle, &mut bodies);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![10.0, 10.0, 10.0], Point::origin());
+}
diff --git a/examples3d/debug_thin_cube_on_mesh3.rs b/examples3d/debug_thin_cube_on_mesh3.rs
new file mode 100644
index 0000000..4ad557a
--- /dev/null
+++ b/examples3d/debug_thin_cube_on_mesh3.rs
@@ -0,0 +1,56 @@
+use rapier3d::prelude::*;
+use rapier_testbed3d::Testbed;
+
+// This shows a bug when a cylinder is in contact with a very large
+// but very thin cuboid. In this case the EPA returns an incorrect
+// contact normal, resulting in the cylinder falling through the floor.
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Ground
+     */
+    // let vertices = vec![
+    //     point![-50.0, 0.0, -50.0],
+    //     point![-50.0, 0.0, 50.0],
+    //     point![50.0, 0.0, 50.0],
+    //     point![50.0, 0.0, -50.0],
+    // ];
+    // let indices = vec![[0, 1, 2], [0, 2, 3]];
+    //
+    // let collider = ColliderBuilder::trimesh_with_flags(vertices, indices, TriMeshFlags::all());
+    // colliders.insert(collider);
+
+    let heights = DMatrix::repeat(2, 2, 0.0);
+    let collider = ColliderBuilder::heightfield_with_flags(
+        heights,
+        Vector::new(50.0, 1.0, 50.0),
+        HeightFieldFlags::FIX_INTERNAL_EDGES,
+    );
+    colliders.insert(collider);
+
+    /*
+     * Create the cubes
+     */
+    // Build the rigid body.
+    let rigid_body = RigidBodyBuilder::dynamic()
+        .translation(vector![0.0, 5.0, 0.0])
+        .rotation(vector![0.5, 0.0, 0.5])
+        .linvel(vector![0.0, -100.0, 0.0])
+        .soft_ccd_prediction(10.0);
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(5.0, 0.015, 5.0);
+    colliders.insert_with_parent(collider, handle, &mut bodies);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
+}
diff --git a/examples3d/dynamic_trimesh3.rs b/examples3d/dynamic_trimesh3.rs
new file mode 100644
index 0000000..0a82476
--- /dev/null
+++ b/examples3d/dynamic_trimesh3.rs
@@ -0,0 +1,145 @@
+use obj::raw::object::Polygon;
+use rapier3d::parry::bounding_volume;
+use rapier3d::prelude::*;
+use rapier_testbed3d::Testbed;
+use std::fs::File;
+use std::io::BufReader;
+
+pub fn init_world(testbed: &mut Testbed) {
+    do_init_world(testbed, false);
+}
+
+pub fn do_init_world(testbed: &mut Testbed, use_convex_decomposition: bool) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Ground
+     */
+    //// OPTION 1: floor made of a single big box.
+    // let ground_size = 50.0;
+    // let ground_height = 0.1;
+    // let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]);
+    // let handle = bodies.insert(rigid_body);
+    // let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
+    // colliders.insert_with_parent(collider, handle, &mut bodies);
+
+    //// OPTION 2: floor made of a wavy mesh.
+    let nsubdivs = 100;
+    let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| {
+        -(i as f32 * 40.0 / (nsubdivs as f32) / 2.0).cos()
+            - (j as f32 * 40.0 / (nsubdivs as f32) / 2.0).cos()
+    });
+    let heightfield = HeightField::new(heights, vector![100.0, 2.0, 100.0]);
+    let mut trimesh = TriMesh::from(heightfield);
+    let _ = trimesh.set_flags(TriMeshFlags::FIX_INTERNAL_EDGES);
+    colliders.insert(ColliderBuilder::new(SharedShape::new(trimesh.clone())));
+
+    /*
+     * Create the convex decompositions.
+     */
+    let geoms = models();
+    let ngeoms = geoms.len();
+    let width = (ngeoms as f32).sqrt() as usize;
+    let num_duplications = 4;
+    let shift_y = 8.0f32;
+    let shift_xz = 9.0f32;
+
+    for (igeom, obj_path) in geoms.into_iter().enumerate() {
+        let deltas = Isometry::identity();
+
+        let mut shapes = Vec::new();
+        println!("Parsing and decomposing: {}", obj_path);
+        let input = BufReader::new(File::open(obj_path).unwrap());
+
+        if let Ok(model) = obj::raw::parse_obj(input) {
+            let mut vertices: Vec<_> = model
+                .positions
+                .iter()
+                .map(|v| point![v.0, v.1, v.2])
+                .collect();
+            let indices: Vec<_> = model
+                .polygons
+                .into_iter()
+                .flat_map(|p| match p {
+                    Polygon::P(idx) => idx.into_iter(),
+                    Polygon::PT(idx) => Vec::from_iter(idx.into_iter().map(|i| i.0)).into_iter(),
+                    Polygon::PN(idx) => Vec::from_iter(idx.into_iter().map(|i| i.0)).into_iter(),
+                    Polygon::PTN(idx) => Vec::from_iter(idx.into_iter().map(|i| i.0)).into_iter(),
+                })
+                .collect();
+
+            // Compute the size of the model, to scale it and have similar size for everything.
+            let aabb = bounding_volume::details::point_cloud_aabb(&deltas, &vertices);
+            let center = aabb.center();
+            let diag = (aabb.maxs - aabb.mins).norm();
+
+            vertices
+                .iter_mut()
+                .for_each(|p| *p = (*p - center.coords) * 10.0 / diag);
+
+            let indices: Vec<_> = indices
+                .chunks(3)
+                .map(|idx| [idx[0] as u32, idx[1] as u32, idx[2] as u32])
+                .collect();
+
+            let decomposed_shape = if use_convex_decomposition {
+                SharedShape::convex_decomposition(&vertices, &indices)
+            } else {
+                // SharedShape::trimesh(vertices, indices)
+                SharedShape::trimesh_with_flags(vertices, indices, TriMeshFlags::FIX_INTERNAL_EDGES)
+            };
+            shapes.push(decomposed_shape);
+
+            // let compound = SharedShape::compound(compound_parts);
+
+            for k in 1..num_duplications + 1 {
+                let x =
+                    (igeom % width) as f32 * shift_xz - num_duplications as f32 * shift_xz / 2.0;
+                let y = (igeom / width) as f32 * shift_y + 7.0;
+                let z = k as f32 * shift_xz - num_duplications as f32 * shift_xz / 2.0;
+
+                let body = RigidBodyBuilder::dynamic().translation(vector![x, y, z]);
+                let handle = bodies.insert(body);
+
+                for shape in &shapes {
+                    let collider = ColliderBuilder::new(shape.clone()).contact_skin(0.1);
+                    colliders.insert_with_parent(collider, handle, &mut bodies);
+                }
+            }
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
+}
+
+fn models() -> Vec<String> {
+    vec![
+        "assets/3d/camel_decimated.obj".to_string(),
+        "assets/3d/chair.obj".to_string(),
+        "assets/3d/cup_decimated.obj".to_string(),
+        "assets/3d/dilo_decimated.obj".to_string(),
+        "assets/3d/tstTorusModel2.obj".to_string(),
+        "assets/3d/feline_decimated.obj".to_string(),
+        "assets/3d/genus3_decimated.obj".to_string(),
+        // "assets/3d/hand2_decimated.obj".to_string(),
+        // "assets/3d/hand_decimated.obj".to_string(),
+        "assets/3d/hornbug.obj".to_string(),
+        "assets/3d/tstTorusModel.obj".to_string(),
+        "assets/3d/octopus_decimated.obj".to_string(),
+        "assets/3d/rabbit_decimated.obj".to_string(),
+        "assets/3d/rust_logo_simplified.obj".to_string(),
+        "assets/3d/screwdriver_decimated.obj".to_string(),
+        "assets/3d/table.obj".to_string(),
+        "assets/3d/tstTorusModel3.obj".to_string(),
+    ]
+}
diff --git a/examples3d/inverse_kinematics3.rs b/examples3d/inverse_kinematics3.rs
new file mode 100644
index 0000000..7905ae0
--- /dev/null
+++ b/examples3d/inverse_kinematics3.rs
@@ -0,0 +1,103 @@
+use rapier3d::prelude::*;
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let mut multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 0.2;
+    let ground_height = 0.01;
+
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]);
+    let floor_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
+    colliders.insert_with_parent(collider, floor_handle, &mut bodies);
+
+    /*
+     * Setup groups
+     */
+    let num_segments = 10;
+    let body = RigidBodyBuilder::fixed();
+    let mut last_body = bodies.insert(body);
+    let mut last_link = MultibodyJointHandle::invalid();
+
+    for i in 0..num_segments {
+        let size = 1.0 / num_segments as f32;
+        let body = RigidBodyBuilder::dynamic().can_sleep(false);
+        let new_body = bodies.insert(body);
+        // NOTE: we add a sensor collider just to make the destbed draw a rectangle to make
+        //       the demo look nicer. IK could be used without cuboid.
+        let collider = ColliderBuilder::cuboid(size / 8.0, size / 2.0, size / 8.0)
+            .density(0.0)
+            .sensor(true);
+        colliders.insert_with_parent(collider, new_body, &mut bodies);
+
+        let link_ab = SphericalJointBuilder::new()
+            .local_anchor1(point![0.0, size / 2.0 * (i != 0) as usize as f32, 0.0])
+            .local_anchor2(point![0.0, -size / 2.0, 0.0])
+            .build()
+            .data;
+
+        last_link = multibody_joints
+            .insert(last_body, new_body, link_ab, true)
+            .unwrap();
+
+        last_body = new_body;
+    }
+
+    let mut displacements = DVector::zeros(0);
+
+    testbed.add_callback(move |graphics, physics, _, _| {
+        let Some(graphics) = graphics else { return };
+        if let Some((multibody, link_id)) = physics.multibody_joints.get_mut(last_link) {
+            // Ensure our displacement vector has the right number of elements.
+            if displacements.nrows() < multibody.ndofs() {
+                displacements = DVector::zeros(multibody.ndofs());
+            } else {
+                displacements.fill(0.0);
+            }
+
+            let Some(mouse_ray) = graphics.mouse().ray else {
+                return;
+            };
+
+            // Cast a ray on a plane aligned with the camera passing through the origin.
+            let halfspace = HalfSpace {
+                normal: -UnitVector::new_normalize(graphics.camera_fwd_dir()),
+            };
+            let mouse_ray = Ray::new(mouse_ray.0, mouse_ray.1);
+            let Some(hit) = halfspace.cast_local_ray(&mouse_ray, f32::MAX, false) else {
+                return;
+            };
+            let target_point = mouse_ray.point_at(hit);
+
+            let options = InverseKinematicsOption {
+                constrained_axes: JointAxesMask::LIN_AXES,
+                ..Default::default()
+            };
+
+            multibody.inverse_kinematics(
+                &physics.bodies,
+                link_id,
+                &options,
+                &Isometry::from(target_point),
+                &mut displacements,
+            );
+            multibody.apply_displacements(displacements.as_slice());
+        }
+    });
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 0.5, 2.5], point![0.0, 0.5, 0.0]);
+}
diff --git a/examples3d/vehicle_joints3.rs b/examples3d/vehicle_joints3.rs
index ddd672c..5eba608 100644
--- a/examples3d/vehicle_joints3.rs
+++ b/examples3d/vehicle_joints3.rs
@@ -138,16 +138,16 @@ pub fn init_world(testbed: &mut Testbed) {
 
         for key in gfx.keys().get_pressed() {
             match *key {
-                KeyCode::Right => {
+                KeyCode::ArrowRight => {
                     steering = -1.0;
                 }
-                KeyCode::Left => {
+                KeyCode::ArrowLeft => {
                     steering = 1.0;
                 }
-                KeyCode::Up => {
+                KeyCode::ArrowUp => {
                     thrust = -drive_strength;
                 }
-                KeyCode::Down => {
+                KeyCode::ArrowDown => {
                     thrust = drive_strength;
                 }
                 KeyCode::ShiftRight => {
diff --git a/src/control/character_controller.rs b/src/control/character_controller.rs
index ca088b2..87b5c16 100644
--- a/src/control/character_controller.rs
+++ b/src/control/character_controller.rs
@@ -1,11 +1,12 @@
 use crate::dynamics::RigidBodySet;
-use crate::geometry::{ColliderHandle, ColliderSet, ContactManifold, Shape, TOI};
+use crate::geometry::{ColliderHandle, ColliderSet, ContactManifold, Shape, ShapeCastHit};
 use crate::math::{Isometry, Point, Real, UnitVector, Vector};
 use crate::pipeline::{QueryFilter, QueryFilterFlags, QueryPipeline};
 use crate::utils;
 use na::{RealField, Vector2};
 use parry::bounding_volume::BoundingVolume;
 use parry::math::Translation;
+use parry::query::details::ShapeCastOptions;
 use parry::query::{DefaultQueryDispatcher, PersistentQueryDispatcher};
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -15,13 +16,13 @@ pub enum CharacterLength {
     /// The length is specified relative to some of the character shape’s size.
     ///
     /// For example setting `CharacterAutostep::max_height` to `CharacterLength::Relative(0.1)`
-    /// for a shape with an height equal to 20.0 will result in a maximum step height
+    /// for a shape with a height equal to 20.0 will result in a maximum step height
     /// of `0.1 * 20.0 = 2.0`.
     Relative(Real),
-    /// The length is specified as an aboslute value, independent from the character shape’s size.
+    /// The length is specified as an absolute value, independent from the character shape’s size.
     ///
     /// For example setting `CharacterAutostep::max_height` to `CharacterLength::Relative(0.1)`
-    /// for a shape with an height equal to 20.0 will result in a maximum step height
+    /// for a shape with a height equal to 20.0 will result in a maximum step height
     /// of `0.1` (the shape height is ignored in for this value).
     Absolute(Real),
 }
@@ -55,6 +56,13 @@ impl CharacterLength {
     }
 }
 
+#[derive(Debug)]
+struct HitInfo {
+    toi: ShapeCastHit,
+    is_wall: bool,
+    is_nonslip_slope: bool,
+}
+
 /// Configuration for the auto-stepping character controller feature.
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq)]
@@ -77,6 +85,21 @@ impl Default for CharacterAutostep {
     }
 }
 
+#[derive(Debug)]
+struct HitDecomposition {
+    normal_part: Vector<Real>,
+    horizontal_tangent: Vector<Real>,
+    vertical_tangent: Vector<Real>,
+    // NOTE: we don’t store the penetration part since we don’t really need it
+    //       for anything.
+}
+
+impl HitDecomposition {
+    pub fn unconstrained_slide_part(&self) -> Vector<Real> {
+        self.normal_part + self.horizontal_tangent + self.vertical_tangent
+    }
+}
+
 /// A collision between the character and its environment during its movement.
 #[derive(Copy, Clone, Debug)]
 pub struct CharacterCollision {
@@ -89,7 +112,7 @@ pub struct CharacterCollision {
     /// The translations that was still waiting to be applied to the character when the hit happens.
     pub translation_remaining: Vector<Real>,
     /// Geometric information about the hit.
-    pub toi: TOI,
+    pub hit: ShapeCastHit,
 }
 
 /// A character controller for kinematic bodies.
@@ -105,7 +128,10 @@ pub struct KinematicCharacterController {
     pub offset: CharacterLength,
     /// Should the character try to slide against the floor if it hits it?
     pub slide: bool,
-    /// Should the character automatically step over small obstacles?
+    /// Should the character automatically step over small obstacles? (disabled by default)
+    ///
+    /// Note that autostepping is currently a very computationally expensive feature, so it
+    /// is disabled by default.
     pub autostep: Option<CharacterAutostep>,
     /// The maximum angle (radians) between the floor’s normal and the `up` vector that the
     /// character is able to climb.
@@ -116,6 +142,15 @@ pub struct KinematicCharacterController {
     /// Should the character be automatically snapped to the ground if the distance between
     /// the ground and its feed are smaller than the specified threshold?
     pub snap_to_ground: Option<CharacterLength>,
+    /// Increase this number if your character appears to get stuck when sliding against surfaces.
+    ///
+    /// This is a small distance applied to the movement toward the contact normals of shapes hit
+    /// by the character controller. This helps shape-casting not getting stuck in an always-penetrating
+    /// state during the sliding calculation.
+    ///
+    /// This value should remain fairly small since it can introduce artificial "bumps" when sliding
+    /// along a flat surface.
+    pub normal_nudge_factor: Real,
 }
 
 impl Default for KinematicCharacterController {
@@ -124,10 +159,11 @@ impl Default for KinematicCharacterController {
             up: Vector::y_axis(),
             offset: CharacterLength::Relative(0.01),
             slide: true,
-            autostep: Some(CharacterAutostep::default()),
+            autostep: None,
             max_slope_climb_angle: Real::frac_pi_4(),
             min_slope_slide_angle: Real::frac_pi_4(),
             snap_to_ground: Some(CharacterLength::Relative(0.2)),
+            normal_nudge_factor: 1.0e-4,
         }
     }
 }
@@ -225,19 +261,22 @@ impl KinematicCharacterController {
             }
 
             // 2. Cast towards the movement direction.
-            if let Some((handle, toi)) = queries.cast_shape(
+            if let Some((handle, hit)) = queries.cast_shape(
                 bodies,
                 colliders,
                 &(Translation::from(result.translation) * character_pos),
                 &translation_dir,
                 character_shape,
-                translation_dist + offset,
-                false,
+                ShapeCastOptions {
+                    target_distance: offset,
+                    stop_at_penetration: false,
+                    max_time_of_impact: translation_dist,
+                    compute_impact_geometry_on_penetration: true,
+                },
                 filter,
             ) {
-                // We hit something, compute the allowed self.
-                let allowed_dist =
-                    (toi.toi - (-toi.normal1.dot(&translation_dir)) * offset).max(0.0);
+                // We hit something, compute and apply the allowed interference-free translation.
+                let allowed_dist = hit.time_of_impact;
                 let allowed_translation = *translation_dir * allowed_dist;
                 result.translation += allowed_translation;
                 translation_remaining -= allowed_translation;
@@ -247,10 +286,12 @@ impl KinematicCharacterController {
                     character_pos: Translation::from(result.translation) * character_pos,
                     translation_applied: result.translation,
                     translation_remaining,
-                    toi,
+                    hit,
                 });
 
-                // Try to go up stairs.
+                let hit_info = self.compute_hit_info(hit);
+
+                // Try to go upstairs.
                 if !self.handle_stairs(
                     bodies,
                     colliders,
@@ -260,12 +301,18 @@ impl KinematicCharacterController {
                     &dims,
                     filter,
                     handle,
+                    &hit_info,
                     &mut translation_remaining,
                     &mut result,
                 ) {
                     // No stairs, try to move along slopes.
-                    translation_remaining =
-                        self.handle_slopes(&toi, &translation_remaining, &mut result);
+                    translation_remaining = self.handle_slopes(
+                        &hit_info,
+                        &desired_translation,
+                        &translation_remaining,
+                        self.normal_nudge_factor,
+                        &mut result,
+                    );
                 }
             } else {
                 // No interference along the path.
@@ -319,7 +366,7 @@ impl KinematicCharacterController {
         dims: &Vector2<Real>,
         filter: QueryFilter,
         result: &mut EffectiveCharacterMovement,
-    ) -> Option<(ColliderHandle, TOI)> {
+    ) -> Option<(ColliderHandle, ShapeCastHit)> {
         if let Some(snap_distance) = self.snap_to_ground {
             if result.translation.dot(&self.up) < -1.0e-5 {
                 let snap_distance = snap_distance.eval(dims.y);
@@ -330,12 +377,16 @@ impl KinematicCharacterController {
                     character_pos,
                     &-self.up,
                     character_shape,
-                    snap_distance + offset,
-                    false,
+                    ShapeCastOptions {
+                        target_distance: offset,
+                        stop_at_penetration: false,
+                        max_time_of_impact: snap_distance,
+                        compute_impact_geometry_on_penetration: true,
+                    },
                     filter,
                 ) {
                     // Apply the snap.
-                    result.translation -= *self.up * (hit.toi - offset).max(0.0);
+                    result.translation -= *self.up * hit.time_of_impact;
                     result.grounded = true;
                     return Some((hit_handle, hit));
                 }
@@ -481,36 +532,40 @@ impl KinematicCharacterController {
 
     fn handle_slopes(
         &self,
-        hit: &TOI,
+        hit: &HitInfo,
+        movement_input: &Vector<Real>,
         translation_remaining: &Vector<Real>,
+        normal_nudge_factor: Real,
         result: &mut EffectiveCharacterMovement,
     ) -> Vector<Real> {
-        let [vertical_translation, horizontal_translation] =
-            self.split_into_components(translation_remaining);
-        let slope_translation = subtract_hit(*translation_remaining, hit);
+        let [_vertical_input, horizontal_input] = self.split_into_components(movement_input);
+        let horiz_input_decomp = self.decompose_hit(&horizontal_input, &hit.toi);
+        let input_decomp = self.decompose_hit(movement_input, &hit.toi);
 
-        // Check if there is a slope to climb.
-        let angle_with_floor = self.up.angle(&hit.normal1);
+        let decomp = self.decompose_hit(translation_remaining, &hit.toi);
 
-        // We are climbing if the movement along the slope goes upward, and the angle with the
-        // floor is smaller than pi/2 (in which case we hit some some sort of ceiling).
-        //
-        // NOTE: part of the slope will already be handled by auto-stepping if it was enabled.
-        //       Therefore, `climbing` may not always be `true` when climbing on a slope at
-        //       slow speed.
-        let climbing = self.up.dot(&slope_translation) >= 0.0 && self.up.dot(&hit.normal1) > 0.0;
+        // An object is trying to slip if the tangential movement induced by its vertical movement
+        // points downward.
+        let slipping_intent = self.up.dot(&horiz_input_decomp.vertical_tangent) < 0.0;
+        let slipping = self.up.dot(&decomp.vertical_tangent) < 0.0;
 
-        if climbing && angle_with_floor >= self.max_slope_climb_angle {
-            // Prevent horizontal movement from pushing through the slope.
-            vertical_translation
-        } else if !climbing && angle_with_floor <= self.min_slope_slide_angle {
+        // An object is trying to climb if its indirect vertical motion points upward.
+        let climbing_intent = self.up.dot(&input_decomp.vertical_tangent) > 0.0;
+        let climbing = self.up.dot(&decomp.vertical_tangent) > 0.0;
+
+        let allowed_movement = if hit.is_wall && climbing && !climbing_intent {
+            // Can’t climb the slope, remove the vertical tangent motion induced by the forward motion.
+            decomp.horizontal_tangent + decomp.normal_part
+        } else if hit.is_nonslip_slope && slipping && !slipping_intent {
             // Prevent the vertical movement from sliding down.
-            horizontal_translation
+            decomp.horizontal_tangent + decomp.normal_part
         } else {
-            // Let it slide
+            // Let it slide (including climbing the slope).
             result.is_sliding_down_slope = true;
-            slope_translation
-        }
+            decomp.unconstrained_slide_part()
+        };
+
+        allowed_movement + *hit.toi.normal1 * normal_nudge_factor
     }
 
     fn split_into_components(&self, translation: &Vector<Real>) -> [Vector<Real>; 2] {
@@ -519,6 +574,51 @@ impl KinematicCharacterController {
         [vertical_translation, horizontal_translation]
     }
 
+    fn compute_hit_info(&self, toi: ShapeCastHit) -> HitInfo {
+        let angle_with_floor = self.up.angle(&toi.normal1);
+        let is_ceiling = self.up.dot(&toi.normal1) < 0.0;
+        let is_wall = angle_with_floor >= self.max_slope_climb_angle && !is_ceiling;
+        let is_nonslip_slope = angle_with_floor <= self.min_slope_slide_angle;
+
+        HitInfo {
+            toi,
+            is_wall,
+            is_nonslip_slope,
+        }
+    }
+
+    fn decompose_hit(&self, translation: &Vector<Real>, hit: &ShapeCastHit) -> HitDecomposition {
+        let dist_to_surface = translation.dot(&hit.normal1);
+        let normal_part;
+        let penetration_part;
+
+        if dist_to_surface < 0.0 {
+            normal_part = Vector::zeros();
+            penetration_part = dist_to_surface * *hit.normal1;
+        } else {
+            penetration_part = Vector::zeros();
+            normal_part = dist_to_surface * *hit.normal1;
+        }
+
+        let tangent = translation - normal_part - penetration_part;
+        #[cfg(feature = "dim3")]
+        let horizontal_tangent_dir = hit.normal1.cross(&self.up);
+        #[cfg(feature = "dim2")]
+        let horizontal_tangent_dir = Vector::zeros();
+
+        let horizontal_tangent_dir = horizontal_tangent_dir
+            .try_normalize(1.0e-5)
+            .unwrap_or_default();
+        let horizontal_tangent = tangent.dot(&horizontal_tangent_dir) * horizontal_tangent_dir;
+        let vertical_tangent = tangent - horizontal_tangent;
+
+        HitDecomposition {
+            normal_part,
+            horizontal_tangent,
+            vertical_tangent,
+        }
+    }
+
     fn compute_dims(&self, character_shape: &dyn Shape) -> Vector2<Real> {
         let extents = character_shape.compute_local_aabb().extents();
         let up_extent = extents.dot(&self.up.abs());
@@ -536,14 +636,19 @@ impl KinematicCharacterController {
         dims: &Vector2<Real>,
         mut filter: QueryFilter,
         stair_handle: ColliderHandle,
+        hit: &HitInfo,
         translation_remaining: &mut Vector<Real>,
         result: &mut EffectiveCharacterMovement,
     ) -> bool {
-        let autostep = match self.autostep {
-            Some(autostep) => autostep,
-            None => return false,
+        let Some(autostep) = self.autostep else {
+            return false;
         };
 
+        // Only try to autostep on walls.
+        if !hit.is_wall {
+            return false;
+        }
+
         let offset = self.offset.eval(dims.y);
         let min_width = autostep.min_width.eval(dims.x) + offset;
         let max_height = autostep.max_height.eval(dims.y) + offset;
@@ -565,12 +670,10 @@ impl KinematicCharacterController {
 
         let shifted_character_pos = Translation::from(*self.up * max_height) * character_pos;
 
-        let horizontal_dir = match (*translation_remaining
+        let Some(horizontal_dir) = (*translation_remaining
             - *self.up * translation_remaining.dot(&self.up))
-        .try_normalize(1.0e-5)
-        {
-            Some(dir) => dir,
-            None => return false,
+        .try_normalize(1.0e-5) else {
+            return false;
         };
 
         if queries
@@ -580,8 +683,12 @@ impl KinematicCharacterController {
                 character_pos,
                 &self.up,
                 character_shape,
-                max_height,
-                false,
+                ShapeCastOptions {
+                    target_distance: offset,
+                    stop_at_penetration: false,
+                    max_time_of_impact: max_height,
+                    compute_impact_geometry_on_penetration: true,
+                },
                 filter,
             )
             .is_some()
@@ -597,8 +704,12 @@ impl KinematicCharacterController {
                 &shifted_character_pos,
                 &horizontal_dir,
                 character_shape,
-                min_width,
-                false,
+                ShapeCastOptions {
+                    target_distance: offset,
+                    stop_at_penetration: false,
+                    max_time_of_impact: min_width,
+                    compute_impact_geometry_on_penetration: true,
+                },
                 filter,
             )
             .is_some()
@@ -615,8 +726,12 @@ impl KinematicCharacterController {
             &(Translation::from(horizontal_dir * min_width) * shifted_character_pos),
             &-self.up,
             character_shape,
-            max_height,
-            false,
+            ShapeCastOptions {
+                target_distance: offset,
+                stop_at_penetration: false,
+                max_time_of_impact: max_height,
+                compute_impact_geometry_on_penetration: true,
+            },
             filter,
         ) {
             let [vertical_slope_translation, horizontal_slope_translation] = self
@@ -642,11 +757,15 @@ impl KinematicCharacterController {
                     &(Translation::from(horizontal_dir * min_width) * shifted_character_pos),
                     &-self.up,
                     character_shape,
-                    max_height,
-                    false,
+                    ShapeCastOptions {
+                        target_distance: offset,
+                        stop_at_penetration: false,
+                        max_time_of_impact: max_height,
+                        compute_impact_geometry_on_penetration: true,
+                    },
                     filter,
                 )
-                .map(|hit| hit.1.toi)
+                .map(|hit| hit.1.time_of_impact)
                 .unwrap_or(max_height);
 
         // Remove the step height from the vertical part of the self.
@@ -681,7 +800,7 @@ impl KinematicCharacterController {
         let extents = character_shape.compute_local_aabb().extents();
         let up_extent = extents.dot(&self.up.abs());
         let movement_to_transfer =
-            *collision.toi.normal1 * collision.translation_remaining.dot(&collision.toi.normal1);
+            *collision.hit.normal1 * collision.translation_remaining.dot(&collision.hit.normal1);
         let prediction = self.predict_ground(up_extent);
 
         // TODO: allow custom dispatchers.
@@ -748,7 +867,7 @@ impl KinematicCharacterController {
     }
 }
 
-fn subtract_hit(translation: Vector<Real>, hit: &TOI) -> Vector<Real> {
+fn subtract_hit(translation: Vector<Real>, hit: &ShapeCastHit) -> Vector<Real> {
     let surface_correction = (-translation).dot(&hit.normal1).max(0.0);
     // This fixes some instances of moving through walls
     let surface_correction = surface_correction * (1.0 + 1.0e-5);
diff --git a/src/control/ray_cast_vehicle_controller.rs b/src/control/ray_cast_vehicle_controller.rs
index 00b11eb..98b6339 100644
--- a/src/control/ray_cast_vehicle_controller.rs
+++ b/src/control/ray_cast_vehicle_controller.rs
@@ -341,7 +341,7 @@ impl DynamicRayCastVehicleController {
         wheel.raycast_info.ground_object = None;
 
         if let Some((collider_hit, mut hit)) = hit {
-            if hit.toi == 0.0 {
+            if hit.time_of_impact == 0.0 {
                 let collider = &colliders[collider_hit];
                 let up_ray = Ray::new(source + rayvector, -rayvector);
                 if let Some(hit2) =
@@ -362,7 +362,7 @@ impl DynamicRayCastVehicleController {
             wheel.raycast_info.is_in_contact = true;
             wheel.raycast_info.ground_object = Some(collider_hit);
 
-            let hit_distance = hit.toi * raylen;
+            let hit_distance = hit.time_of_impact * raylen;
             wheel.raycast_info.suspension_length = hit_distance - wheel.radius;
 
             // clamp on max suspension travel
@@ -372,7 +372,7 @@ impl DynamicRayCastVehicleController {
                 .raycast_info
                 .suspension_length
                 .clamp(min_suspension_length, max_suspension_length);
-            wheel.raycast_info.contact_point_ws = ray.point_at(hit.toi);
+            wheel.raycast_info.contact_point_ws = ray.point_at(hit.time_of_impact);
 
             let denominator = wheel
                 .raycast_info
diff --git a/src/counters/mod.rs b/src/counters/mod.rs
index 324696f..99a627a 100644
--- a/src/counters/mod.rs
+++ b/src/counters/mod.rs
@@ -17,7 +17,7 @@ mod timer;
 /// Aggregation of all the performances counters tracked by rapier.
 #[derive(Clone, Copy)]
 pub struct Counters {
-    /// Whether thi counter is enabled or not.
+    /// Whether this counter is enabled or not.
     pub enabled: bool,
     /// Timer for a whole timestep.
     pub step_time: Timer,
@@ -69,7 +69,7 @@ impl Counters {
         }
     }
 
-    /// Notfy that the time-step has finished.
+    /// Notify that the time-step has finished.
     pub fn step_completed(&mut self) {
         if self.enabled {
             self.step_time.pause();
@@ -88,7 +88,7 @@ impl Counters {
         }
     }
 
-    /// Notfy that the custom operation has finished.
+    /// Notify that the custom operation has finished.
     pub fn custom_completed(&mut self) {
         if self.enabled {
             self.custom.pause();
@@ -182,6 +182,12 @@ measure_method!(
     stages.solver_time
 );
 measure_method!(ccd_started, ccd_completed, ccd_time, stages.ccd_time);
+measure_method!(
+    query_pipeline_update_started,
+    query_pipeline_update_completed,
+    query_pipeline_update_time,
+    stages.query_pipeline_time
+);
 
 measure_method!(
     assembly_started,
@@ -201,12 +207,6 @@ measure_method!(
     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,
diff --git a/src/counters/solver_counters.rs b/src/counters/solver_counters.rs
index babcf41..aced5f3 100644
--- a/src/counters/solver_counters.rs
+++ b/src/counters/solver_counters.rs
@@ -14,10 +14,8 @@ pub struct SolverCounters {
     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,
+    /// Time spent to write force back to user-accessible data.
+    pub velocity_writeback_time: Timer,
 }
 
 impl SolverCounters {
@@ -29,8 +27,7 @@ impl SolverCounters {
             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(),
+            velocity_writeback_time: Timer::new(),
         }
     }
 
@@ -41,8 +38,7 @@ impl SolverCounters {
         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();
+        self.velocity_writeback_time.reset();
     }
 }
 
@@ -57,11 +53,10 @@ impl Display for SolverCounters {
             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
+            "Velocity writeback time: {}",
+            self.velocity_writeback_time
         )
     }
 }
diff --git a/src/counters/stages_counters.rs b/src/counters/stages_counters.rs
index b61adb7..3134986 100644
--- a/src/counters/stages_counters.rs
+++ b/src/counters/stages_counters.rs
@@ -14,10 +14,14 @@ pub struct StagesCounters {
     pub solver_time: Timer,
     /// Total time spent for CCD and CCD resolution.
     pub ccd_time: Timer,
+    /// Total time spent updating the query pipeline (if provided to `PhysicsPipeline::step`).
+    pub query_pipeline_time: Timer,
+    /// Total time spent propagating user changes.
+    pub user_changes: Timer,
 }
 
 impl StagesCounters {
-    /// Create a new counter intialized to zero.
+    /// Create a new counter initialized to zero.
     pub fn new() -> Self {
         StagesCounters {
             update_time: Timer::new(),
@@ -25,6 +29,8 @@ impl StagesCounters {
             island_construction_time: Timer::new(),
             solver_time: Timer::new(),
             ccd_time: Timer::new(),
+            query_pipeline_time: Timer::new(),
+            user_changes: Timer::new(),
         }
     }
 
@@ -35,6 +41,8 @@ impl StagesCounters {
         self.island_construction_time.reset();
         self.solver_time.reset();
         self.ccd_time.reset();
+        self.query_pipeline_time.reset();
+        self.user_changes.reset();
     }
 }
 
@@ -52,6 +60,8 @@ impl Display for StagesCounters {
             self.island_construction_time
         )?;
         writeln!(f, "Solver time: {}", self.solver_time)?;
-        writeln!(f, "CCD time: {}", self.ccd_time)
+        writeln!(f, "CCD time: {}", self.ccd_time)?;
+        writeln!(f, "Query pipeline time: {}", self.query_pipeline_time)?;
+        writeln!(f, "User changes time: {}", self.user_changes)
     }
 }
diff --git a/src/data/graph.rs b/src/data/graph.rs
index 6a3ba15..6c701e4 100644
--- a/src/data/graph.rs
+++ b/src/data/graph.rs
@@ -370,7 +370,7 @@ impl<N, E> Graph<N, E> {
         // indices.
         let edge = self.edges.swap_remove(e.index());
         let swap = match self.edges.get(e.index()) {
-            // no elment needed to be swapped.
+            // no element needed to be swapped.
             None => return Some(edge.weight),
             Some(ed) => ed.node,
         };
diff --git a/src/data/pubsub.rs b/src/data/pubsub.rs
index 619521e..92789f2 100644
--- a/src/data/pubsub.rs
+++ b/src/data/pubsub.rs
@@ -103,7 +103,7 @@ impl<T> PubSub<T> {
         subscription
     }
 
-    /// Read the i-th message not yet read by the given subsciber.
+    /// Read the i-th message not yet read by the given subscriber.
     pub fn read_ith(&self, sub: &Subscription<T>, i: usize) -> Option<&T> {
         let cursor = &self.cursors[sub.id as usize];
         self.messages.get(cursor.next(self.deleted_messages) + i)
@@ -114,11 +114,7 @@ impl<T> PubSub<T> {
         let cursor = &self.cursors[sub.id as usize];
         let next = cursor.next(self.deleted_messages);
 
-        // TODO: use self.queue.range(next..) once it is stabilised.
-        MessageRange {
-            queue: &self.messages,
-            next,
-        }
+        self.messages.range(next..)
     }
 
     /// Makes the given subscribe acknowledge all the messages in the queue.
@@ -159,19 +155,3 @@ impl<T> PubSub<T> {
         }
     }
 }
-
-struct MessageRange<'a, T> {
-    queue: &'a VecDeque<T>,
-    next: usize,
-}
-
-impl<'a, T> Iterator for MessageRange<'a, T> {
-    type Item = &'a T;
-
-    #[inline(always)]
-    fn next(&mut self) -> Option<&'a T> {
-        let result = self.queue.get(self.next);
-        self.next += 1;
-        result
-    }
-}
diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs
index 719f3c5..426cbb1 100644
--- a/src/dynamics/ccd/toi_entry.rs
+++ b/src/dynamics/ccd/toi_entry.rs
@@ -129,7 +129,7 @@ impl TOIEntry {
         //     .ok();
 
         let res_toi = query_dispatcher
-            .nonlinear_time_of_impact(
+            .cast_shapes_nonlinear(
                 &motion_c1,
                 co1.shape.as_ref(),
                 &motion_c2,
@@ -143,7 +143,7 @@ impl TOIEntry {
         let toi = res_toi??;
 
         Some(Self::new(
-            toi.toi,
+            toi.time_of_impact,
             ch1,
             co1.parent.map(|p| p.handle),
             ch2,
diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index 13b3fde..2de58ae 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/src/dynamics/integration_parameters.rs
@@ -1,13 +1,18 @@
 use crate::math::Real;
+use na::RealField;
 use std::num::NonZeroUsize;
 
+// TODO: enabling the block solver in 3d introduces a lot of jitters in
+//       the 3D domino demo. So for now we dont enable it in 3D.
+pub(crate) static BLOCK_SOLVER_ENABLED: bool = cfg!(feature = "dim2");
+
 /// Parameters for a time-step of the physics engine.
 #[derive(Copy, Clone, Debug)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 pub struct IntegrationParameters {
-    /// The timestep length (default: `1.0 / 60.0`)
+    /// The timestep length (default: `1.0 / 60.0`).
     pub dt: Real,
-    /// Minimum timestep size when using CCD with multiple substeps (default `1.0 / 60.0 / 100.0`)
+    /// Minimum timestep size when using CCD with multiple substeps (default: `1.0 / 60.0 / 100.0`).
     ///
     /// When CCD with multiple substeps is enabled, the timestep is subdivided
     /// into smaller pieces. This timestep subdivision won't generate timestep
@@ -19,37 +24,78 @@ pub struct IntegrationParameters {
     /// to numerical instabilities.
     pub min_ccd_dt: Real,
 
-    /// 0-1: multiplier for how much of the constraint violation (e.g. contact penetration)
-    /// will be compensated for during the velocity solve.
-    /// (default `0.8`).
-    pub erp: Real,
-    /// 0-1: the damping ratio used by the springs for Baumgarte constraints stabilization.
-    /// Lower values make the constraints more compliant (more "springy", allowing more visible penetrations
-    /// before stabilization).
-    /// (default `0.25`).
-    pub damping_ratio: Real,
+    /// > 0: the damping ratio used by the springs for contact constraint stabilization.
+    ///
+    /// Larger values make the constraints more compliant (allowing more visible
+    /// penetrations before stabilization).
+    /// (default `5.0`).
+    pub contact_damping_ratio: Real,
 
-    /// 0-1: multiplier for how much of the joint violation
-    /// will be compensated for during the velocity solve.
-    /// (default `1.0`).
-    pub joint_erp: Real,
+    /// > 0: the natural frequency used by the springs for contact constraint regularization.
+    ///
+    /// Increasing this value will make it so that penetrations get fixed more quickly at the
+    /// expense of potential jitter effects due to overshooting. In order to make the simulation
+    /// look stiffer, it is recommended to increase the [`Self::contact_damping_ratio`] instead of this
+    /// value.
+    /// (default: `30.0`).
+    pub contact_natural_frequency: Real,
+
+    /// > 0: the natural frequency used by the springs for joint constraint regularization.
+    ///
+    /// Increasing this value will make it so that penetrations get fixed more quickly.
+    /// (default: `1.0e6`).
+    pub joint_natural_frequency: Real,
 
     /// The fraction of critical damping applied to the joint for constraints regularization.
-    /// (default `0.25`).
+    ///
+    /// Larger values make the constraints more compliant (allowing more joint
+    /// drift before stabilization).
+    /// (default `1.0`).
     pub joint_damping_ratio: Real,
 
-    /// Amount of penetration the engine wont attempt to correct (default: `0.001m`).
-    pub allowed_linear_error: Real,
-    /// Maximum amount of penetration the solver will attempt to resolve in one timestep.
-    pub max_penetration_correction: Real,
-    /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`).
-    pub prediction_distance: Real,
+    /// The coefficient in `[0, 1]` applied to warmstart impulses, i.e., impulses that are used as the
+    /// initial solution (instead of 0) at the next simulation step.
+    ///
+    /// This should generally be set to 1.
+    ///
+    /// (default `1.0`).
+    pub warmstart_coefficient: Real,
+
+    /// The approximate size of most dynamic objects in the scene.
+    ///
+    /// This value is used internally to estimate some length-based tolerance. In particular, the
+    /// values [`IntegrationParameters::allowed_linear_error`],
+    /// [`IntegrationParameters::max_corrective_velocity`],
+    /// [`IntegrationParameters::prediction_distance`], [`RigidBodyActivation::linear_threshold`]
+    /// are scaled by this value implicitly.
+    ///
+    /// This value can be understood as the number of units-per-meter in your physical world compared
+    /// to a human-sized world in meter. For example, in a 2d game, if your typical object size is 100
+    /// pixels, set the [`Self::length_unit`] parameter to 100.0. The physics engine will interpret
+    /// it as if 100 pixels is equivalent to 1 meter in its various internal threshold.
+    /// (default `1.0`).
+    pub length_unit: Real,
+
+    /// Amount of penetration the engine won’t attempt to correct (default: `0.001m`).
+    ///
+    /// This value is implicitly scaled by [`IntegrationParameters::length_unit`].
+    pub normalized_allowed_linear_error: Real,
+    /// Maximum amount of penetration the solver will attempt to resolve in one timestep (default: `10.0`).
+    ///
+    /// This value is implicitly scaled by [`IntegrationParameters::length_unit`].
+    pub normalized_max_corrective_velocity: Real,
+    /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002m`).
+    ///
+    /// This value is implicitly scaled by [`IntegrationParameters::length_unit`].
+    pub normalized_prediction_distance: Real,
     /// The number of solver iterations run by the constraints solver for calculating forces (default: `4`).
     pub num_solver_iterations: NonZeroUsize,
-    /// Number of addition friction resolution iteration run during the last solver sub-step (default: `4`).
+    /// Number of addition friction resolution iteration run during the last solver sub-step (default: `0`).
     pub num_additional_friction_iterations: usize,
     /// Number of internal Project Gauss Seidel (PGS) iterations run at each solver iteration (default: `1`).
     pub num_internal_pgs_iterations: usize,
+    /// The number of stabilization iterations run at each solver iterations (default: `2`).
+    pub num_internal_stabilization_iterations: usize,
     /// Minimum number of dynamic bodies in each active island (default: `128`).
     pub min_island_size: usize,
     /// Maximum number of substeps performed by the  solver (default: `1`).
@@ -57,51 +103,6 @@ pub struct IntegrationParameters {
 }
 
 impl IntegrationParameters {
-    /// Configures the integration parameters to match the old PGS solver
-    /// from Rapier version <= 0.17.
-    ///
-    /// This solver was slightly faster than the new one but resulted
-    /// in less stable joints and worse convergence rates.
-    ///
-    /// This should only be used for comparison purpose or if you are
-    /// experiencing problems with the new solver.
-    ///
-    /// NOTE: this does not affect any [`RigidBody::additional_solver_iterations`] that will
-    ///       still create solver iterations based on the new "small-steps" PGS solver.
-    /// NOTE: this resets [`Self::erp`], [`Self::damping_ratio`], [`Self::joint_erp`],
-    ///       [`Self::joint_damping_ratio`] to their former default values.
-    pub fn switch_to_standard_pgs_solver(&mut self) {
-        self.num_internal_pgs_iterations *= self.num_solver_iterations.get();
-        self.num_solver_iterations = NonZeroUsize::new(1).unwrap();
-        self.erp = 0.8;
-        self.damping_ratio = 0.25;
-        self.joint_erp = 1.0;
-        self.joint_damping_ratio = 1.0;
-    }
-
-    /// Configures the integration parameters to match the new "small-steps" PGS solver
-    /// from Rapier version >= 0.18.
-    ///
-    /// The "small-steps" PGS solver is the default one given by [`Self::default()`] so
-    /// calling this function is generally not needed unless
-    /// [`Self::switch_to_standard_pgs_solver()`] was called.
-    ///
-    /// This solver results in more stable joints and significantly better convergence
-    /// rates but is slightly slower in its default settings.
-    ///
-    /// NOTE: this resets [`Self::erp`], [`Self::damping_ratio`], [`Self::joint_erp`],
-    ///       [`Self::joint_damping_ratio`] to their default values.
-    pub fn switch_to_small_steps_pgs_solver(&mut self) {
-        self.num_solver_iterations = NonZeroUsize::new(self.num_internal_pgs_iterations).unwrap();
-        self.num_internal_pgs_iterations = 1;
-
-        let default = Self::default();
-        self.erp = default.erp;
-        self.damping_ratio = default.damping_ratio;
-        self.joint_erp = default.joint_erp;
-        self.joint_damping_ratio = default.joint_damping_ratio;
-    }
-
     /// The inverse of the time-stepping length, i.e. the steps per seconds (Hz).
     ///
     /// This is zero if `self.dt` is zero.
@@ -134,29 +135,65 @@ impl IntegrationParameters {
         }
     }
 
-    /// The ERP coefficient, multiplied by the inverse timestep length.
-    pub fn erp_inv_dt(&self) -> Real {
-        self.erp * self.inv_dt()
+    /// The contact’s spring angular frequency for constraints regularization.
+    pub fn contact_angular_frequency(&self) -> Real {
+        self.contact_natural_frequency * Real::two_pi()
     }
 
-    /// The joint ERP coefficient, multiplied by the inverse timestep length.
+    /// The [`Self::contact_erp`] coefficient, multiplied by the inverse timestep length.
+    pub fn contact_erp_inv_dt(&self) -> Real {
+        let ang_freq = self.contact_angular_frequency();
+        ang_freq / (self.dt * ang_freq + 2.0 * self.contact_damping_ratio)
+    }
+
+    /// The effective Error Reduction Parameter applied for calculating regularization forces
+    /// on contacts.
+    ///
+    /// This parameter is computed automatically from [`Self::contact_natural_frequency`],
+    /// [`Self::contact_damping_ratio`] and the substep length.
+    pub fn contact_erp(&self) -> Real {
+        self.dt * self.contact_erp_inv_dt()
+    }
+
+    /// The joint’s spring angular frequency for constraint regularization.
+    pub fn joint_angular_frequency(&self) -> Real {
+        self.joint_natural_frequency * Real::two_pi()
+    }
+
+    /// The [`Self::joint_erp`] coefficient, multiplied by the inverse timestep length.
     pub fn joint_erp_inv_dt(&self) -> Real {
-        self.joint_erp * self.inv_dt()
+        let ang_freq = self.joint_angular_frequency();
+        ang_freq / (self.dt * ang_freq + 2.0 * self.joint_damping_ratio)
     }
 
-    /// The CFM factor to be used in the constraints resolution.
-    pub fn cfm_factor(&self) -> Real {
+    /// The effective Error Reduction Parameter applied for calculating regularization forces
+    /// on joints.
+    ///
+    /// This parameter is computed automatically from [`Self::joint_natural_frequency`],
+    /// [`Self::joint_damping_ratio`] and the substep length.
+    pub fn joint_erp(&self) -> Real {
+        self.dt * self.joint_erp_inv_dt()
+    }
+
+    /// The CFM factor to be used in the constraint resolution.
+    ///
+    /// This parameter is computed automatically from [`Self::contact_natural_frequency`],
+    /// [`Self::contact_damping_ratio`] and the substep length.
+    pub fn contact_cfm_factor(&self) -> Real {
         // Compute CFM assuming a critically damped spring multiplied by the damping ratio.
-        let inv_erp_minus_one = 1.0 / self.erp - 1.0;
+        let inv_erp_minus_one = 1.0 / self.contact_erp() - 1.0;
 
         // let stiffness = 4.0 * damping_ratio * damping_ratio * projected_mass
         //     / (dt * dt * inv_erp_minus_one * inv_erp_minus_one);
         // let damping = 4.0 * damping_ratio * damping_ratio * projected_mass
         //     / (dt * inv_erp_minus_one);
         // let cfm = 1.0 / (dt * dt * stiffness + dt * damping);
-        // NOTE: This simplies to cfm = cfm_coefff / projected_mass:
+        // NOTE: This simplifies to cfm = cfm_coeff / projected_mass:
         let cfm_coeff = inv_erp_minus_one * inv_erp_minus_one
-            / ((1.0 + inv_erp_minus_one) * 4.0 * self.damping_ratio * self.damping_ratio);
+            / ((1.0 + inv_erp_minus_one)
+                * 4.0
+                * self.contact_damping_ratio
+                * self.contact_damping_ratio);
 
         // Furthermore, we use this coefficient inside of the impulse resolution.
         // Surprisingly, several simplifications happen there.
@@ -173,36 +210,65 @@ impl IntegrationParameters {
         // new_impulse = cfm_factor * (old_impulse - m * delta_vel)
         //
         // The value returned by this function is this cfm_factor that can be used directly
-        // in the constraints solver.
+        // in the constraint solver.
         1.0 / (1.0 + cfm_coeff)
     }
 
-    /// The CFM (constraints force mixing) coefficient applied to all joints for constraints regularization
+    /// The CFM (constraints force mixing) coefficient applied to all joints for constraints regularization.
+    ///
+    /// This parameter is computed automatically from [`Self::joint_natural_frequency`],
+    /// [`Self::joint_damping_ratio`] and the substep length.
     pub fn joint_cfm_coeff(&self) -> Real {
         // Compute CFM assuming a critically damped spring multiplied by the damping ratio.
-        let inv_erp_minus_one = 1.0 / self.joint_erp - 1.0;
+        // The logic is similar to `Self::cfm_factor`.
+        let inv_erp_minus_one = 1.0 / self.joint_erp() - 1.0;
         inv_erp_minus_one * inv_erp_minus_one
             / ((1.0 + inv_erp_minus_one)
                 * 4.0
                 * self.joint_damping_ratio
                 * self.joint_damping_ratio)
     }
-}
 
-impl Default for IntegrationParameters {
-    fn default() -> Self {
+    /// Amount of penetration the engine won’t attempt to correct (default: `0.001` multiplied by
+    /// [`Self::length_unit`]).
+    pub fn allowed_linear_error(&self) -> Real {
+        self.normalized_allowed_linear_error * self.length_unit
+    }
+
+    /// Maximum amount of penetration the solver will attempt to resolve in one timestep.
+    ///
+    /// This is equal to [`Self::normalized_max_corrective_velocity`] multiplied by
+    /// [`Self::length_unit`].
+    pub fn max_corrective_velocity(&self) -> Real {
+        if self.normalized_max_corrective_velocity != Real::MAX {
+            self.normalized_max_corrective_velocity * self.length_unit
+        } else {
+            Real::MAX
+        }
+    }
+
+    /// The maximal distance separating two objects that will generate predictive contacts
+    /// (default: `0.002m` multiped by [`Self::length_unit`]).
+    pub fn prediction_distance(&self) -> Real {
+        self.normalized_prediction_distance * self.length_unit
+    }
+
+    /// Initialize the simulation parameters with settings matching the TGS-soft solver
+    /// with warmstarting.
+    ///
+    /// This is the default configuration, equivalent to [`IntegrationParameters::default()`].
+    pub fn tgs_soft() -> Self {
         Self {
             dt: 1.0 / 60.0,
             min_ccd_dt: 1.0 / 60.0 / 100.0,
-            erp: 0.6,
-            damping_ratio: 1.0,
-            joint_erp: 1.0,
+            contact_natural_frequency: 30.0,
+            contact_damping_ratio: 5.0,
+            joint_natural_frequency: 1.0e6,
             joint_damping_ratio: 1.0,
-            allowed_linear_error: 0.001,
-            max_penetration_correction: Real::MAX,
-            prediction_distance: 0.002,
+            warmstart_coefficient: 1.0,
             num_internal_pgs_iterations: 1,
-            num_additional_friction_iterations: 4,
+            num_internal_stabilization_iterations: 2,
+            num_additional_friction_iterations: 0,
             num_solver_iterations: NonZeroUsize::new(4).unwrap(),
             // TODO: what is the optimal value for min_island_size?
             // It should not be too big so that we don't end up with
@@ -210,7 +276,42 @@ impl Default for IntegrationParameters {
             // 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,
+            normalized_allowed_linear_error: 0.001,
+            normalized_max_corrective_velocity: 10.0,
+            normalized_prediction_distance: 0.002,
             max_ccd_substeps: 1,
+            length_unit: 1.0,
+        }
+    }
+
+    /// Initialize the simulation parameters with settings matching the TGS-soft solver
+    /// **without** warmstarting.
+    ///
+    /// The [`IntegrationParameters::tgs_soft()`] configuration should be preferred unless
+    /// warmstarting proves to be undesirable for your use-case.
+    pub fn tgs_soft_without_warmstart() -> Self {
+        Self {
+            contact_damping_ratio: 0.25,
+            warmstart_coefficient: 0.0,
+            num_additional_friction_iterations: 4,
+            ..Self::tgs_soft()
+        }
+    }
+
+    /// Initializes the integration parameters to match the legacy PGS solver from Rapier version <= 0.17.
+    ///
+    /// This exists mainly for testing and comparison purpose.
+    pub fn pgs_legacy() -> Self {
+        Self {
+            num_solver_iterations: NonZeroUsize::new(1).unwrap(),
+            num_internal_pgs_iterations: 4,
+            ..Self::tgs_soft_without_warmstart()
         }
     }
 }
+
+impl Default for IntegrationParameters {
+    fn default() -> Self {
+        Self::tgs_soft()
+    }
+}
diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs
index 8f18941..a576946 100644
--- a/src/dynamics/island_manager.rs
+++ b/src/dynamics/island_manager.rs
@@ -150,6 +150,7 @@ impl IslandManager {
     pub(crate) fn update_active_set_with_contacts(
         &mut self,
         dt: Real,
+        length_unit: Real,
         bodies: &mut RigidBodySet,
         colliders: &ColliderSet,
         narrow_phase: &NarrowPhase,
@@ -181,7 +182,7 @@ impl IslandManager {
             let sq_linvel = rb.vels.linvel.norm_squared();
             let sq_angvel = rb.vels.angvel.gdot(rb.vels.angvel);
 
-            update_energy(&mut rb.activation, sq_linvel, sq_angvel, dt);
+            update_energy(length_unit, &mut rb.activation, sq_linvel, sq_angvel, dt);
 
             if rb.activation.time_since_can_sleep >= rb.activation.time_until_sleep {
                 // Mark them as sleeping for now. This will
@@ -324,8 +325,15 @@ impl IslandManager {
     }
 }
 
-fn update_energy(activation: &mut RigidBodyActivation, sq_linvel: Real, sq_angvel: Real, dt: Real) {
-    if sq_linvel < activation.linear_threshold * activation.linear_threshold.abs()
+fn update_energy(
+    length_unit: Real,
+    activation: &mut RigidBodyActivation,
+    sq_linvel: Real,
+    sq_angvel: Real,
+    dt: Real,
+) {
+    let linear_threshold = activation.normalized_linear_threshold * length_unit;
+    if sq_linvel < linear_threshold * linear_threshold.abs()
         && sq_angvel < activation.angular_threshold * activation.angular_threshold.abs()
     {
         activation.time_since_can_sleep += dt;
diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs
index 76a7fe1..8c8a4aa 100644
--- a/src/dynamics/joint/generic_joint.rs
+++ b/src/dynamics/joint/generic_joint.rs
@@ -496,6 +496,24 @@ impl GenericJoint {
         self.motors[i].damping = damping;
         self
     }
+
+    /// Flips the orientation of the joint, including limits and motors.
+    pub fn flip(&mut self) {
+        std::mem::swap(&mut self.local_frame1, &mut self.local_frame2);
+
+        let coupled_bits = self.coupled_axes.bits();
+
+        for dim in 0..SPATIAL_DIM {
+            if coupled_bits & (1 << dim) == 0 {
+                let limit = self.limits[dim];
+                self.limits[dim].min = -limit.max;
+                self.limits[dim].max = -limit.min;
+            }
+
+            self.motors[dim].target_vel = -self.motors[dim].target_vel;
+            self.motors[dim].target_pos = -self.motors[dim].target_pos;
+        }
+    }
 }
 
 macro_rules! joint_conversion_methods(
diff --git a/src/dynamics/joint/multibody_joint/mod.rs b/src/dynamics/joint/multibody_joint/mod.rs
index c789004..dc1fa75 100644
--- a/src/dynamics/joint/multibody_joint/mod.rs
+++ b/src/dynamics/joint/multibody_joint/mod.rs
@@ -1,6 +1,7 @@
 //! MultibodyJoints using the reduced-coordinates formalism or using constraints.
 
 pub use self::multibody::Multibody;
+pub use self::multibody_ik::InverseKinematicsOption;
 pub use self::multibody_joint::MultibodyJoint;
 pub use self::multibody_joint_set::{
     MultibodyIndex, MultibodyJointHandle, MultibodyJointSet, MultibodyLinkId,
@@ -13,5 +14,6 @@ mod multibody_joint_set;
 mod multibody_link;
 mod multibody_workspace;
 
+mod multibody_ik;
 mod multibody_joint;
 mod unit_multibody_joint;
diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs
index 617d447..fb56087 100644
--- a/src/dynamics/joint/multibody_joint/multibody.rs
+++ b/src/dynamics/joint/multibody_joint/multibody.rs
@@ -89,6 +89,7 @@ impl Default for Multibody {
         Multibody::new()
     }
 }
+
 impl Multibody {
     /// Creates a new multibody with no link.
     pub fn new() -> Self {
@@ -115,6 +116,8 @@ impl Multibody {
 
     pub(crate) fn with_root(handle: RigidBodyHandle) -> Self {
         let mut mb = Multibody::new();
+        // NOTE: we have no way of knowing if the root in fixed at this point, so
+        //       we mark it as dynamic and will fixe later with `Self::update_root_type`.
         mb.root_is_dynamic = true;
         let joint = MultibodyJoint::free(Isometry::identity());
         mb.add_link(None, joint, handle);
@@ -747,6 +750,12 @@ impl Multibody {
         self.velocities.rows(0, self.ndofs)
     }
 
+    /// The body jacobian for link `link_id` calculated by the last call to [`Multibody::forward_kinematics`].
+    #[inline]
+    pub fn body_jacobian(&self, link_id: usize) -> &Jacobian<Real> {
+        &self.body_jacobians[link_id]
+    }
+
     /// The mutable generalized velocities of this multibodies.
     #[inline]
     pub fn generalized_velocity_mut(&mut self) -> DVectorViewMut<Real> {
@@ -762,17 +771,27 @@ impl Multibody {
     }
 
     /// Apply displacements, in generalized coordinates, to this multibody.
+    ///
+    /// Note this does **not** updates the link poses, only their generalized coordinates.
+    /// To update the link poses and associated rigid-bodies, call [`Self::forward_kinematics`]
+    /// or [`Self::finalize`].
     pub fn apply_displacements(&mut self, disp: &[Real]) {
         for link in self.links.iter_mut() {
             link.joint.apply_displacement(&disp[link.assembly_id..])
         }
     }
 
-    pub(crate) fn update_root_type(&mut self, bodies: &mut RigidBodySet) {
+    pub(crate) fn update_root_type(&mut self, bodies: &RigidBodySet, take_body_pose: bool) {
         if let Some(rb) = bodies.get(self.links[0].rigid_body) {
             if rb.is_dynamic() != self.root_is_dynamic {
+                let root_pose = if take_body_pose {
+                    *rb.position()
+                } else {
+                    self.links[0].local_to_world
+                };
+
                 if rb.is_dynamic() {
-                    let free_joint = MultibodyJoint::free(*rb.position());
+                    let free_joint = MultibodyJoint::free(root_pose);
                     let prev_root_ndofs = self.links[0].joint().ndofs();
                     self.links[0].joint = free_joint;
                     self.links[0].assembly_id = 0;
@@ -791,7 +810,7 @@ impl Multibody {
                     assert!(self.damping.len() >= SPATIAL_DIM);
                     assert!(self.accelerations.len() >= SPATIAL_DIM);
 
-                    let fixed_joint = MultibodyJoint::fixed(*rb.position());
+                    let fixed_joint = MultibodyJoint::fixed(root_pose);
                     let prev_root_ndofs = self.links[0].joint().ndofs();
                     self.links[0].joint = fixed_joint;
                     self.links[0].assembly_id = 0;
@@ -820,30 +839,86 @@ impl Multibody {
             }
 
             // Make sure the positions are properly set to match the rigid-body’s.
-            if self.links[0].joint.data.locked_axes.is_empty() {
-                self.links[0].joint.set_free_pos(*rb.position());
+            if take_body_pose {
+                if self.links[0].joint.data.locked_axes.is_empty() {
+                    self.links[0].joint.set_free_pos(*rb.position());
+                } else {
+                    self.links[0].joint.data.local_frame1 = *rb.position();
+                }
+            }
+        }
+    }
+
+    /// Update the rigid-body poses based on this multibody joint poses.
+    ///
+    /// This is typically called after [`Self::forward_kinematics`] to apply the new joint poses
+    /// to the rigid-bodies.
+    pub fn update_rigid_bodies(&self, bodies: &mut RigidBodySet, update_mass_properties: bool) {
+        self.update_rigid_bodies_internal(bodies, update_mass_properties, false, true)
+    }
+
+    pub(crate) fn update_rigid_bodies_internal(
+        &self,
+        bodies: &mut RigidBodySet,
+        update_mass_properties: bool,
+        update_next_positions_only: bool,
+        change_tracking: bool,
+    ) {
+        // Handle the children. They all have a parent within this multibody.
+        for link in self.links.iter() {
+            let rb = if change_tracking {
+                bodies.get_mut_internal_with_modification_tracking(link.rigid_body)
             } else {
-                self.links[0].joint.data.local_frame1 = *rb.position();
+                bodies.get_mut_internal(link.rigid_body)
+            };
+
+            if let Some(rb) = rb {
+                rb.pos.next_position = link.local_to_world;
+
+                if !update_next_positions_only {
+                    rb.pos.position = link.local_to_world;
+                }
+
+                if update_mass_properties {
+                    rb.mprops.update_world_mass_properties(&link.local_to_world);
+                }
             }
         }
     }
 
     // TODO: make a version that doesn’t write back to bodies and doesn’t update the jacobians
     //       (i.e. just something used by the velocity solver’s small steps).
-    /// Apply forward-kinematics to this multibody and its related rigid-bodies.
-    pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_rb_mass_props: bool) {
+    /// Apply forward-kinematics to this multibody.
+    ///
+    /// This will update the [`MultibodyLink`] pose information as wall as the body jacobians.
+    /// This will also ensure that the multibody has the proper number of degrees of freedom if
+    /// its root node changed between dynamic and non-dynamic.
+    ///
+    /// Note that this does **not** update the poses of the [`RigidBody`] attached to the joints.
+    /// Run [`Self::update_rigid_bodies`] to trigger that update.
+    ///
+    /// This method updates `self` with the result of the forward-kinematics operation.
+    /// For a non-mutable version running forward kinematics on a single link, see
+    /// [`Self::forward_kinematics_single_link`].
+    ///
+    /// ## Parameters
+    /// - `bodies`: the set of rigid-bodies.
+    /// - `read_root_pose_from_rigid_body`: if set to `true`, the root joint (either a fixed joint,
+    ///   or a free joint) will have its pose set to its associated-rigid-body pose. Set this to `true`
+    ///   when the root rigid-body pose has been modified and needs to affect the multibody.
+    pub fn forward_kinematics(
+        &mut self,
+        bodies: &RigidBodySet,
+        read_root_pose_from_rigid_body: bool,
+    ) {
+        // Be sure the degrees of freedom match and take the root position if needed.
+        self.update_root_type(bodies, read_root_pose_from_rigid_body);
+
         // Special case for the root, which has no parent.
         {
             let link = &mut self.links[0];
             link.local_to_parent = link.joint.body_to_parent();
             link.local_to_world = link.local_to_parent;
-
-            if let Some(rb) = bodies.get_mut_internal(link.rigid_body) {
-                rb.pos.next_position = link.local_to_world;
-                if update_rb_mass_props {
-                    rb.mprops.update_world_mass_properties(&link.local_to_world);
-                }
-            }
         }
 
         // Handle the children. They all have a parent within this multibody.
@@ -865,20 +940,11 @@ impl Multibody {
                 link.shift23 = c3 - c2;
             }
 
-            let link_rb = bodies.index_mut_internal(link.rigid_body);
-            link_rb.pos.next_position = link.local_to_world;
-
             assert_eq!(
-                link_rb.body_type,
+                bodies[link.rigid_body].body_type,
                 RigidBodyType::Dynamic,
                 "A rigid-body that is not at the root of a multibody must be dynamic."
             );
-
-            if update_rb_mass_props {
-                link_rb
-                    .mprops
-                    .update_world_mass_properties(&link.local_to_world);
-            }
         }
 
         /*
@@ -887,6 +953,107 @@ impl Multibody {
         self.update_body_jacobians();
     }
 
+    /// Apply forward-kinematics to compute the position of a single link of this multibody.
+    ///
+    /// If `out_jacobian` is `Some`, this will simultaneously compute the new jacobian of this link.
+    /// If `displacement` is `Some`, the generalized position considered during transform propagation
+    /// is the sum of the current position of `self` and this `displacement`.
+    // TODO: this shares a lot of code with `forward_kinematics` and `update_body_jacobians`, except
+    //       that we are only traversing a single kinematic chain. Could this be refactored?
+    pub fn forward_kinematics_single_link(
+        &self,
+        bodies: &RigidBodySet,
+        link_id: usize,
+        displacement: Option<&[Real]>,
+        mut out_jacobian: Option<&mut Jacobian<Real>>,
+    ) -> Isometry<Real> {
+        let mut branch = vec![]; // Perf: avoid allocation.
+        let mut curr_id = Some(link_id);
+
+        while let Some(id) = curr_id {
+            branch.push(id);
+            curr_id = self.links[id].parent_id();
+        }
+
+        branch.reverse();
+
+        if let Some(out_jacobian) = out_jacobian.as_deref_mut() {
+            if out_jacobian.ncols() != self.ndofs {
+                *out_jacobian = Jacobian::zeros(self.ndofs);
+            } else {
+                out_jacobian.fill(0.0);
+            }
+        }
+
+        let mut parent_link: Option<MultibodyLink> = None;
+
+        for i in branch {
+            let mut link = self.links[i];
+
+            if let Some(displacement) = displacement {
+                link.joint
+                    .apply_displacement(&displacement[link.assembly_id..]);
+            }
+
+            let parent_to_world;
+
+            if let Some(parent_link) = parent_link {
+                link.local_to_parent = link.joint.body_to_parent();
+                link.local_to_world = parent_link.local_to_world * link.local_to_parent;
+
+                {
+                    let parent_rb = &bodies[parent_link.rigid_body];
+                    let link_rb = &bodies[link.rigid_body];
+                    let c0 = parent_link.local_to_world * parent_rb.mprops.local_mprops.local_com;
+                    let c2 = link.local_to_world
+                        * Point::from(link.joint.data.local_frame2.translation.vector);
+                    let c3 = link.local_to_world * link_rb.mprops.local_mprops.local_com;
+
+                    link.shift02 = c2 - c0;
+                    link.shift23 = c3 - c2;
+                }
+
+                parent_to_world = parent_link.local_to_world;
+
+                if let Some(out_jacobian) = out_jacobian.as_deref_mut() {
+                    let (mut link_j_v, parent_j_w) =
+                        out_jacobian.rows_range_pair_mut(0..DIM, DIM..DIM + ANG_DIM);
+                    let shift_tr = (link.shift02).gcross_matrix_tr();
+                    link_j_v.gemm(1.0, &shift_tr, &parent_j_w, 1.0);
+                }
+            } else {
+                link.local_to_parent = link.joint.body_to_parent();
+                link.local_to_world = link.local_to_parent;
+                parent_to_world = Isometry::identity();
+            }
+
+            if let Some(out_jacobian) = out_jacobian.as_deref_mut() {
+                let ndofs = link.joint.ndofs();
+                let mut tmp = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
+                let mut link_joint_j = tmp.columns_mut(0, ndofs);
+                let mut link_j_part = out_jacobian.columns_mut(link.assembly_id, ndofs);
+                link.joint.jacobian(
+                    &(parent_to_world.rotation * link.joint.data.local_frame1.rotation),
+                    &mut link_joint_j,
+                );
+                link_j_part += link_joint_j;
+
+                {
+                    let (mut link_j_v, link_j_w) =
+                        out_jacobian.rows_range_pair_mut(0..DIM, DIM..DIM + ANG_DIM);
+                    let shift_tr = link.shift23.gcross_matrix_tr();
+                    link_j_v.gemm(1.0, &shift_tr, &link_j_w, 1.0);
+                }
+            }
+
+            parent_link = Some(link);
+        }
+
+        parent_link
+            .map(|link| link.local_to_world)
+            .unwrap_or_else(Isometry::identity)
+    }
+
     /// The total number of freedoms of this multibody.
     #[inline]
     pub fn ndofs(&self) -> usize {
diff --git a/src/dynamics/joint/multibody_joint/multibody_ik.rs b/src/dynamics/joint/multibody_joint/multibody_ik.rs
new file mode 100644
index 0000000..7e0fc15
--- /dev/null
+++ b/src/dynamics/joint/multibody_joint/multibody_ik.rs
@@ -0,0 +1,238 @@
+use crate::dynamics::{JointAxesMask, Multibody, RigidBodySet};
+use crate::math::{Isometry, Jacobian, Real, ANG_DIM, DIM, SPATIAL_DIM};
+use na::{self, DVector, SMatrix};
+use parry::math::SpacialVector;
+
+#[derive(Copy, Clone, Debug, PartialEq)]
+/// Options for the jacobian-based Inverse Kinematics solver for multibodies.
+pub struct InverseKinematicsOption {
+    /// A damping coefficient.
+    ///
+    /// Small value can lead to overshooting preventing convergence. Large
+    /// values can slown down convergence, requiring more iterations to converge.
+    pub damping: Real,
+    /// The maximum number of iterations the iterative IK solver can take.
+    pub max_iters: usize,
+    /// The axes the IK solver will solve for.
+    pub constrained_axes: JointAxesMask,
+    /// The error threshold on the linear error.
+    ///
+    /// If errors on both linear and angular parts fall below this
+    /// threshold, the iterative resolution will stop.
+    pub epsilon_linear: Real,
+    /// The error threshold on the angular error.
+    ///
+    /// If errors on both linear and angular parts fall below this
+    /// threshold, the iterative resolution will stop.
+    pub epsilon_angular: Real,
+}
+
+impl Default for InverseKinematicsOption {
+    fn default() -> Self {
+        Self {
+            damping: 1.0,
+            max_iters: 10,
+            constrained_axes: JointAxesMask::all(),
+            epsilon_linear: 1.0e-3,
+            epsilon_angular: 1.0e-3,
+        }
+    }
+}
+
+impl Multibody {
+    /// Computes the displacement needed to have the link identified by `link_id` move by the
+    /// desired transform.
+    ///
+    /// The displacement calculated by this function is added to the `displacement` vector.
+    pub fn inverse_kinematics_delta(
+        &self,
+        link_id: usize,
+        desired_movement: &SpacialVector<Real>,
+        damping: Real,
+        displacements: &mut DVector<Real>,
+    ) {
+        let body_jacobian = self.body_jacobian(link_id);
+        Self::inverse_kinematics_delta_with_jacobian(
+            body_jacobian,
+            desired_movement,
+            damping,
+            displacements,
+        );
+    }
+
+    /// Computes the displacement needed to have a link with the given jacobian move by the
+    /// desired transform.
+    ///
+    /// The displacement calculated by this function is added to the `displacement` vector.
+    pub fn inverse_kinematics_delta_with_jacobian(
+        jacobian: &Jacobian<Real>,
+        desired_movement: &SpacialVector<Real>,
+        damping: Real,
+        displacements: &mut DVector<Real>,
+    ) {
+        let identity = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::identity();
+        let jj = jacobian * &jacobian.transpose() + identity * (damping * damping);
+        let inv_jj = jj.pseudo_inverse(1.0e-5).unwrap_or(identity);
+        displacements.gemv_tr(1.0, jacobian, &(inv_jj * desired_movement), 1.0);
+    }
+
+    /// Computes the displacement needed to have the link identified by `link_id` have a pose
+    /// equal (or as close as possible) to `target_pose`.
+    ///
+    /// If `displacement` is given non-zero, the current pose of the rigid-body is considered to be
+    /// obtained from its current generalized coordinates summed with the `displacement` vector.
+    ///
+    /// The `displacements` vector is overwritten with the new displacement.
+    pub fn inverse_kinematics(
+        &self,
+        bodies: &RigidBodySet,
+        link_id: usize,
+        options: &InverseKinematicsOption,
+        target_pose: &Isometry<Real>,
+        displacements: &mut DVector<Real>,
+    ) {
+        let mut jacobian = Jacobian::zeros(0);
+
+        for _ in 0..options.max_iters {
+            let pose = self.forward_kinematics_single_link(
+                bodies,
+                link_id,
+                Some(displacements.as_slice()),
+                Some(&mut jacobian),
+            );
+
+            let delta_lin = target_pose.translation.vector - pose.translation.vector;
+            let delta_ang = (target_pose.rotation * pose.rotation.inverse()).scaled_axis();
+
+            #[cfg(feature = "dim2")]
+            let mut delta = na::vector![delta_lin.x, delta_lin.y, delta_ang.x];
+            #[cfg(feature = "dim3")]
+            let mut delta = na::vector![
+                delta_lin.x,
+                delta_lin.y,
+                delta_lin.z,
+                delta_ang.x,
+                delta_ang.y,
+                delta_ang.z
+            ];
+
+            if !options.constrained_axes.contains(JointAxesMask::X) {
+                delta[0] = 0.0;
+            }
+            if !options.constrained_axes.contains(JointAxesMask::Y) {
+                delta[1] = 0.0;
+            }
+            #[cfg(feature = "dim3")]
+            if !options.constrained_axes.contains(JointAxesMask::Z) {
+                delta[2] = 0.0;
+            }
+            if !options.constrained_axes.contains(JointAxesMask::ANG_X) {
+                delta[DIM] = 0.0;
+            }
+            #[cfg(feature = "dim3")]
+            if !options.constrained_axes.contains(JointAxesMask::ANG_Y) {
+                delta[DIM + 1] = 0.0;
+            }
+            #[cfg(feature = "dim3")]
+            if !options.constrained_axes.contains(JointAxesMask::ANG_Z) {
+                delta[DIM + 2] = 0.0;
+            }
+
+            // TODO: measure convergence on the error variation instead?
+            if delta.rows(0, DIM).norm() <= options.epsilon_linear
+                && delta.rows(DIM, ANG_DIM).norm() <= options.epsilon_angular
+            {
+                break;
+            }
+
+            Self::inverse_kinematics_delta_with_jacobian(
+                &jacobian,
+                &delta,
+                options.damping,
+                displacements,
+            );
+        }
+    }
+}
+
+#[cfg(test)]
+mod test {
+    use crate::dynamics::{
+        MultibodyJointHandle, MultibodyJointSet, RevoluteJointBuilder, RigidBodyBuilder,
+        RigidBodySet,
+    };
+    use crate::math::{Jacobian, Real, Vector};
+    use approx::assert_relative_eq;
+
+    #[test]
+    fn one_link_fwd_kinematics() {
+        let mut bodies = RigidBodySet::new();
+        let mut multibodies = MultibodyJointSet::new();
+
+        let num_segments = 10;
+        let body = RigidBodyBuilder::fixed();
+        let mut last_body = bodies.insert(body);
+        let mut last_link = MultibodyJointHandle::invalid();
+
+        for _ in 0..num_segments {
+            let body = RigidBodyBuilder::dynamic().can_sleep(false);
+            let new_body = bodies.insert(body);
+
+            #[cfg(feature = "dim2")]
+            let builder = RevoluteJointBuilder::new();
+            #[cfg(feature = "dim3")]
+            let builder = RevoluteJointBuilder::new(Vector::z_axis());
+            let link_ab = builder
+                .local_anchor1((Vector::y() * (0.5 / num_segments as Real)).into())
+                .local_anchor2((Vector::y() * (-0.5 / num_segments as Real)).into());
+            last_link = multibodies
+                .insert(last_body, new_body, link_ab, true)
+                .unwrap();
+
+            last_body = new_body;
+        }
+
+        let (multibody, last_id) = multibodies.get_mut(last_link).unwrap();
+        multibody.forward_kinematics(&bodies, true); // Be sure all the dofs are up to date.
+        assert_eq!(multibody.ndofs(), num_segments);
+
+        /*
+         * No displacement.
+         */
+        let mut jacobian2 = Jacobian::zeros(0);
+        let link_pose1 = *multibody.link(last_id).unwrap().local_to_world();
+        let jacobian1 = multibody.body_jacobian(last_id);
+        let link_pose2 =
+            multibody.forward_kinematics_single_link(&bodies, last_id, None, Some(&mut jacobian2));
+        assert_eq!(link_pose1, link_pose2);
+        assert_eq!(jacobian1, &jacobian2);
+
+        /*
+         * Arbitrary displacement.
+         */
+        let niter = 100;
+        let displacement_part: Vec<_> = (0..multibody.ndofs())
+            .map(|i| i as Real * -0.1 / niter as Real)
+            .collect();
+        let displacement_total: Vec<_> = displacement_part
+            .iter()
+            .map(|d| *d * niter as Real)
+            .collect();
+        let link_pose2 = multibody.forward_kinematics_single_link(
+            &bodies,
+            last_id,
+            Some(&displacement_total),
+            Some(&mut jacobian2),
+        );
+
+        for _ in 0..niter {
+            multibody.apply_displacements(&displacement_part);
+            multibody.forward_kinematics(&bodies, false);
+        }
+
+        let link_pose1 = *multibody.link(last_id).unwrap().local_to_world();
+        let jacobian1 = multibody.body_jacobian(last_id);
+        assert_relative_eq!(link_pose1, link_pose2, epsilon = 1.0e-5);
+        assert_relative_eq!(jacobian1, &jacobian2, epsilon = 1.0e-5);
+    }
+}
diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
index fa0ffdb..a95c10b 100644
--- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
+++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
@@ -286,6 +286,13 @@ impl MultibodyJointSet {
         self.multibodies.get(index.0)
     }
 
+    /// Gets a mutable reference to a multibody, based on its temporary index.
+    /// `MultibodyJointSet`.
+    pub fn get_multibody_mut(&mut self, index: MultibodyIndex) -> Option<&mut Multibody> {
+        // TODO: modification tracking.
+        self.multibodies.get_mut(index.0)
+    }
+
     /// Gets a mutable reference to a multibody, based on its temporary index.
     ///
     /// This method will bypass any modification-detection automatically done by the
@@ -363,13 +370,13 @@ impl MultibodyJointSet {
         let parent1 = link1.parent_id();
 
         if parent1 == Some(id2.id) {
-            Some((MultibodyJointHandle(rb1.0), mb, &link1))
+            Some((MultibodyJointHandle(rb1.0), mb, link1))
         } else {
             let link2 = mb.link(id2.id)?;
             let parent2 = link2.parent_id();
 
             if parent2 == Some(id1.id) {
-                Some((MultibodyJointHandle(rb2.0), mb, &link2))
+                Some((MultibodyJointHandle(rb2.0), mb, link2))
             } else {
                 None
             }
diff --git a/src/dynamics/joint/multibody_joint/multibody_link.rs b/src/dynamics/joint/multibody_joint/multibody_link.rs
index 7336a6c..bf6d605 100644
--- a/src/dynamics/joint/multibody_joint/multibody_link.rs
+++ b/src/dynamics/joint/multibody_joint/multibody_link.rs
@@ -6,7 +6,7 @@ use crate::prelude::RigidBodyVelocity;
 
 /// One link of a multibody.
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-#[derive(Clone)]
+#[derive(Copy, Clone)]
 pub struct MultibodyLink {
     // FIXME: make all those private.
     pub(crate) internal_id: usize,
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index b27b58e..25631fc 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -74,6 +74,61 @@ impl RigidBody {
         self.ids = Default::default();
     }
 
+    /// Copy all the characteristics from `other` to `self`.
+    ///
+    /// If you have a mutable reference to a rigid-body `rigid_body: &mut RigidBody`, attempting to
+    /// assign it a whole new rigid-body instance, e.g., `*rigid_body = RigidBodyBuilder::dynamic().build()`,
+    /// will crash due to some internal indices being overwritten. Instead, use
+    /// `rigid_body.copy_from(&RigidBodyBuilder::dynamic().build())`.
+    ///
+    /// This method will allow you to set most characteristics of this rigid-body from another
+    /// rigid-body instance without causing any breakage.
+    ///
+    /// This method **cannot** be used for editing the list of colliders attached to this rigid-body.
+    /// Therefore, the list of colliders attached to `self` won’t be replaced by the one attached
+    /// to `other`.
+    ///
+    /// The pose of `other` will only copied into `self` if `self` doesn’t have a parent (if it has
+    /// a parent, its position is directly controlled by the parent rigid-body).
+    pub fn copy_from(&mut self, other: &RigidBody) {
+        // NOTE: we deconstruct the rigid-body struct to be sure we don’t forget to
+        //       add some copies here if we add more field to RigidBody in the future.
+        let RigidBody {
+            pos,
+            mprops,
+            integrated_vels,
+            vels,
+            damping,
+            forces,
+            ccd,
+            ids: _ids,             // Internal ids must not be overwritten.
+            colliders: _colliders, // This function cannot be used to edit collider sets.
+            activation,
+            changes: _changes, // Will be set to ALL.
+            body_type,
+            dominance,
+            enabled,
+            additional_solver_iterations,
+            user_data,
+        } = other;
+
+        self.pos = *pos;
+        self.mprops = mprops.clone();
+        self.integrated_vels = *integrated_vels;
+        self.vels = *vels;
+        self.damping = *damping;
+        self.forces = *forces;
+        self.ccd = *ccd;
+        self.activation = *activation;
+        self.body_type = *body_type;
+        self.dominance = *dominance;
+        self.enabled = *enabled;
+        self.additional_solver_iterations = *additional_solver_iterations;
+        self.user_data = *user_data;
+
+        self.changes = RigidBodyChanges::all();
+    }
+
     /// Set the additional number of solver iterations run for this rigid-body and
     /// everything interacting with it.
     ///
@@ -237,9 +292,9 @@ impl RigidBody {
         allow_rotations_z: bool,
         wake_up: bool,
     ) {
-        if self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_X) != !allow_rotations_x
-            || self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Y) != !allow_rotations_y
-            || self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z) != !allow_rotations_z
+        if self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_X) == allow_rotations_x
+            || self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Y) == allow_rotations_y
+            || self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z) == allow_rotations_z
         {
             if self.is_dynamic() && wake_up {
                 self.wake_up(true);
@@ -381,18 +436,40 @@ impl RigidBody {
         ]
     }
 
-    /// Enables of disable CCD (continuous collision-detection) for this rigid-body.
+    /// Enables of disable CCD (Continuous Collision-Detection) for this rigid-body.
     ///
     /// CCD prevents tunneling, but may still allow limited interpenetration of colliders.
     pub fn enable_ccd(&mut self, enabled: bool) {
         self.ccd.ccd_enabled = enabled;
     }
 
-    /// Is CCD (continous collision-detection) enabled for this rigid-body?
+    /// Is CCD (continuous collision-detection) enabled for this rigid-body?
     pub fn is_ccd_enabled(&self) -> bool {
         self.ccd.ccd_enabled
     }
 
+    /// Sets the maximum prediction distance Soft Continuous Collision-Detection.
+    ///
+    /// When set to 0, soft-CCD is disabled. Soft-CCD helps prevent tunneling especially of
+    /// slow-but-thin to moderately fast objects. The soft CCD prediction distance indicates how
+    /// far in the object’s path the CCD algorithm is allowed to inspect. Large values can impact
+    /// performance badly by increasing the work needed from the broad-phase.
+    ///
+    /// It is a generally cheaper variant of regular CCD (that can be enabled with
+    /// [`RigidBody::enable_ccd`] since it relies on predictive constraints instead of
+    /// shape-cast and substeps.
+    pub fn set_soft_ccd_prediction(&mut self, prediction_distance: Real) {
+        self.ccd.soft_ccd_prediction = prediction_distance;
+    }
+
+    /// The soft-CCD prediction distance for this rigid-body.
+    ///
+    /// See the documentation of [`RigidBody::set_soft_ccd_prediction`] for additional details on
+    /// soft-CCD.
+    pub fn soft_ccd_prediction(&self) -> Real {
+        self.ccd.soft_ccd_prediction
+    }
+
     // This is different from `is_ccd_enabled`. This checks that CCD
     // is active for this rigid-body, i.e., if it was seen to move fast
     // enough to justify a CCD run.
@@ -810,6 +887,25 @@ impl RigidBody {
         }
     }
 
+    /// Predicts the next position of this rigid-body, by integrating its velocity and forces
+    /// by a time of `dt`.
+    pub(crate) fn predict_position_using_velocity_and_forces_with_max_dist(
+        &self,
+        dt: Real,
+        max_dist: Real,
+    ) -> Isometry<Real> {
+        let new_vels = self.forces.integrate(dt, &self.vels, &self.mprops);
+        // Compute the clamped dt such that the body doesn’t travel more than `max_dist`.
+        let linvel_norm = new_vels.linvel.norm();
+        let clamped_linvel = linvel_norm.min(max_dist * crate::utils::inv(dt));
+        let clamped_dt = dt * clamped_linvel * crate::utils::inv(linvel_norm);
+        new_vels.integrate(
+            clamped_dt,
+            &self.pos.position,
+            &self.mprops.local_mprops.local_com,
+        )
+    }
+
     /// Predicts the next position of this rigid-body, by integrating its velocity and forces
     /// by a time of `dt`.
     pub fn predict_position_using_velocity_and_forces(&self, dt: Real) -> Isometry<Real> {
@@ -817,6 +913,17 @@ impl RigidBody {
             .integrate_forces_and_velocities(dt, &self.forces, &self.vels, &self.mprops)
     }
 
+    /// Predicts the next position of this rigid-body, by integrating only its velocity
+    /// by a time of `dt`.
+    ///
+    /// The forces that were applied to this rigid-body since the last physics step will
+    /// be ignored by this function. Use [`Self::predict_position_using_velocity_and_forces`]
+    /// instead to take forces into account.
+    pub fn predict_position_using_velocity(&self, dt: Real) -> Isometry<Real> {
+        self.vels
+            .integrate(dt, &self.pos.position, &self.mprops.local_mprops.local_com)
+    }
+
     pub(crate) fn update_world_mass_properties(&mut self) {
         self.mprops.update_world_mass_properties(&self.pos.position);
     }
@@ -1031,14 +1138,25 @@ pub struct RigidBodyBuilder {
     mprops_flags: LockedAxes,
     /// The additional mass-properties of the rigid-body being built. See [`RigidBodyBuilder::additional_mass_properties`] for more information.
     additional_mass_properties: RigidBodyAdditionalMassProps,
-    /// Whether or not the rigid-body to be created can sleep if it reaches a dynamic equilibrium.
+    /// Whether the rigid-body to be created can sleep if it reaches a dynamic equilibrium.
     pub can_sleep: bool,
-    /// Whether or not the rigid-body is to be created asleep.
+    /// Whether the rigid-body is to be created asleep.
     pub sleeping: bool,
-    /// Whether continuous collision-detection is enabled for the rigid-body to be built.
+    /// Whether Continuous Collision-Detection is enabled for the rigid-body to be built.
     ///
     /// CCD prevents tunneling, but may still allow limited interpenetration of colliders.
     pub ccd_enabled: bool,
+    /// The maximum prediction distance Soft Continuous Collision-Detection.
+    ///
+    /// When set to 0, soft CCD is disabled. Soft-CCD helps prevent tunneling especially of
+    /// slow-but-thin to moderately fast objects. The soft CCD prediction distance indicates how
+    /// far in the object’s path the CCD algorithm is allowed to inspect. Large values can impact
+    /// performance badly by increasing the work needed from the broad-phase.
+    ///
+    /// It is a generally cheaper variant of regular CCD (that can be enabled with
+    /// [`RigidBodyBuilder::ccd_enabled`] since it relies on predictive constraints instead of
+    /// shape-cast and substeps.
+    pub soft_ccd_prediction: Real,
     /// The dominance group of the rigid-body to be built.
     pub dominance_group: i8,
     /// Will the rigid-body being built be enabled?
@@ -1068,6 +1186,7 @@ impl RigidBodyBuilder {
             can_sleep: true,
             sleeping: false,
             ccd_enabled: false,
+            soft_ccd_prediction: 0.0,
             dominance_group: 0,
             enabled: true,
             user_data: 0,
@@ -1306,13 +1425,13 @@ impl RigidBodyBuilder {
         self
     }
 
-    /// Sets whether or not the rigid-body to be created can sleep if it reaches a dynamic equilibrium.
+    /// Sets whether 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
     }
 
-    /// Sets whether or not continuous collision-detection is enabled for this rigid-body.
+    /// Sets whether Continuous Collision-Detection is enabled for this rigid-body.
     ///
     /// CCD prevents tunneling, but may still allow limited interpenetration of colliders.
     pub fn ccd_enabled(mut self, enabled: bool) -> Self {
@@ -1320,7 +1439,22 @@ impl RigidBodyBuilder {
         self
     }
 
-    /// Sets whether or not the rigid-body is to be created asleep.
+    /// Sets the maximum prediction distance Soft Continuous Collision-Detection.
+    ///
+    /// When set to 0, soft-CCD is disabled. Soft-CCD helps prevent tunneling especially of
+    /// slow-but-thin to moderately fast objects. The soft CCD prediction distance indicates how
+    /// far in the object’s path the CCD algorithm is allowed to inspect. Large values can impact
+    /// performance badly by increasing the work needed from the broad-phase.
+    ///
+    /// It is a generally cheaper variant of regular CCD (that can be enabled with
+    /// [`RigidBodyBuilder::ccd_enabled`] since it relies on predictive constraints instead of
+    /// shape-cast and substeps.
+    pub fn soft_ccd_prediction(mut self, prediction_distance: Real) -> Self {
+        self.soft_ccd_prediction = prediction_distance;
+        self
+    }
+
+    /// Sets whether the rigid-body is to be created asleep.
     pub fn sleeping(mut self, sleeping: bool) -> Self {
         self.sleeping = sleeping;
         self
@@ -1357,13 +1491,14 @@ impl RigidBodyBuilder {
         rb.dominance = RigidBodyDominance(self.dominance_group);
         rb.enabled = self.enabled;
         rb.enable_ccd(self.ccd_enabled);
+        rb.set_soft_ccd_prediction(self.soft_ccd_prediction);
 
         if self.can_sleep && self.sleeping {
             rb.sleep();
         }
 
         if !self.can_sleep {
-            rb.activation.linear_threshold = -1.0;
+            rb.activation.normalized_linear_threshold = -1.0;
             rb.activation.angular_threshold = -1.0;
         }
 
diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs
index 2291742..a391feb 100644
--- a/src/dynamics/rigid_body_components.rs
+++ b/src/dynamics/rigid_body_components.rs
@@ -571,10 +571,11 @@ impl RigidBodyVelocity {
     /// The approximate kinetic energy of this rigid-body.
     ///
     /// This approximation does not take the rigid-body's mass and angular inertia
-    /// into account.
+    /// into account. Some physics engines call this the "mass-normalized kinetic
+    /// energy".
     #[must_use]
     pub fn pseudo_kinetic_energy(&self) -> Real {
-        self.linvel.norm_squared() + self.angvel.gdot(self.angvel)
+        0.5 * (self.linvel.norm_squared() + self.angvel.gdot(self.angvel))
     }
 
     /// Returns the update velocities after applying the given damping.
@@ -594,7 +595,7 @@ impl RigidBodyVelocity {
     }
 
     /// Integrate the velocities in `self` to compute obtain new positions when moving from the given
-    /// inital position `init_pos`.
+    /// initial position `init_pos`.
     #[must_use]
     pub fn integrate(
         &self,
@@ -821,6 +822,8 @@ pub struct RigidBodyCcd {
     pub ccd_active: bool,
     /// Is CCD enabled for this rigid-body?
     pub ccd_enabled: bool,
+    /// The soft-CCD prediction distance for this rigid-body.
+    pub soft_ccd_prediction: Real,
 }
 
 impl Default for RigidBodyCcd {
@@ -830,6 +833,7 @@ impl Default for RigidBodyCcd {
             ccd_max_dist: 0.0,
             ccd_active: false,
             ccd_enabled: false,
+            soft_ccd_prediction: 0.0,
         }
     }
 }
@@ -992,7 +996,10 @@ impl RigidBodyDominance {
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 pub struct RigidBodyActivation {
     /// The threshold linear velocity bellow which the body can fall asleep.
-    pub linear_threshold: Real,
+    ///
+    /// The value is "normalized", i.e., the actual threshold applied by the physics engine
+    /// is equal to this value multiplied by [`IntegrationParameters::length_unit`].
+    pub normalized_linear_threshold: Real,
     /// The angular linear velocity bellow which the body can fall asleep.
     pub angular_threshold: Real,
     /// The amount of time the rigid-body must remain below the thresholds to be put to sleep.
@@ -1011,7 +1018,7 @@ impl Default for RigidBodyActivation {
 
 impl RigidBodyActivation {
     /// The default linear velocity bellow which a body can be put to sleep.
-    pub fn default_linear_threshold() -> Real {
+    pub fn default_normalized_linear_threshold() -> Real {
         0.4
     }
 
@@ -1029,7 +1036,7 @@ impl RigidBodyActivation {
     /// Create a new rb_activation status initialised with the default rb_activation threshold and is active.
     pub fn active() -> Self {
         RigidBodyActivation {
-            linear_threshold: Self::default_linear_threshold(),
+            normalized_linear_threshold: Self::default_normalized_linear_threshold(),
             angular_threshold: Self::default_angular_threshold(),
             time_until_sleep: Self::default_time_until_sleep(),
             time_since_can_sleep: 0.0,
@@ -1040,7 +1047,7 @@ impl RigidBodyActivation {
     /// Create a new rb_activation status initialised with the default rb_activation threshold and is inactive.
     pub fn inactive() -> Self {
         RigidBodyActivation {
-            linear_threshold: Self::default_linear_threshold(),
+            normalized_linear_threshold: Self::default_normalized_linear_threshold(),
             angular_threshold: Self::default_angular_threshold(),
             time_until_sleep: Self::default_time_until_sleep(),
             time_since_can_sleep: Self::default_time_until_sleep(),
@@ -1051,7 +1058,7 @@ impl RigidBodyActivation {
     /// Create a new activation status that prevents the rigid-body from sleeping.
     pub fn cannot_sleep() -> Self {
         RigidBodyActivation {
-            linear_threshold: -1.0,
+            normalized_linear_threshold: -1.0,
             angular_threshold: -1.0,
             ..Self::active()
         }
diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs
index 2356364..a7d2ec3 100644
--- a/src/dynamics/rigid_body_set.rs
+++ b/src/dynamics/rigid_body_set.rs
@@ -209,7 +209,7 @@ impl RigidBodySet {
     /// Update colliders positions after rigid-bodies moved.
     ///
     /// When a rigid-body moves, the positions of the colliders attached to it need to be updated.
-    /// This update is generally automatically done at the beggining and the end of each simulation
+    /// This update is generally automatically done at the beginning and the end of each simulation
     /// step with `PhysicsPipeline::step`. If the positions need to be updated without running a
     /// simulation step (for example when using the `QueryPipeline` alone), this method can be called
     /// manually.  
diff --git a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs
index b5fdb6d..6809c37 100644
--- a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs
+++ b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs
@@ -25,6 +25,7 @@ use {
     crate::math::SIMD_WIDTH,
 };
 
+#[derive(Debug)]
 pub struct ConstraintsCounts {
     pub num_constraints: usize,
     pub num_jacobian_lines: usize,
@@ -441,6 +442,17 @@ impl ContactConstraintsSet {
         assert_eq!(curr_start, total_num_constraints);
     }
 
+    pub fn warmstart(
+        &mut self,
+        solver_vels: &mut [SolverVel<Real>],
+        generic_solver_vels: &mut DVector<Real>,
+    ) {
+        let (jac, constraints) = self.iter_constraints_mut();
+        for mut c in constraints {
+            c.warmstart(jac, solver_vels, generic_solver_vels);
+        }
+    }
+
     pub fn solve_restitution(
         &mut self,
         solver_vels: &mut [SolverVel<Real>],
diff --git a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
index 7b1f8ea..e254995 100644
--- a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
@@ -153,8 +153,9 @@ impl GenericOneBodyConstraintBuilder {
                         rhs: na::zero(),
                         rhs_wo_bias: na::zero(),
                         impulse: na::zero(),
-                        total_impulse: na::zero(),
+                        impulse_accumulator: na::zero(),
                         r,
+                        r_mat_elts: [0.0; 2],
                     };
                 }
 
@@ -230,13 +231,8 @@ impl GenericOneBodyConstraintBuilder {
             .unwrap()
             .local_to_world;
 
-        self.inner.update_with_positions(
-            params,
-            solved_dt,
-            pos2,
-            self.ccd_thickness,
-            &mut constraint.inner,
-        );
+        self.inner
+            .update_with_positions(params, solved_dt, pos2, &mut constraint.inner);
     }
 }
 
@@ -258,6 +254,24 @@ impl GenericOneBodyConstraint {
         }
     }
 
+    pub fn warmstart(
+        &mut self,
+        jacobians: &DVector<Real>,
+        generic_solver_vels: &mut DVector<Real>,
+    ) {
+        let solver_vel2 = self.inner.solver_vel2;
+
+        let elements = &mut self.inner.elements[..self.inner.num_contacts as usize];
+        OneBodyConstraintElement::generic_warmstart_group(
+            elements,
+            jacobians,
+            self.ndofs2,
+            self.j_id,
+            solver_vel2,
+            generic_solver_vels,
+        );
+    }
+
     pub fn solve(
         &mut self,
         jacobians: &DVector<Real>,
diff --git a/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs
index 1003a9a..4928f36 100644
--- a/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs
+++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs
@@ -7,6 +7,40 @@ use na::DVector;
 use na::SimdPartialOrd;
 
 impl OneBodyConstraintTangentPart<Real> {
+    #[inline]
+    pub fn generic_warmstart(
+        &mut self,
+        j_id2: usize,
+        jacobians: &DVector<Real>,
+        ndofs2: usize,
+        solver_vel2: usize,
+        solver_vels: &mut DVector<Real>,
+    ) {
+        #[cfg(feature = "dim2")]
+        {
+            solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
+                self.impulse[0],
+                &jacobians.rows(j_id2 + ndofs2, ndofs2),
+                1.0,
+            );
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            let j_step = ndofs2 * 2;
+            solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
+                self.impulse[0],
+                &jacobians.rows(j_id2 + ndofs2, ndofs2),
+                1.0,
+            );
+            solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
+                self.impulse[1],
+                &jacobians.rows(j_id2 + j_step + ndofs2, ndofs2),
+                1.0,
+            );
+        }
+    }
+
     #[inline]
     pub fn generic_solve(
         &mut self,
@@ -71,6 +105,22 @@ impl OneBodyConstraintTangentPart<Real> {
 }
 
 impl OneBodyConstraintNormalPart<Real> {
+    #[inline]
+    pub fn generic_warmstart(
+        &mut self,
+        j_id2: usize,
+        jacobians: &DVector<Real>,
+        ndofs2: usize,
+        solver_vel2: usize,
+        solver_vels: &mut DVector<Real>,
+    ) {
+        solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
+            self.impulse,
+            &jacobians.rows(j_id2 + ndofs2, ndofs2),
+            1.0,
+        );
+    }
+
     #[inline]
     pub fn generic_solve(
         &mut self,
@@ -99,6 +149,42 @@ impl OneBodyConstraintNormalPart<Real> {
 }
 
 impl OneBodyConstraintElement<Real> {
+    #[inline]
+    pub fn generic_warmstart_group(
+        elements: &mut [Self],
+        jacobians: &DVector<Real>,
+        ndofs2: usize,
+        // Jacobian index of the first constraint.
+        j_id: usize,
+        solver_vel2: usize,
+        solver_vels: &mut DVector<Real>,
+    ) {
+        let j_step = ndofs2 * 2 * DIM;
+
+        // Solve penetration.
+        let mut nrm_j_id = j_id;
+
+        for element in elements.iter_mut() {
+            element.normal_part.generic_warmstart(
+                nrm_j_id,
+                jacobians,
+                ndofs2,
+                solver_vel2,
+                solver_vels,
+            );
+            nrm_j_id += j_step;
+        }
+
+        // Solve friction.
+        let mut tng_j_id = j_id + ndofs2 * 2;
+
+        for element in elements.iter_mut() {
+            let part = &mut element.tangent_part;
+            part.generic_warmstart(tng_j_id, jacobians, ndofs2, solver_vel2, solver_vels);
+            tng_j_id += j_step;
+        }
+    }
+
     #[inline]
     pub fn generic_solve_group(
         cfm_factor: Real,
diff --git a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
index 073f585..dd63d1f 100644
--- a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
@@ -201,15 +201,17 @@ impl GenericTwoBodyConstraintBuilder {
                         gcross2,
                         rhs: na::zero(),
                         rhs_wo_bias: na::zero(),
-                        total_impulse: na::zero(),
-                        impulse: na::zero(),
+                        impulse_accumulator: na::zero(),
+                        impulse: manifold_point.warmstart_impulse,
                         r,
+                        r_mat_elts: [0.0; 2],
                     };
                 }
 
                 // Tangent parts.
                 {
-                    constraint.inner.elements[k].tangent_part.impulse = na::zero();
+                    constraint.inner.elements[k].tangent_part.impulse =
+                        manifold_point.warmstart_tangent_impulse;
 
                     for j in 0..DIM - 1 {
                         let torque_dir1 = dp1.gcross(tangents1[j]);
@@ -340,14 +342,8 @@ impl GenericTwoBodyConstraintBuilder {
             .map(|m| &multibodies[m.multibody].link(m.id).unwrap().local_to_world)
             .unwrap_or_else(|| &bodies[constraint.inner.solver_vel2].position);
 
-        self.inner.update_with_positions(
-            params,
-            solved_dt,
-            pos1,
-            pos2,
-            self.ccd_thickness,
-            &mut constraint.inner,
-        );
+        self.inner
+            .update_with_positions(params, solved_dt, pos1, pos2, &mut constraint.inner);
     }
 }
 
@@ -373,6 +369,50 @@ impl GenericTwoBodyConstraint {
         }
     }
 
+    pub fn warmstart(
+        &mut self,
+        jacobians: &DVector<Real>,
+        solver_vels: &mut [SolverVel<Real>],
+        generic_solver_vels: &mut DVector<Real>,
+    ) {
+        let mut solver_vel1 = if self.generic_constraint_mask & 0b01 == 0 {
+            GenericRhs::SolverVel(solver_vels[self.inner.solver_vel1])
+        } else {
+            GenericRhs::GenericId(self.inner.solver_vel1)
+        };
+
+        let mut solver_vel2 = if self.generic_constraint_mask & 0b10 == 0 {
+            GenericRhs::SolverVel(solver_vels[self.inner.solver_vel2])
+        } else {
+            GenericRhs::GenericId(self.inner.solver_vel2)
+        };
+
+        let elements = &mut self.inner.elements[..self.inner.num_contacts as usize];
+        TwoBodyConstraintElement::generic_warmstart_group(
+            elements,
+            jacobians,
+            &self.inner.dir1,
+            #[cfg(feature = "dim3")]
+            &self.inner.tangent1,
+            &self.inner.im1,
+            &self.inner.im2,
+            self.ndofs1,
+            self.ndofs2,
+            self.j_id,
+            &mut solver_vel1,
+            &mut solver_vel2,
+            generic_solver_vels,
+        );
+
+        if let GenericRhs::SolverVel(solver_vel1) = solver_vel1 {
+            solver_vels[self.inner.solver_vel1] = solver_vel1;
+        }
+
+        if let GenericRhs::SolverVel(solver_vel2) = solver_vel2 {
+            solver_vels[self.inner.solver_vel2] = solver_vel2;
+        }
+    }
+
     pub fn solve(
         &mut self,
         jacobians: &DVector<Real>,
diff --git a/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs
index 40740a0..389b603 100644
--- a/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs
+++ b/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs
@@ -88,6 +88,95 @@ impl GenericRhs {
 }
 
 impl TwoBodyConstraintTangentPart<Real> {
+    #[inline]
+    pub fn generic_warmstart(
+        &mut self,
+        j_id: usize,
+        jacobians: &DVector<Real>,
+        tangents1: [&Vector<Real>; DIM - 1],
+        im1: &Vector<Real>,
+        im2: &Vector<Real>,
+        ndofs1: usize,
+        ndofs2: usize,
+        solver_vel1: &mut GenericRhs,
+        solver_vel2: &mut GenericRhs,
+        solver_vels: &mut DVector<Real>,
+    ) {
+        let j_id1 = j_id1(j_id, ndofs1, ndofs2);
+        let j_id2 = j_id2(j_id, ndofs1, ndofs2);
+        #[cfg(feature = "dim3")]
+        let j_step = j_step(ndofs1, ndofs2);
+
+        #[cfg(feature = "dim2")]
+        {
+            solver_vel1.apply_impulse(
+                j_id1,
+                ndofs1,
+                self.impulse[0],
+                jacobians,
+                tangents1[0],
+                &self.gcross1[0],
+                solver_vels,
+                im1,
+            );
+            solver_vel2.apply_impulse(
+                j_id2,
+                ndofs2,
+                self.impulse[0],
+                jacobians,
+                &-tangents1[0],
+                &self.gcross2[0],
+                solver_vels,
+                im2,
+            );
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            solver_vel1.apply_impulse(
+                j_id1,
+                ndofs1,
+                self.impulse[0],
+                jacobians,
+                tangents1[0],
+                &self.gcross1[0],
+                solver_vels,
+                im1,
+            );
+            solver_vel1.apply_impulse(
+                j_id1 + j_step,
+                ndofs1,
+                self.impulse[1],
+                jacobians,
+                tangents1[1],
+                &self.gcross1[1],
+                solver_vels,
+                im1,
+            );
+
+            solver_vel2.apply_impulse(
+                j_id2,
+                ndofs2,
+                self.impulse[0],
+                jacobians,
+                &-tangents1[0],
+                &self.gcross2[0],
+                solver_vels,
+                im2,
+            );
+            solver_vel2.apply_impulse(
+                j_id2 + j_step,
+                ndofs2,
+                self.impulse[1],
+                jacobians,
+                &-tangents1[1],
+                &self.gcross2[1],
+                solver_vels,
+                im2,
+            );
+        }
+    }
+
     #[inline]
     pub fn generic_solve(
         &mut self,
@@ -240,6 +329,45 @@ impl TwoBodyConstraintTangentPart<Real> {
 }
 
 impl TwoBodyConstraintNormalPart<Real> {
+    #[inline]
+    pub fn generic_warmstart(
+        &mut self,
+        j_id: usize,
+        jacobians: &DVector<Real>,
+        dir1: &Vector<Real>,
+        im1: &Vector<Real>,
+        im2: &Vector<Real>,
+        ndofs1: usize,
+        ndofs2: usize,
+        solver_vel1: &mut GenericRhs,
+        solver_vel2: &mut GenericRhs,
+        solver_vels: &mut DVector<Real>,
+    ) {
+        let j_id1 = j_id1(j_id, ndofs1, ndofs2);
+        let j_id2 = j_id2(j_id, ndofs1, ndofs2);
+
+        solver_vel1.apply_impulse(
+            j_id1,
+            ndofs1,
+            self.impulse,
+            jacobians,
+            dir1,
+            &self.gcross1,
+            solver_vels,
+            im1,
+        );
+        solver_vel2.apply_impulse(
+            j_id2,
+            ndofs2,
+            self.impulse,
+            jacobians,
+            &-dir1,
+            &self.gcross2,
+            solver_vels,
+            im2,
+        );
+    }
+
     #[inline]
     pub fn generic_solve(
         &mut self,
@@ -290,6 +418,74 @@ impl TwoBodyConstraintNormalPart<Real> {
 }
 
 impl TwoBodyConstraintElement<Real> {
+    #[inline]
+    pub fn generic_warmstart_group(
+        elements: &mut [Self],
+        jacobians: &DVector<Real>,
+        dir1: &Vector<Real>,
+        #[cfg(feature = "dim3")] tangent1: &Vector<Real>,
+        im1: &Vector<Real>,
+        im2: &Vector<Real>,
+        // ndofs is 0 for a non-multibody body, or a multibody with zero
+        // degrees of freedom.
+        ndofs1: usize,
+        ndofs2: usize,
+        // Jacobian index of the first constraint.
+        j_id: usize,
+        solver_vel1: &mut GenericRhs,
+        solver_vel2: &mut GenericRhs,
+        solver_vels: &mut DVector<Real>,
+    ) {
+        let j_step = j_step(ndofs1, ndofs2) * DIM;
+
+        // Solve penetration.
+        {
+            let mut nrm_j_id = normal_j_id(j_id, ndofs1, ndofs2);
+
+            for element in elements.iter_mut() {
+                element.normal_part.generic_warmstart(
+                    nrm_j_id,
+                    jacobians,
+                    dir1,
+                    im1,
+                    im2,
+                    ndofs1,
+                    ndofs2,
+                    solver_vel1,
+                    solver_vel2,
+                    solver_vels,
+                );
+                nrm_j_id += j_step;
+            }
+        }
+
+        // Solve friction.
+        {
+            #[cfg(feature = "dim3")]
+            let tangents1 = [tangent1, &dir1.cross(tangent1)];
+            #[cfg(feature = "dim2")]
+            let tangents1 = [&dir1.orthonormal_vector()];
+            let mut tng_j_id = tangent_j_id(j_id, ndofs1, ndofs2);
+
+            for element in elements.iter_mut() {
+                let part = &mut element.tangent_part;
+                part.generic_warmstart(
+                    tng_j_id,
+                    jacobians,
+                    tangents1,
+                    im1,
+                    im2,
+                    ndofs1,
+                    ndofs2,
+                    solver_vel1,
+                    solver_vel2,
+                    solver_vels,
+                );
+                tng_j_id += j_step;
+            }
+        }
+    }
+
     #[inline]
     pub fn generic_solve_group(
         cfm_factor: Real,
diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint.rs b/src/dynamics/solver/contact_constraint/one_body_constraint.rs
index be108a4..5e6e86b 100644
--- a/src/dynamics/solver/contact_constraint/one_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/one_body_constraint.rs
@@ -3,8 +3,10 @@ use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
 #[cfg(feature = "dim2")]
 use crate::utils::SimdBasis;
 use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot, SimdRealCopy};
+use na::Matrix2;
 use parry::math::Isometry;
 
+use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
 use crate::dynamics::solver::solver_body::SolverBody;
 use crate::dynamics::solver::SolverVel;
 use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet, RigidBodyVelocity};
@@ -130,10 +132,11 @@ impl OneBodyConstraintBuilder {
                         .effective_world_inv_inertia_sqrt
                         .transform_vector(dp2.gcross(-force_dir1));
 
-                    let projected_mass = utils::inv(
-                        force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
-                            + gcross2.gdot(gcross2),
-                    );
+                    let projected_lin_mass =
+                        force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1));
+                    let projected_ang_mass = gcross2.gdot(gcross2);
+
+                    let projected_mass = utils::inv(projected_lin_mass + projected_ang_mass);
 
                     let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
 
@@ -148,15 +151,17 @@ impl OneBodyConstraintBuilder {
                         gcross2,
                         rhs: na::zero(),
                         rhs_wo_bias: na::zero(),
-                        impulse: na::zero(),
-                        total_impulse: na::zero(),
+                        impulse: manifold_point.warmstart_impulse,
+                        impulse_accumulator: na::zero(),
                         r: projected_mass,
+                        r_mat_elts: [0.0; 2],
                     };
                 }
 
                 // Tangent parts.
                 {
-                    constraint.elements[k].tangent_part.impulse = na::zero();
+                    constraint.elements[k].tangent_part.impulse =
+                        manifold_point.warmstart_tangent_impulse;
 
                     for j in 0..DIM - 1 {
                         let gcross2 = mprops2
@@ -205,6 +210,44 @@ impl OneBodyConstraintBuilder {
                     builder.infos[k] = infos;
                 }
             }
+
+            if BLOCK_SOLVER_ENABLED {
+                // Coupling between consecutive pairs.
+                for k in 0..manifold_points.len() / 2 {
+                    let k0 = k * 2;
+                    let k1 = k * 2 + 1;
+
+                    let mut r_mat = Matrix2::zeros();
+                    let r0 = constraint.elements[k0].normal_part.r;
+                    let r1 = constraint.elements[k1].normal_part.r;
+                    r_mat.m12 = force_dir1
+                        .dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+                        + constraint.elements[k0]
+                            .normal_part
+                            .gcross2
+                            .gdot(constraint.elements[k1].normal_part.gcross2);
+                    r_mat.m21 = r_mat.m12;
+                    r_mat.m11 = utils::inv(r0);
+                    r_mat.m22 = utils::inv(r1);
+
+                    if let Some(inv) = r_mat.try_inverse() {
+                        constraint.elements[k0].normal_part.r_mat_elts = [inv.m11, inv.m22];
+                        constraint.elements[k1].normal_part.r_mat_elts = [inv.m12, r_mat.m12];
+                    } else {
+                        // If inversion failed, the contacts are redundant.
+                        // Ignore the one with the smallest depth (it is too late to
+                        // have the constraint removed from the constraint set, so just
+                        // set the mass (r) matrix elements to 0.
+                        constraint.elements[k0].normal_part.r_mat_elts =
+                            if manifold_points[k0].dist <= manifold_points[k1].dist {
+                                [r0, 0.0]
+                            } else {
+                                [0.0, r1]
+                            };
+                        constraint.elements[k1].normal_part.r_mat_elts = [0.0; 2];
+                    }
+                }
+            }
         }
     }
 
@@ -217,13 +260,7 @@ impl OneBodyConstraintBuilder {
         constraint: &mut OneBodyConstraint,
     ) {
         let rb2 = &bodies[constraint.solver_vel2];
-        self.update_with_positions(
-            params,
-            solved_dt,
-            &rb2.position,
-            rb2.ccd_thickness,
-            constraint,
-        )
+        self.update_with_positions(params, solved_dt, &rb2.position, constraint)
     }
 
     // TODO: this code is SOOOO similar to TwoBodyConstraint::update.
@@ -233,12 +270,11 @@ impl OneBodyConstraintBuilder {
         params: &IntegrationParameters,
         solved_dt: Real,
         rb2_pos: &Isometry<Real>,
-        ccd_thickness: Real,
         constraint: &mut OneBodyConstraint,
     ) {
-        let cfm_factor = params.cfm_factor();
+        let cfm_factor = params.contact_cfm_factor();
         let inv_dt = params.inv_dt();
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = params.contact_erp_inv_dt();
 
         let all_infos = &self.infos[..constraint.num_contacts as usize];
         let all_elements = &mut constraint.elements[..constraint.num_contacts as usize];
@@ -247,7 +283,6 @@ impl OneBodyConstraintBuilder {
         let new_pos1 = self
             .vels1
             .integrate(solved_dt, &rb1.position, &rb1.local_com);
-        let mut is_fast_contact = false;
 
         #[cfg(feature = "dim2")]
         let tangents1 = constraint.dir1.orthonormal_basis();
@@ -266,23 +301,20 @@ impl OneBodyConstraintBuilder {
             // Normal part.
             {
                 let rhs_wo_bias = info.normal_rhs_wo_bias + dist.max(0.0) * inv_dt;
-                let rhs_bias = erp_inv_dt
-                    * (dist + params.allowed_linear_error)
-                        .clamp(-params.max_penetration_correction, 0.0);
+                let rhs_bias = (erp_inv_dt * (dist + params.allowed_linear_error()))
+                    .clamp(-params.max_corrective_velocity(), 0.0);
                 let new_rhs = rhs_wo_bias + rhs_bias;
-                let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse;
-                is_fast_contact = is_fast_contact || (-new_rhs * params.dt > ccd_thickness * 0.5);
 
                 element.normal_part.rhs_wo_bias = rhs_wo_bias;
                 element.normal_part.rhs = new_rhs;
-                element.normal_part.total_impulse = total_impulse;
-                element.normal_part.impulse = na::zero();
+                element.normal_part.impulse_accumulator += element.normal_part.impulse;
+                element.normal_part.impulse *= params.warmstart_coefficient;
             }
 
             // Tangent part.
             {
-                element.tangent_part.total_impulse += element.tangent_part.impulse;
-                element.tangent_part.impulse = na::zero();
+                element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
+                element.tangent_part.impulse *= params.warmstart_coefficient;
 
                 for j in 0..DIM - 1 {
                     let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
@@ -291,7 +323,7 @@ impl OneBodyConstraintBuilder {
             }
         }
 
-        constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
+        constraint.cfm_factor = cfm_factor;
     }
 }
 
@@ -328,6 +360,21 @@ impl OneBodyConstraint {
         }
     }
 
+    pub fn warmstart(&mut self, solver_vels: &mut [SolverVel<Real>]) {
+        let mut solver_vel2 = solver_vels[self.solver_vel2];
+
+        OneBodyConstraintElement::warmstart_group(
+            &mut self.elements[..self.num_contacts as usize],
+            &self.dir1,
+            #[cfg(feature = "dim3")]
+            &self.tangent1,
+            &self.im2,
+            &mut solver_vel2,
+        );
+
+        solver_vels[self.solver_vel2] = solver_vel2;
+    }
+
     pub fn solve(
         &mut self,
         solver_vels: &mut [SolverVel<Real>],
@@ -359,16 +406,11 @@ impl OneBodyConstraint {
         for k in 0..self.num_contacts as usize {
             let contact_id = self.manifold_contact_id[k];
             let active_contact = &mut manifold.points[contact_id as usize];
-            active_contact.data.impulse = self.elements[k].normal_part.impulse;
 
-            #[cfg(feature = "dim2")]
-            {
-                active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0];
-            }
-            #[cfg(feature = "dim3")]
-            {
-                active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse;
-            }
+            active_contact.data.warmstart_impulse = self.elements[k].normal_part.impulse;
+            active_contact.data.warmstart_tangent_impulse = self.elements[k].tangent_part.impulse;
+            active_contact.data.impulse = self.elements[k].normal_part.total_impulse();
+            active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse();
         }
     }
 
diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs
index d9ff7f4..7ec8c5d 100644
--- a/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs
+++ b/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs
@@ -1,20 +1,17 @@
+use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
+use crate::dynamics::solver::contact_constraint::TwoBodyConstraintNormalPart;
 use crate::dynamics::solver::SolverVel;
-use crate::math::{AngVector, Vector, DIM};
+use crate::math::{AngVector, TangentImpulse, Vector, DIM};
 use crate::utils::{SimdBasis, SimdDot, SimdRealCopy};
+use na::Vector2;
 
 #[derive(Copy, Clone, Debug)]
 pub(crate) struct OneBodyConstraintTangentPart<N: SimdRealCopy> {
     pub gcross2: [AngVector<N>; DIM - 1],
     pub rhs: [N; DIM - 1],
     pub rhs_wo_bias: [N; DIM - 1],
-    #[cfg(feature = "dim2")]
-    pub impulse: na::Vector1<N>,
-    #[cfg(feature = "dim3")]
-    pub impulse: na::Vector2<N>,
-    #[cfg(feature = "dim2")]
-    pub total_impulse: na::Vector1<N>,
-    #[cfg(feature = "dim3")]
-    pub total_impulse: na::Vector2<N>,
+    pub impulse: TangentImpulse<N>,
+    pub impulse_accumulator: TangentImpulse<N>,
     #[cfg(feature = "dim2")]
     pub r: [N; 1],
     #[cfg(feature = "dim3")]
@@ -28,7 +25,7 @@ impl<N: SimdRealCopy> OneBodyConstraintTangentPart<N> {
             rhs: [na::zero(); DIM - 1],
             rhs_wo_bias: [na::zero(); DIM - 1],
             impulse: na::zero(),
-            total_impulse: na::zero(),
+            impulse_accumulator: na::zero(),
             #[cfg(feature = "dim2")]
             r: [na::zero(); 1],
             #[cfg(feature = "dim3")]
@@ -36,41 +33,31 @@ impl<N: SimdRealCopy> OneBodyConstraintTangentPart<N> {
         }
     }
 
+    /// Total impulse applied across all the solver substeps.
     #[inline]
-    pub fn apply_limit(
+    pub fn total_impulse(&self) -> TangentImpulse<N> {
+        self.impulse_accumulator + self.impulse
+    }
+
+    #[inline]
+    pub fn warmstart(
         &mut self,
         tangents1: [&Vector<N>; DIM - 1],
         im2: &Vector<N>,
-        limit: N,
         solver_vel2: &mut SolverVel<N>,
-    ) where
-        AngVector<N>: SimdDot<AngVector<N>, Result = N>,
-    {
+    ) {
         #[cfg(feature = "dim2")]
         {
-            let new_impulse = self.impulse[0].simd_clamp(-limit, limit);
-            let dlambda = new_impulse - self.impulse[0];
-            self.impulse[0] = new_impulse;
-
-            solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda;
-            solver_vel2.angular += self.gcross2[0] * dlambda;
+            solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0];
+            solver_vel2.angular += self.gcross2[0] * self.impulse[0];
         }
 
         #[cfg(feature = "dim3")]
         {
-            let new_impulse = self.impulse;
-            let new_impulse = {
-                let _disable_fe_except =
-                    crate::utils::DisableFloatingPointExceptionsFlags::
-                    disable_floating_point_exceptions();
-                new_impulse.simd_cap_magnitude(limit)
-            };
-            let dlambda = new_impulse - self.impulse;
-            self.impulse = new_impulse;
-
-            solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
-                + tangents1[1].component_mul(im2) * -dlambda[1];
-            solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
+            solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0]
+                + tangents1[1].component_mul(im2) * -self.impulse[1];
+            solver_vel2.angular +=
+                self.gcross2[0] * self.impulse[0] + self.gcross2[1] * self.impulse[1];
         }
     }
 
@@ -137,8 +124,9 @@ pub(crate) struct OneBodyConstraintNormalPart<N: SimdRealCopy> {
     pub rhs: N,
     pub rhs_wo_bias: N,
     pub impulse: N,
-    pub total_impulse: N,
+    pub impulse_accumulator: N,
     pub r: N,
+    pub r_mat_elts: [N; 2],
 }
 
 impl<N: SimdRealCopy> OneBodyConstraintNormalPart<N> {
@@ -148,11 +136,24 @@ impl<N: SimdRealCopy> OneBodyConstraintNormalPart<N> {
             rhs: na::zero(),
             rhs_wo_bias: na::zero(),
             impulse: na::zero(),
-            total_impulse: na::zero(),
+            impulse_accumulator: na::zero(),
             r: na::zero(),
+            r_mat_elts: [N::zero(); 2],
         }
     }
 
+    /// Total impulse applied across all the solver substeps.
+    #[inline]
+    pub fn total_impulse(&self) -> N {
+        self.impulse_accumulator + self.impulse
+    }
+
+    #[inline]
+    pub fn warmstart(&mut self, dir1: &Vector<N>, im2: &Vector<N>, solver_vel2: &mut SolverVel<N>) {
+        solver_vel2.linear += dir1.component_mul(im2) * -self.impulse;
+        solver_vel2.angular += self.gcross2 * self.impulse;
+    }
+
     #[inline]
     pub fn solve(
         &mut self,
@@ -172,6 +173,44 @@ impl<N: SimdRealCopy> OneBodyConstraintNormalPart<N> {
         solver_vel2.linear += dir1.component_mul(im2) * -dlambda;
         solver_vel2.angular += self.gcross2 * dlambda;
     }
+
+    #[inline]
+    pub fn solve_pair(
+        constraint_a: &mut Self,
+        constraint_b: &mut Self,
+        cfm_factor: N,
+        dir1: &Vector<N>,
+        im2: &Vector<N>,
+        solver_vel2: &mut SolverVel<N>,
+    ) where
+        AngVector<N>: SimdDot<AngVector<N>, Result = N>,
+    {
+        let dvel_a = -dir1.dot(&solver_vel2.linear)
+            + constraint_a.gcross2.gdot(solver_vel2.angular)
+            + constraint_a.rhs;
+        let dvel_b = -dir1.dot(&solver_vel2.linear)
+            + constraint_b.gcross2.gdot(solver_vel2.angular)
+            + constraint_b.rhs;
+
+        let prev_impulse = Vector2::new(constraint_a.impulse, constraint_b.impulse);
+        let new_impulse = TwoBodyConstraintNormalPart::solve_mlcp_two_constraints(
+            Vector2::new(dvel_a, dvel_b),
+            prev_impulse,
+            constraint_a.r,
+            constraint_b.r,
+            constraint_a.r_mat_elts,
+            constraint_b.r_mat_elts,
+            cfm_factor,
+        );
+
+        let dlambda = new_impulse - prev_impulse;
+
+        constraint_a.impulse = new_impulse.x;
+        constraint_b.impulse = new_impulse.y;
+
+        solver_vel2.linear += dir1.component_mul(im2) * (-dlambda.x - dlambda.y);
+        solver_vel2.angular += constraint_a.gcross2 * dlambda.x + constraint_b.gcross2 * dlambda.y;
+    }
 }
 
 #[derive(Copy, Clone, Debug)]
@@ -188,6 +227,25 @@ impl<N: SimdRealCopy> OneBodyConstraintElement<N> {
         }
     }
 
+    #[inline]
+    pub fn warmstart_group(
+        elements: &mut [Self],
+        dir1: &Vector<N>,
+        #[cfg(feature = "dim3")] tangent1: &Vector<N>,
+        im2: &Vector<N>,
+        solver_vel2: &mut SolverVel<N>,
+    ) {
+        #[cfg(feature = "dim3")]
+        let tangents1 = [tangent1, &dir1.cross(tangent1)];
+        #[cfg(feature = "dim2")]
+        let tangents1 = [&dir1.orthonormal_vector()];
+
+        for element in elements.iter_mut() {
+            element.normal_part.warmstart(dir1, im2, solver_vel2);
+            element.tangent_part.warmstart(tangents1, im2, solver_vel2);
+        }
+    }
+
     #[inline]
     pub fn solve_group(
         cfm_factor: N,
@@ -210,13 +268,34 @@ impl<N: SimdRealCopy> OneBodyConstraintElement<N> {
 
         // Solve penetration.
         if solve_normal {
-            for element in elements.iter_mut() {
-                element
-                    .normal_part
-                    .solve(cfm_factor, dir1, im2, solver_vel2);
-                let limit = limit * element.normal_part.impulse;
-                let part = &mut element.tangent_part;
-                part.apply_limit(tangents1, im2, limit, solver_vel2);
+            if BLOCK_SOLVER_ENABLED {
+                for elements in elements.chunks_exact_mut(2) {
+                    let [element_a, element_b] = elements else {
+                        unreachable!()
+                    };
+
+                    OneBodyConstraintNormalPart::solve_pair(
+                        &mut element_a.normal_part,
+                        &mut element_b.normal_part,
+                        cfm_factor,
+                        dir1,
+                        im2,
+                        solver_vel2,
+                    );
+                }
+
+                if elements.len() % 2 == 1 {
+                    let element = elements.last_mut().unwrap();
+                    element
+                        .normal_part
+                        .solve(cfm_factor, dir1, im2, solver_vel2);
+                }
+            } else {
+                for element in elements.iter_mut() {
+                    element
+                        .normal_part
+                        .solve(cfm_factor, dir1, im2, solver_vel2);
+                }
             }
         }
 
diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs b/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs
index cd2ea56..077dd75 100644
--- a/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs
+++ b/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs
@@ -1,4 +1,5 @@
 use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart};
+use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
 use crate::dynamics::solver::solver_body::SolverBody;
 use crate::dynamics::solver::{ContactPointInfos, SolverVel};
 use crate::dynamics::{
@@ -7,14 +8,14 @@ use crate::dynamics::{
 };
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{
-    AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS,
-    SIMD_WIDTH,
+    AngVector, AngularInertia, Isometry, Point, Real, SimdReal, TangentImpulse, Vector, DIM,
+    MAX_MANIFOLD_POINTS, SIMD_WIDTH,
 };
 #[cfg(feature = "dim2")]
 use crate::utils::SimdBasis;
 use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot};
 use num::Zero;
-use parry::math::SimdBool;
+use parry::utils::SdpMatrix2;
 use simba::simd::{SimdPartialOrd, SimdValue};
 
 #[derive(Copy, Clone, Debug)]
@@ -118,6 +119,11 @@ impl SimdOneBodyConstraintBuilder {
                 let is_bouncy = SimdReal::from(gather![
                     |ii| manifold_points[ii][k].is_bouncy() as u32 as Real
                 ]);
+                let warmstart_impulse =
+                    SimdReal::from(gather![|ii| manifold_points[ii][k].warmstart_impulse]);
+                let warmstart_tangent_impulse = TangentImpulse::from(gather![|ii| manifold_points
+                    [ii][k]
+                    .warmstart_tangent_impulse]);
 
                 let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]);
                 let point = Point::from(gather![|ii| manifold_points[ii][k].point]);
@@ -153,14 +159,15 @@ impl SimdOneBodyConstraintBuilder {
                         gcross2,
                         rhs: na::zero(),
                         rhs_wo_bias: na::zero(),
-                        impulse: na::zero(),
-                        total_impulse: na::zero(),
+                        impulse: warmstart_impulse,
+                        impulse_accumulator: na::zero(),
                         r: projected_mass,
+                        r_mat_elts: [SimdReal::zero(); 2],
                     };
                 }
 
                 // tangent parts.
-                constraint.elements[k].tangent_part.impulse = na::zero();
+                constraint.elements[k].tangent_part.impulse = warmstart_tangent_impulse;
 
                 for j in 0..DIM - 1 {
                     let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
@@ -200,6 +207,47 @@ impl SimdOneBodyConstraintBuilder {
                     builder.infos[k] = infos;
                 }
             }
+
+            if BLOCK_SOLVER_ENABLED {
+                // Coupling between consecutive pairs.
+                for k in 0..num_points / 2 {
+                    let k0 = k * 2;
+                    let k1 = k * 2 + 1;
+
+                    let r0 = constraint.elements[k0].normal_part.r;
+                    let r1 = constraint.elements[k1].normal_part.r;
+
+                    let mut r_mat = SdpMatrix2::zero();
+                    r_mat.m12 = force_dir1.dot(&im2.component_mul(&force_dir1))
+                        + constraint.elements[k0]
+                            .normal_part
+                            .gcross2
+                            .gdot(constraint.elements[k1].normal_part.gcross2);
+                    r_mat.m11 = utils::simd_inv(r0);
+                    r_mat.m22 = utils::simd_inv(r1);
+
+                    let (inv, det) = {
+                        let _disable_fe_except =
+                            crate::utils::DisableFloatingPointExceptionsFlags::
+                            disable_floating_point_exceptions();
+                        r_mat.inverse_and_get_determinant_unchecked()
+                    };
+                    let is_invertible = det.simd_gt(SimdReal::zero());
+
+                    // If inversion failed, the contacts are redundant.
+                    // Ignore the one with the smallest depth (it is too late to
+                    // have the constraint removed from the constraint set, so just
+                    // set the mass (r) matrix elements to 0.
+                    constraint.elements[k0].normal_part.r_mat_elts = [
+                        inv.m11.select(is_invertible, r0),
+                        inv.m22.select(is_invertible, SimdReal::zero()),
+                    ];
+                    constraint.elements[k1].normal_part.r_mat_elts = [
+                        inv.m12.select(is_invertible, SimdReal::zero()),
+                        r_mat.m12.select(is_invertible, SimdReal::zero()),
+                    ];
+                }
+            }
         }
     }
 
@@ -213,15 +261,14 @@ impl SimdOneBodyConstraintBuilder {
         _multibodies: &MultibodyJointSet,
         constraint: &mut OneBodyConstraintSimd,
     ) {
-        let cfm_factor = SimdReal::splat(params.cfm_factor());
-        let dt = SimdReal::splat(params.dt);
+        let cfm_factor = SimdReal::splat(params.contact_cfm_factor());
         let inv_dt = SimdReal::splat(params.inv_dt());
-        let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
-        let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
-        let max_penetration_correction = SimdReal::splat(params.max_penetration_correction);
+        let allowed_lin_err = SimdReal::splat(params.allowed_linear_error());
+        let erp_inv_dt = SimdReal::splat(params.contact_erp_inv_dt());
+        let max_corrective_velocity = SimdReal::splat(params.max_corrective_velocity());
+        let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient);
 
         let rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]];
-        let ccd_thickness = SimdReal::from(gather![|ii| rb2[ii].ccd_thickness]);
         let poss2 = Isometry::from(gather![|ii| rb2[ii].position]);
 
         let all_infos = &self.infos[..constraint.num_contacts as usize];
@@ -242,7 +289,6 @@ impl SimdOneBodyConstraintBuilder {
             constraint.dir1.cross(&constraint.tangent1),
         ];
 
-        let mut is_fast_contact = SimdBool::splat(false);
         let solved_dt = SimdReal::splat(solved_dt);
 
         for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) {
@@ -255,24 +301,20 @@ impl SimdOneBodyConstraintBuilder {
             {
                 let rhs_wo_bias =
                     info.normal_rhs_wo_bias + dist.simd_max(SimdReal::zero()) * inv_dt;
-                let rhs_bias = (dist + allowed_lin_err)
-                    .simd_clamp(-max_penetration_correction, SimdReal::zero())
-                    * erp_inv_dt;
+                let rhs_bias = ((dist + allowed_lin_err) * erp_inv_dt)
+                    .simd_clamp(-max_corrective_velocity, SimdReal::zero());
                 let new_rhs = rhs_wo_bias + rhs_bias;
-                let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse;
-                is_fast_contact =
-                    is_fast_contact | (-new_rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
 
                 element.normal_part.rhs_wo_bias = rhs_wo_bias;
                 element.normal_part.rhs = new_rhs;
-                element.normal_part.total_impulse = total_impulse;
-                element.normal_part.impulse = na::zero();
+                element.normal_part.impulse_accumulator += element.normal_part.impulse;
+                element.normal_part.impulse *= warmstart_coeff;
             }
 
             // tangent parts.
             {
-                element.tangent_part.total_impulse += element.tangent_part.impulse;
-                element.tangent_part.impulse = na::zero();
+                element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
+                element.tangent_part.impulse *= warmstart_coeff;
 
                 for j in 0..DIM - 1 {
                     let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
@@ -281,7 +323,7 @@ impl SimdOneBodyConstraintBuilder {
             }
         }
 
-        constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
+        constraint.cfm_factor = cfm_factor;
     }
 }
 
@@ -301,6 +343,27 @@ pub(crate) struct OneBodyConstraintSimd {
 }
 
 impl OneBodyConstraintSimd {
+    pub fn warmstart(&mut self, solver_vels: &mut [SolverVel<Real>]) {
+        let mut solver_vel2 = SolverVel {
+            linear: Vector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].linear]),
+            angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].angular]),
+        };
+
+        OneBodyConstraintElement::warmstart_group(
+            &mut self.elements[..self.num_contacts as usize],
+            &self.dir1,
+            #[cfg(feature = "dim3")]
+            &self.tangent1,
+            &self.im2,
+            &mut solver_vel2,
+        );
+
+        for ii in 0..SIMD_WIDTH {
+            solver_vels[self.solver_vel2[ii]].linear = solver_vel2.linear.extract(ii);
+            solver_vels[self.solver_vel2[ii]].angular = solver_vel2.angular.extract(ii);
+        }
+    }
+
     pub fn solve(
         &mut self,
         solver_vels: &mut [SolverVel<Real>],
@@ -334,26 +397,21 @@ impl OneBodyConstraintSimd {
     // FIXME: duplicated code. This is exactly the same as in the two-body velocity constraint.
     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();
-            #[cfg(feature = "dim2")]
-            let tangent_impulses: [_; SIMD_WIDTH] = self.elements[k].tangent_part.impulse[0].into();
-            #[cfg(feature = "dim3")]
-            let tangent_impulses = self.elements[k].tangent_part.impulse;
+            let warmstart_impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into();
+            let warmstart_tangent_impulses = self.elements[k].tangent_part.impulse;
+            let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.total_impulse().into();
+            let tangent_impulses = self.elements[k].tangent_part.total_impulse();
 
             for ii in 0..SIMD_WIDTH {
                 let manifold = &mut manifolds_all[self.manifold_id[ii]];
                 let contact_id = self.manifold_contact_id[k][ii];
                 let active_contact = &mut manifold.points[contact_id as usize];
-                active_contact.data.impulse = impulses[ii];
 
-                #[cfg(feature = "dim2")]
-                {
-                    active_contact.data.tangent_impulse = tangent_impulses[ii];
-                }
-                #[cfg(feature = "dim3")]
-                {
-                    active_contact.data.tangent_impulse = tangent_impulses.extract(ii);
-                }
+                active_contact.data.warmstart_impulse = warmstart_impulses[ii];
+                active_contact.data.warmstart_tangent_impulse =
+                    warmstart_tangent_impulses.extract(ii);
+                active_contact.data.impulse = impulses[ii];
+                active_contact.data.tangent_impulse = tangent_impulses.extract(ii);
             }
         }
     }
diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint.rs b/src/dynamics/solver/contact_constraint/two_body_constraint.rs
index 0a0ebd6..d2a2c49 100644
--- a/src/dynamics/solver/contact_constraint/two_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/two_body_constraint.rs
@@ -2,11 +2,12 @@ use super::{ContactConstraintTypes, ContactPointInfos};
 use crate::dynamics::solver::SolverVel;
 use crate::dynamics::solver::{AnyConstraintMut, SolverBody};
 
+use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
 use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{Isometry, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
 use crate::utils::{self, SimdAngularInertia, SimdBasis, SimdCross, SimdDot};
-use na::DVector;
+use na::{DVector, Matrix2};
 
 use super::{TwoBodyConstraintElement, TwoBodyConstraintNormalPart};
 
@@ -23,6 +24,25 @@ impl<'a> AnyConstraintMut<'a, ContactConstraintTypes> {
             Self::SimdTwoBodies(c) => c.remove_cfm_and_bias_from_rhs(),
         }
     }
+    pub fn warmstart(
+        &mut self,
+        generic_jacobians: &DVector<Real>,
+        solver_vels: &mut [SolverVel<Real>],
+        generic_solver_vels: &mut DVector<Real>,
+    ) {
+        match self {
+            Self::OneBody(c) => c.warmstart(solver_vels),
+            Self::TwoBodies(c) => c.warmstart(solver_vels),
+            Self::GenericOneBody(c) => c.warmstart(generic_jacobians, generic_solver_vels),
+            Self::GenericTwoBodies(c) => {
+                c.warmstart(generic_jacobians, solver_vels, generic_solver_vels)
+            }
+            #[cfg(feature = "simd-is-enabled")]
+            Self::SimdOneBody(c) => c.warmstart(solver_vels),
+            #[cfg(feature = "simd-is-enabled")]
+            Self::SimdTwoBodies(c) => c.warmstart(solver_vels),
+        }
+    }
 
     pub fn solve_restitution(
         &mut self,
@@ -222,15 +242,17 @@ impl TwoBodyConstraintBuilder {
                         gcross2,
                         rhs: na::zero(),
                         rhs_wo_bias: na::zero(),
-                        impulse: na::zero(),
-                        total_impulse: na::zero(),
+                        impulse: manifold_point.warmstart_impulse,
+                        impulse_accumulator: na::zero(),
                         r: projected_mass,
+                        r_mat_elts: [0.0; 2],
                     };
                 }
 
                 // Tangent parts.
                 {
-                    constraint.elements[k].tangent_part.impulse = na::zero();
+                    constraint.elements[k].tangent_part.impulse =
+                        manifold_point.warmstart_tangent_impulse;
 
                     for j in 0..DIM - 1 {
                         let gcross1 = mprops1
@@ -284,6 +306,48 @@ impl TwoBodyConstraintBuilder {
                 builder.infos[k] = infos;
                 constraint.manifold_contact_id[k] = manifold_point.contact_id;
             }
+
+            if BLOCK_SOLVER_ENABLED {
+                // Coupling between consecutive pairs.
+                for k in 0..manifold_points.len() / 2 {
+                    let k0 = k * 2;
+                    let k1 = k * 2 + 1;
+
+                    let mut r_mat = Matrix2::zeros();
+                    let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
+                    let r0 = constraint.elements[k0].normal_part.r;
+                    let r1 = constraint.elements[k1].normal_part.r;
+                    r_mat.m12 = force_dir1.dot(&imsum.component_mul(&force_dir1))
+                        + constraint.elements[k0]
+                            .normal_part
+                            .gcross1
+                            .gdot(constraint.elements[k1].normal_part.gcross1)
+                        + constraint.elements[k0]
+                            .normal_part
+                            .gcross2
+                            .gdot(constraint.elements[k1].normal_part.gcross2);
+                    r_mat.m21 = r_mat.m12;
+                    r_mat.m11 = utils::inv(r0);
+                    r_mat.m22 = utils::inv(r1);
+
+                    if let Some(inv) = r_mat.try_inverse() {
+                        constraint.elements[k0].normal_part.r_mat_elts = [inv.m11, inv.m22];
+                        constraint.elements[k1].normal_part.r_mat_elts = [inv.m12, r_mat.m12];
+                    } else {
+                        // If inversion failed, the contacts are redundant.
+                        // Ignore the one with the smallest depth (it is too late to
+                        // have the constraint removed from the constraint set, so just
+                        // set the mass (r) matrix elements to 0.
+                        constraint.elements[k0].normal_part.r_mat_elts =
+                            if manifold_points[k0].dist <= manifold_points[k1].dist {
+                                [r0, 0.0]
+                            } else {
+                                [0.0, r1]
+                            };
+                        constraint.elements[k1].normal_part.r_mat_elts = [0.0; 2];
+                    }
+                }
+            }
         }
     }
 
@@ -297,15 +361,7 @@ impl TwoBodyConstraintBuilder {
     ) {
         let rb1 = &bodies[constraint.solver_vel1];
         let rb2 = &bodies[constraint.solver_vel2];
-        let ccd_thickness = rb1.ccd_thickness + rb2.ccd_thickness;
-        self.update_with_positions(
-            params,
-            solved_dt,
-            &rb1.position,
-            &rb2.position,
-            ccd_thickness,
-            constraint,
-        )
+        self.update_with_positions(params, solved_dt, &rb1.position, &rb2.position, constraint)
     }
 
     // Used by both generic and non-generic builders..
@@ -315,18 +371,15 @@ impl TwoBodyConstraintBuilder {
         solved_dt: Real,
         rb1_pos: &Isometry<Real>,
         rb2_pos: &Isometry<Real>,
-        ccd_thickness: Real,
         constraint: &mut TwoBodyConstraint,
     ) {
-        let cfm_factor = params.cfm_factor();
+        let cfm_factor = params.contact_cfm_factor();
         let inv_dt = params.inv_dt();
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = params.contact_erp_inv_dt();
 
         let all_infos = &self.infos[..constraint.num_contacts as usize];
         let all_elements = &mut constraint.elements[..constraint.num_contacts as usize];
 
-        let mut is_fast_contact = false;
-
         #[cfg(feature = "dim2")]
         let tangents1 = constraint.dir1.orthonormal_basis();
         #[cfg(feature = "dim3")]
@@ -344,23 +397,20 @@ impl TwoBodyConstraintBuilder {
             // Normal part.
             {
                 let rhs_wo_bias = info.normal_rhs_wo_bias + dist.max(0.0) * inv_dt;
-                let rhs_bias = erp_inv_dt
-                    * (dist + params.allowed_linear_error)
-                        .clamp(-params.max_penetration_correction, 0.0);
+                let rhs_bias = (erp_inv_dt * (dist + params.allowed_linear_error()))
+                    .clamp(-params.max_corrective_velocity(), 0.0);
                 let new_rhs = rhs_wo_bias + rhs_bias;
-                let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse;
-                is_fast_contact = is_fast_contact || (-new_rhs * params.dt > ccd_thickness * 0.5);
 
                 element.normal_part.rhs_wo_bias = rhs_wo_bias;
                 element.normal_part.rhs = new_rhs;
-                element.normal_part.total_impulse = total_impulse;
-                element.normal_part.impulse = na::zero();
+                element.normal_part.impulse_accumulator += element.normal_part.impulse;
+                element.normal_part.impulse *= params.warmstart_coefficient;
             }
 
             // Tangent part.
             {
-                element.tangent_part.total_impulse += element.tangent_part.impulse;
-                element.tangent_part.impulse = na::zero();
+                element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
+                element.tangent_part.impulse *= params.warmstart_coefficient;
 
                 for j in 0..DIM - 1 {
                     let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
@@ -369,11 +419,30 @@ impl TwoBodyConstraintBuilder {
             }
         }
 
-        constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
+        constraint.cfm_factor = cfm_factor;
     }
 }
 
 impl TwoBodyConstraint {
+    pub fn warmstart(&mut self, solver_vels: &mut [SolverVel<Real>]) {
+        let mut solver_vel1 = solver_vels[self.solver_vel1];
+        let mut solver_vel2 = solver_vels[self.solver_vel2];
+
+        TwoBodyConstraintElement::warmstart_group(
+            &mut self.elements[..self.num_contacts as usize],
+            &self.dir1,
+            #[cfg(feature = "dim3")]
+            &self.tangent1,
+            &self.im1,
+            &self.im2,
+            &mut solver_vel1,
+            &mut solver_vel2,
+        );
+
+        solver_vels[self.solver_vel1] = solver_vel1;
+        solver_vels[self.solver_vel2] = solver_vel2;
+    }
+
     pub fn solve(
         &mut self,
         solver_vels: &mut [SolverVel<Real>],
@@ -408,16 +477,10 @@ impl TwoBodyConstraint {
         for k in 0..self.num_contacts as usize {
             let contact_id = self.manifold_contact_id[k];
             let active_contact = &mut manifold.points[contact_id as usize];
-            active_contact.data.impulse = self.elements[k].normal_part.impulse;
-
-            #[cfg(feature = "dim2")]
-            {
-                active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0];
-            }
-            #[cfg(feature = "dim3")]
-            {
-                active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse;
-            }
+            active_contact.data.warmstart_impulse = self.elements[k].normal_part.impulse;
+            active_contact.data.warmstart_tangent_impulse = self.elements[k].tangent_part.impulse;
+            active_contact.data.impulse = self.elements[k].normal_part.total_impulse();
+            active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse();
         }
     }
 
diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs
index 8c720b9..b794833 100644
--- a/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs
+++ b/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs
@@ -1,6 +1,9 @@
+use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
 use crate::dynamics::solver::SolverVel;
-use crate::math::{AngVector, Vector, DIM};
+use crate::math::{AngVector, TangentImpulse, Vector, DIM};
 use crate::utils::{SimdBasis, SimdDot, SimdRealCopy};
+use na::Vector2;
+use simba::simd::SimdValue;
 
 #[derive(Copy, Clone, Debug)]
 pub(crate) struct TwoBodyConstraintTangentPart<N: SimdRealCopy> {
@@ -13,9 +16,9 @@ pub(crate) struct TwoBodyConstraintTangentPart<N: SimdRealCopy> {
     #[cfg(feature = "dim3")]
     pub impulse: na::Vector2<N>,
     #[cfg(feature = "dim2")]
-    pub total_impulse: na::Vector1<N>,
+    pub impulse_accumulator: na::Vector1<N>,
     #[cfg(feature = "dim3")]
-    pub total_impulse: na::Vector2<N>,
+    pub impulse_accumulator: na::Vector2<N>,
     #[cfg(feature = "dim2")]
     pub r: [N; 1],
     #[cfg(feature = "dim3")]
@@ -30,7 +33,7 @@ impl<N: SimdRealCopy> TwoBodyConstraintTangentPart<N> {
             rhs: [na::zero(); DIM - 1],
             rhs_wo_bias: [na::zero(); DIM - 1],
             impulse: na::zero(),
-            total_impulse: na::zero(),
+            impulse_accumulator: na::zero(),
             #[cfg(feature = "dim2")]
             r: [na::zero(); 1],
             #[cfg(feature = "dim3")]
@@ -38,13 +41,18 @@ impl<N: SimdRealCopy> TwoBodyConstraintTangentPart<N> {
         }
     }
 
+    /// Total impulse applied across all the solver substeps.
     #[inline]
-    pub fn apply_limit(
+    pub fn total_impulse(&self) -> TangentImpulse<N> {
+        self.impulse_accumulator + self.impulse
+    }
+
+    #[inline]
+    pub fn warmstart(
         &mut self,
         tangents1: [&Vector<N>; DIM - 1],
         im1: &Vector<N>,
         im2: &Vector<N>,
-        limit: N,
         solver_vel1: &mut SolverVel<N>,
         solver_vel2: &mut SolverVel<N>,
     ) where
@@ -52,37 +60,24 @@ impl<N: SimdRealCopy> TwoBodyConstraintTangentPart<N> {
     {
         #[cfg(feature = "dim2")]
         {
-            let new_impulse = self.impulse[0].simd_clamp(-limit, limit);
-            let dlambda = new_impulse - self.impulse[0];
-            self.impulse[0] = new_impulse;
+            solver_vel1.linear += tangents1[0].component_mul(im1) * self.impulse[0];
+            solver_vel1.angular += self.gcross1[0] * self.impulse[0];
 
-            solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda;
-            solver_vel1.angular += self.gcross1[0] * dlambda;
-
-            solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda;
-            solver_vel2.angular += self.gcross2[0] * dlambda;
+            solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0];
+            solver_vel2.angular += self.gcross2[0] * self.impulse[0];
         }
 
         #[cfg(feature = "dim3")]
         {
-            let new_impulse = self.impulse;
-            let new_impulse = {
-                let _disable_fe_except =
-                    crate::utils::DisableFloatingPointExceptionsFlags::
-                    disable_floating_point_exceptions();
-                new_impulse.simd_cap_magnitude(limit)
-            };
+            solver_vel1.linear += tangents1[0].component_mul(im1) * self.impulse[0]
+                + tangents1[1].component_mul(im1) * self.impulse[1];
+            solver_vel1.angular +=
+                self.gcross1[0] * self.impulse[0] + self.gcross1[1] * self.impulse[1];
 
-            let dlambda = new_impulse - self.impulse;
-            self.impulse = new_impulse;
-
-            solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda[0]
-                + tangents1[1].component_mul(im1) * dlambda[1];
-            solver_vel1.angular += self.gcross1[0] * dlambda[0] + self.gcross1[1] * dlambda[1];
-
-            solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
-                + tangents1[1].component_mul(im2) * -dlambda[1];
-            solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
+            solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0]
+                + tangents1[1].component_mul(im2) * -self.impulse[1];
+            solver_vel2.angular +=
+                self.gcross2[0] * self.impulse[0] + self.gcross2[1] * self.impulse[1];
         }
     }
 
@@ -166,8 +161,13 @@ pub(crate) struct TwoBodyConstraintNormalPart<N: SimdRealCopy> {
     pub rhs: N,
     pub rhs_wo_bias: N,
     pub impulse: N,
-    pub total_impulse: N,
+    pub impulse_accumulator: N,
     pub r: N,
+    // For coupled constraint pairs, even constraints store the
+    // diagonal of the projected mass matrix. Odd constraints
+    // store the off-diagonal element of the projected mass matrix,
+    // as well as the off-diagonal element of the inverse projected mass matrix.
+    pub r_mat_elts: [N; 2],
 }
 
 impl<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
@@ -178,11 +178,34 @@ impl<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
             rhs: na::zero(),
             rhs_wo_bias: na::zero(),
             impulse: na::zero(),
-            total_impulse: na::zero(),
+            impulse_accumulator: na::zero(),
             r: na::zero(),
+            r_mat_elts: [N::zero(); 2],
         }
     }
 
+    /// Total impulse applied across all the solver substeps.
+    #[inline]
+    pub fn total_impulse(&self) -> N {
+        self.impulse_accumulator + self.impulse
+    }
+
+    #[inline]
+    pub fn warmstart(
+        &mut self,
+        dir1: &Vector<N>,
+        im1: &Vector<N>,
+        im2: &Vector<N>,
+        solver_vel1: &mut SolverVel<N>,
+        solver_vel2: &mut SolverVel<N>,
+    ) {
+        solver_vel1.linear += dir1.component_mul(im1) * self.impulse;
+        solver_vel1.angular += self.gcross1 * self.impulse;
+
+        solver_vel2.linear += dir1.component_mul(im2) * -self.impulse;
+        solver_vel2.angular += self.gcross2 * self.impulse;
+    }
+
     #[inline]
     pub fn solve(
         &mut self,
@@ -209,6 +232,83 @@ impl<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
         solver_vel2.linear += dir1.component_mul(im2) * -dlambda;
         solver_vel2.angular += self.gcross2 * dlambda;
     }
+
+    #[inline(always)]
+    pub(crate) fn solve_mlcp_two_constraints(
+        dvel: Vector2<N>,
+        prev_impulse: Vector2<N>,
+        r_a: N,
+        r_b: N,
+        [r_mat11, r_mat22]: [N; 2],
+        [r_mat12, r_mat_inv12]: [N; 2],
+        cfm_factor: N,
+    ) -> Vector2<N> {
+        let r_dvel = Vector2::new(
+            r_mat11 * dvel.x + r_mat12 * dvel.y,
+            r_mat12 * dvel.x + r_mat22 * dvel.y,
+        );
+        let new_impulse0 = prev_impulse - r_dvel;
+        let new_impulse1 = Vector2::new(prev_impulse.x - r_a * dvel.x, N::zero());
+        let new_impulse2 = Vector2::new(N::zero(), prev_impulse.y - r_b * dvel.y);
+        let new_impulse3 = Vector2::new(N::zero(), N::zero());
+
+        let keep0 = new_impulse0.x.simd_ge(N::zero()) & new_impulse0.y.simd_ge(N::zero());
+        let keep1 = new_impulse1.x.simd_ge(N::zero())
+            & (dvel.y + r_mat_inv12 * new_impulse1.x).simd_ge(N::zero());
+        let keep2 = new_impulse2.y.simd_ge(N::zero())
+            & (dvel.x + r_mat_inv12 * new_impulse2.y).simd_ge(N::zero());
+        let keep3 = dvel.x.simd_ge(N::zero()) & dvel.y.simd_ge(N::zero());
+
+        let selected3 = (new_impulse3 * cfm_factor).select(keep3, prev_impulse);
+        let selected2 = (new_impulse2 * cfm_factor).select(keep2, selected3);
+        let selected1 = (new_impulse1 * cfm_factor).select(keep1, selected2);
+        (new_impulse0 * cfm_factor).select(keep0, selected1)
+    }
+
+    #[inline]
+    pub fn solve_pair(
+        constraint_a: &mut Self,
+        constraint_b: &mut Self,
+        cfm_factor: N,
+        dir1: &Vector<N>,
+        im1: &Vector<N>,
+        im2: &Vector<N>,
+        solver_vel1: &mut SolverVel<N>,
+        solver_vel2: &mut SolverVel<N>,
+    ) where
+        AngVector<N>: SimdDot<AngVector<N>, Result = N>,
+    {
+        let dvel_lin = dir1.dot(&solver_vel1.linear) - dir1.dot(&solver_vel2.linear);
+        let dvel_a = dvel_lin
+            + constraint_a.gcross1.gdot(solver_vel1.angular)
+            + constraint_a.gcross2.gdot(solver_vel2.angular)
+            + constraint_a.rhs;
+        let dvel_b = dvel_lin
+            + constraint_b.gcross1.gdot(solver_vel1.angular)
+            + constraint_b.gcross2.gdot(solver_vel2.angular)
+            + constraint_b.rhs;
+
+        let prev_impulse = Vector2::new(constraint_a.impulse, constraint_b.impulse);
+        let new_impulse = Self::solve_mlcp_two_constraints(
+            Vector2::new(dvel_a, dvel_b),
+            prev_impulse,
+            constraint_a.r,
+            constraint_b.r,
+            constraint_a.r_mat_elts,
+            constraint_b.r_mat_elts,
+            cfm_factor,
+        );
+
+        let dlambda = new_impulse - prev_impulse;
+
+        constraint_a.impulse = new_impulse.x;
+        constraint_b.impulse = new_impulse.y;
+
+        solver_vel1.linear += dir1.component_mul(im1) * (dlambda.x + dlambda.y);
+        solver_vel1.angular += constraint_a.gcross1 * dlambda.x + constraint_b.gcross1 * dlambda.y;
+        solver_vel2.linear += dir1.component_mul(im2) * (-dlambda.x - dlambda.y);
+        solver_vel2.angular += constraint_a.gcross2 * dlambda.x + constraint_b.gcross2 * dlambda.y;
+    }
 }
 
 #[derive(Copy, Clone, Debug)]
@@ -225,6 +325,31 @@ impl<N: SimdRealCopy> TwoBodyConstraintElement<N> {
         }
     }
 
+    #[inline]
+    pub fn warmstart_group(
+        elements: &mut [Self],
+        dir1: &Vector<N>,
+        #[cfg(feature = "dim3")] tangent1: &Vector<N>,
+        im1: &Vector<N>,
+        im2: &Vector<N>,
+        solver_vel1: &mut SolverVel<N>,
+        solver_vel2: &mut SolverVel<N>,
+    ) {
+        #[cfg(feature = "dim3")]
+        let tangents1 = [tangent1, &dir1.cross(tangent1)];
+        #[cfg(feature = "dim2")]
+        let tangents1 = [&dir1.orthonormal_vector()];
+
+        for element in elements.iter_mut() {
+            element
+                .normal_part
+                .warmstart(dir1, im1, im2, solver_vel1, solver_vel2);
+            element
+                .tangent_part
+                .warmstart(tangents1, im1, im2, solver_vel1, solver_vel2);
+        }
+    }
+
     #[inline]
     pub fn solve_group(
         cfm_factor: N,
@@ -236,31 +361,53 @@ impl<N: SimdRealCopy> TwoBodyConstraintElement<N> {
         limit: N,
         solver_vel1: &mut SolverVel<N>,
         solver_vel2: &mut SolverVel<N>,
-        solve_normal: bool,
+        solve_restitution: bool,
         solve_friction: bool,
     ) where
         Vector<N>: SimdBasis,
         AngVector<N>: SimdDot<AngVector<N>, Result = N>,
     {
-        #[cfg(feature = "dim3")]
-        let tangents1 = [tangent1, &dir1.cross(tangent1)];
-        #[cfg(feature = "dim2")]
-        let tangents1 = [&dir1.orthonormal_vector()];
+        if solve_restitution {
+            if BLOCK_SOLVER_ENABLED {
+                for elements in elements.chunks_exact_mut(2) {
+                    let [element_a, element_b] = elements else {
+                        unreachable!()
+                    };
 
-        // Solve penetration.
-        if solve_normal {
-            for element in elements.iter_mut() {
-                element
-                    .normal_part
-                    .solve(cfm_factor, dir1, im1, im2, solver_vel1, solver_vel2);
-                let limit = limit * element.normal_part.impulse;
-                let part = &mut element.tangent_part;
-                part.apply_limit(tangents1, im1, im2, limit, solver_vel1, solver_vel2);
+                    TwoBodyConstraintNormalPart::solve_pair(
+                        &mut element_a.normal_part,
+                        &mut element_b.normal_part,
+                        cfm_factor,
+                        dir1,
+                        im1,
+                        im2,
+                        solver_vel1,
+                        solver_vel2,
+                    );
+                }
+
+                // There is one constraint left to solve if there isn’t an even number.
+                if elements.len() % 2 == 1 {
+                    let element = elements.last_mut().unwrap();
+                    element
+                        .normal_part
+                        .solve(cfm_factor, dir1, im1, im2, solver_vel1, solver_vel2);
+                }
+            } else {
+                for element in elements.iter_mut() {
+                    element
+                        .normal_part
+                        .solve(cfm_factor, dir1, im1, im2, solver_vel1, solver_vel2);
+                }
             }
         }
 
-        // Solve friction.
         if solve_friction {
+            #[cfg(feature = "dim3")]
+            let tangents1 = [tangent1, &dir1.cross(tangent1)];
+            #[cfg(feature = "dim2")]
+            let tangents1 = [&dir1.orthonormal_vector()];
+
             for element in elements.iter_mut() {
                 let limit = limit * element.normal_part.impulse;
                 let part = &mut element.tangent_part;
diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs b/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs
index ee176d6..2e2c68a 100644
--- a/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs
+++ b/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs
@@ -1,4 +1,5 @@
 use super::{TwoBodyConstraintElement, TwoBodyConstraintNormalPart};
+use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
 use crate::dynamics::solver::solver_body::SolverBody;
 use crate::dynamics::solver::{ContactPointInfos, SolverVel};
 use crate::dynamics::{
@@ -7,14 +8,14 @@ use crate::dynamics::{
 };
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{
-    AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS,
-    SIMD_WIDTH,
+    AngVector, AngularInertia, Isometry, Point, Real, SimdReal, TangentImpulse, Vector, DIM,
+    MAX_MANIFOLD_POINTS, SIMD_WIDTH,
 };
 #[cfg(feature = "dim2")]
 use crate::utils::SimdBasis;
 use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot};
 use num::Zero;
-use parry::math::SimdBool;
+use parry::utils::SdpMatrix2;
 use simba::simd::{SimdPartialOrd, SimdValue};
 
 #[derive(Copy, Clone, Debug)]
@@ -101,6 +102,11 @@ impl TwoBodyConstraintBuilderSimd {
                 let is_bouncy = SimdReal::from(gather![
                     |ii| manifold_points[ii][k].is_bouncy() as u32 as Real
                 ]);
+                let warmstart_impulse =
+                    SimdReal::from(gather![|ii| manifold_points[ii][k].warmstart_impulse]);
+                let warmstart_tangent_impulse = TangentImpulse::from(gather![|ii| manifold_points
+                    [ii][k]
+                    .warmstart_tangent_impulse]);
 
                 let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]);
                 let point = Point::from(gather![|ii| manifold_points[ii][k].point]);
@@ -137,14 +143,15 @@ impl TwoBodyConstraintBuilderSimd {
                         gcross2,
                         rhs: na::zero(),
                         rhs_wo_bias: na::zero(),
-                        impulse: SimdReal::splat(0.0),
-                        total_impulse: SimdReal::splat(0.0),
+                        impulse: warmstart_impulse,
+                        impulse_accumulator: SimdReal::splat(0.0),
                         r: projected_mass,
+                        r_mat_elts: [SimdReal::zero(); 2],
                     };
                 }
 
                 // tangent parts.
-                constraint.elements[k].tangent_part.impulse = na::zero();
+                constraint.elements[k].tangent_part.impulse = warmstart_tangent_impulse;
 
                 for j in 0..DIM - 1 {
                     let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j]));
@@ -186,6 +193,52 @@ impl TwoBodyConstraintBuilderSimd {
 
                 builder.infos[k] = infos;
             }
+
+            if BLOCK_SOLVER_ENABLED {
+                // Coupling between consecutive pairs.
+                for k in 0..num_points / 2 {
+                    let k0 = k * 2;
+                    let k1 = k * 2 + 1;
+
+                    let imsum = im1 + im2;
+                    let r0 = constraint.elements[k0].normal_part.r;
+                    let r1 = constraint.elements[k1].normal_part.r;
+
+                    let mut r_mat = SdpMatrix2::zero();
+                    r_mat.m12 = force_dir1.dot(&imsum.component_mul(&force_dir1))
+                        + constraint.elements[k0]
+                            .normal_part
+                            .gcross1
+                            .gdot(constraint.elements[k1].normal_part.gcross1)
+                        + constraint.elements[k0]
+                            .normal_part
+                            .gcross2
+                            .gdot(constraint.elements[k1].normal_part.gcross2);
+                    r_mat.m11 = utils::simd_inv(r0);
+                    r_mat.m22 = utils::simd_inv(r1);
+
+                    let (inv, det) = {
+                        let _disable_fe_except =
+                            crate::utils::DisableFloatingPointExceptionsFlags::
+                            disable_floating_point_exceptions();
+                        r_mat.inverse_and_get_determinant_unchecked()
+                    };
+                    let is_invertible = det.simd_gt(SimdReal::zero());
+
+                    // If inversion failed, the contacts are redundant.
+                    // Ignore the one with the smallest depth (it is too late to
+                    // have the constraint removed from the constraint set, so just
+                    // set the mass (r) matrix elements to 0.
+                    constraint.elements[k0].normal_part.r_mat_elts = [
+                        inv.m11.select(is_invertible, r0),
+                        inv.m22.select(is_invertible, SimdReal::zero()),
+                    ];
+                    constraint.elements[k1].normal_part.r_mat_elts = [
+                        inv.m12.select(is_invertible, SimdReal::zero()),
+                        r_mat.m12.select(is_invertible, SimdReal::zero()),
+                    ];
+                }
+            }
         }
     }
 
@@ -197,19 +250,16 @@ impl TwoBodyConstraintBuilderSimd {
         _multibodies: &MultibodyJointSet,
         constraint: &mut TwoBodyConstraintSimd,
     ) {
-        let cfm_factor = SimdReal::splat(params.cfm_factor());
-        let dt = SimdReal::splat(params.dt);
+        let cfm_factor = SimdReal::splat(params.contact_cfm_factor());
         let inv_dt = SimdReal::splat(params.inv_dt());
-        let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
-        let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
-        let max_penetration_correction = SimdReal::splat(params.max_penetration_correction);
+        let allowed_lin_err = SimdReal::splat(params.allowed_linear_error());
+        let erp_inv_dt = SimdReal::splat(params.contact_erp_inv_dt());
+        let max_corrective_velocity = SimdReal::splat(params.max_corrective_velocity());
+        let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient);
 
         let rb1 = gather![|ii| &bodies[constraint.solver_vel1[ii]]];
         let rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]];
 
-        let ccd_thickness = SimdReal::from(gather![|ii| rb1[ii].ccd_thickness])
-            + SimdReal::from(gather![|ii| rb2[ii].ccd_thickness]);
-
         let poss1 = Isometry::from(gather![|ii| rb1[ii].position]);
         let poss2 = Isometry::from(gather![|ii| rb2[ii].position]);
 
@@ -224,7 +274,6 @@ impl TwoBodyConstraintBuilderSimd {
             constraint.dir1.cross(&constraint.tangent1),
         ];
 
-        let mut is_fast_contact = SimdBool::splat(false);
         let solved_dt = SimdReal::splat(solved_dt);
 
         for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) {
@@ -237,24 +286,20 @@ impl TwoBodyConstraintBuilderSimd {
             {
                 let rhs_wo_bias =
                     info.normal_rhs_wo_bias + dist.simd_max(SimdReal::zero()) * inv_dt;
-                let rhs_bias = (dist + allowed_lin_err)
-                    .simd_clamp(-max_penetration_correction, SimdReal::zero())
-                    * erp_inv_dt;
+                let rhs_bias = ((dist + allowed_lin_err) * erp_inv_dt)
+                    .simd_clamp(-max_corrective_velocity, SimdReal::zero());
                 let new_rhs = rhs_wo_bias + rhs_bias;
-                let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse;
-                is_fast_contact =
-                    is_fast_contact | (-new_rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
 
                 element.normal_part.rhs_wo_bias = rhs_wo_bias;
                 element.normal_part.rhs = new_rhs;
-                element.normal_part.total_impulse = total_impulse;
-                element.normal_part.impulse = na::zero();
+                element.normal_part.impulse_accumulator += element.normal_part.impulse;
+                element.normal_part.impulse *= warmstart_coeff;
             }
 
             // tangent parts.
             {
-                element.tangent_part.total_impulse += element.tangent_part.impulse;
-                element.tangent_part.impulse = na::zero();
+                element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
+                element.tangent_part.impulse *= warmstart_coeff;
 
                 for j in 0..DIM - 1 {
                     let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
@@ -263,7 +308,7 @@ impl TwoBodyConstraintBuilderSimd {
             }
         }
 
-        constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
+        constraint.cfm_factor = cfm_factor;
     }
 }
 
@@ -285,6 +330,38 @@ pub(crate) struct TwoBodyConstraintSimd {
 }
 
 impl TwoBodyConstraintSimd {
+    pub fn warmstart(&mut self, solver_vels: &mut [SolverVel<Real>]) {
+        let mut solver_vel1 = SolverVel {
+            linear: Vector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].linear]),
+            angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].angular]),
+        };
+
+        let mut solver_vel2 = SolverVel {
+            linear: Vector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].linear]),
+            angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].angular]),
+        };
+
+        TwoBodyConstraintElement::warmstart_group(
+            &mut self.elements[..self.num_contacts as usize],
+            &self.dir1,
+            #[cfg(feature = "dim3")]
+            &self.tangent1,
+            &self.im1,
+            &self.im2,
+            &mut solver_vel1,
+            &mut solver_vel2,
+        );
+
+        for ii in 0..SIMD_WIDTH {
+            solver_vels[self.solver_vel1[ii]].linear = solver_vel1.linear.extract(ii);
+            solver_vels[self.solver_vel1[ii]].angular = solver_vel1.angular.extract(ii);
+        }
+        for ii in 0..SIMD_WIDTH {
+            solver_vels[self.solver_vel2[ii]].linear = solver_vel2.linear.extract(ii);
+            solver_vels[self.solver_vel2[ii]].angular = solver_vel2.angular.extract(ii);
+        }
+    }
+
     pub fn solve(
         &mut self,
         solver_vels: &mut [SolverVel<Real>],
@@ -328,26 +405,20 @@ impl TwoBodyConstraintSimd {
 
     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();
-            #[cfg(feature = "dim2")]
-            let tangent_impulses: [_; SIMD_WIDTH] = self.elements[k].tangent_part.impulse[0].into();
-            #[cfg(feature = "dim3")]
-            let tangent_impulses = self.elements[k].tangent_part.impulse;
+            let warmstart_impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into();
+            let warmstart_tangent_impulses = self.elements[k].tangent_part.impulse;
+            let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.total_impulse().into();
+            let tangent_impulses = self.elements[k].tangent_part.total_impulse();
 
             for ii in 0..SIMD_WIDTH {
                 let manifold = &mut manifolds_all[self.manifold_id[ii]];
                 let contact_id = self.manifold_contact_id[k][ii];
                 let active_contact = &mut manifold.points[contact_id as usize];
+                active_contact.data.warmstart_impulse = warmstart_impulses[ii];
+                active_contact.data.warmstart_tangent_impulse =
+                    warmstart_tangent_impulses.extract(ii);
                 active_contact.data.impulse = impulses[ii];
-
-                #[cfg(feature = "dim2")]
-                {
-                    active_contact.data.tangent_impulse = tangent_impulses[ii];
-                }
-                #[cfg(feature = "dim3")]
-                {
-                    active_contact.data.tangent_impulse = tangent_impulses.extract(ii);
-                }
+                active_contact.data.tangent_impulse = tangent_impulses.extract(ii);
             }
         }
     }
diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs
index 2953f98..718542b 100644
--- a/src/dynamics/solver/island_solver.rs
+++ b/src/dynamics/solver/island_solver.rs
@@ -41,14 +41,12 @@ impl IslandSolver {
         joint_indices: &[JointIndex],
         multibodies: &mut MultibodyJointSet,
     ) {
-        counters.solver.velocity_resolution_time.resume();
+        counters.solver.velocity_assembly_time.resume();
         let num_solver_iterations = base_params.num_solver_iterations.get()
             + islands.active_island_additional_solver_iterations(island_id);
 
         let mut params = *base_params;
         params.dt /= num_solver_iterations as Real;
-        params.damping_ratio /= num_solver_iterations as Real;
-        // params.joint_damping_ratio /= num_solver_iterations as Real;
 
         /*
          *
@@ -76,8 +74,10 @@ impl IslandSolver {
             &mut self.contact_constraints,
             &mut self.joint_constraints,
         );
+        counters.solver.velocity_assembly_time.pause();
 
         // SOLVE
+        counters.solver.velocity_resolution_time.resume();
         self.velocity_solver.solve_constraints(
             &params,
             num_solver_iterations,
@@ -86,8 +86,10 @@ impl IslandSolver {
             &mut self.contact_constraints,
             &mut self.joint_constraints,
         );
+        counters.solver.velocity_resolution_time.pause();
 
         // WRITEBACK
+        counters.solver.velocity_writeback_time.resume();
         self.joint_constraints.writeback_impulses(impulse_joints);
         self.contact_constraints.writeback_impulses(manifolds);
         self.velocity_solver.writeback_bodies(
@@ -98,6 +100,6 @@ impl IslandSolver {
             bodies,
             multibodies,
         );
-        counters.solver.velocity_resolution_time.pause();
+        counters.solver.velocity_writeback_time.pause();
     }
 }
diff --git a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs
index 00bead1..40613c1 100644
--- a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs
+++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs
@@ -198,7 +198,7 @@ impl JointOneBodyConstraintBuilder {
 
         if flipped {
             std::mem::swap(&mut handle1, &mut handle2);
-            std::mem::swap(&mut joint_data.local_frame1, &mut joint_data.local_frame2);
+            joint_data.flip();
         };
 
         let rb1 = &bodies[handle1];
@@ -551,6 +551,82 @@ impl<N: SimdRealCopy> JointTwoBodyConstraintHelper<N> {
         constraint
     }
 
+    pub fn motor_linear_coupled<const LANES: usize>(
+        &self,
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; LANES],
+        body1: &JointSolverBody<N, LANES>,
+        body2: &JointSolverBody<N, LANES>,
+        coupled_axes: u8,
+        motor_params: &MotorParameters<N>,
+        limits: Option<[N; 2]>,
+        writeback_id: WritebackId,
+    ) -> JointTwoBodyConstraint<N, LANES> {
+        let inv_dt = N::splat(params.inv_dt());
+
+        let mut lin_jac = Vector::zeros();
+        let mut ang_jac1: AngVector<N> = na::zero();
+        let mut ang_jac2: AngVector<N> = na::zero();
+
+        for i in 0..DIM {
+            if coupled_axes & (1 << i) != 0 {
+                let coeff = self.basis.column(i).dot(&self.lin_err);
+                lin_jac += self.basis.column(i) * coeff;
+                #[cfg(feature = "dim2")]
+                {
+                    ang_jac1 += self.cmat1_basis[i] * coeff;
+                    ang_jac2 += self.cmat2_basis[i] * coeff;
+                }
+                #[cfg(feature = "dim3")]
+                {
+                    ang_jac1 += self.cmat1_basis.column(i) * coeff;
+                    ang_jac2 += self.cmat2_basis.column(i) * coeff;
+                }
+            }
+        }
+
+        let dist = lin_jac.norm();
+        let inv_dist = crate::utils::simd_inv(dist);
+        lin_jac *= inv_dist;
+        ang_jac1 *= inv_dist;
+        ang_jac2 *= inv_dist;
+
+        let mut rhs_wo_bias = N::zero();
+        if motor_params.erp_inv_dt != N::zero() {
+            rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
+        }
+
+        let mut target_vel = motor_params.target_vel;
+        if let Some(limits) = limits {
+            target_vel =
+                target_vel.simd_clamp((limits[0] - dist) * inv_dt, (limits[1] - dist) * inv_dt);
+        };
+
+        rhs_wo_bias += -target_vel;
+
+        ang_jac1 = body1.sqrt_ii * ang_jac1;
+        ang_jac2 = body2.sqrt_ii * ang_jac2;
+
+        JointTwoBodyConstraint {
+            joint_id,
+            solver_vel1: body1.solver_vel,
+            solver_vel2: body2.solver_vel,
+            im1: body1.im,
+            im2: body2.im,
+            impulse: N::zero(),
+            impulse_bounds: [-motor_params.max_impulse, motor_params.max_impulse],
+            lin_jac,
+            ang_jac1,
+            ang_jac2,
+            inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff: motor_params.cfm_coeff,
+            cfm_gain: motor_params.cfm_gain,
+            rhs: rhs_wo_bias,
+            rhs_wo_bias,
+            writeback_id,
+        }
+    }
+
     pub fn lock_linear<const LANES: usize>(
         &self,
         params: &IntegrationParameters,
diff --git a/src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs
index 34256a2..9876195 100644
--- a/src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs
+++ b/src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs
@@ -319,7 +319,6 @@ pub struct JointGenericVelocityOneBodyExternalConstraintBuilder {
     joint_id: JointIndex,
     joint: GenericJoint,
     j_id: usize,
-    flipped: bool,
     constraint_id: usize,
     // These are solver body for both joints, except that
     // the world_com is actually in local-space.
@@ -337,21 +336,20 @@ impl JointGenericVelocityOneBodyExternalConstraintBuilder {
         jacobians: &mut DVector<Real>,
         out_constraint_id: &mut usize,
     ) {
+        let mut joint_data = joint.data;
         let mut handle1 = joint.body1;
         let mut handle2 = joint.body2;
         let flipped = !bodies[handle2].is_dynamic();
 
-        let local_frame1 = if flipped {
+        if flipped {
             std::mem::swap(&mut handle1, &mut handle2);
-            joint.data.local_frame2
-        } else {
-            joint.data.local_frame1
-        };
+            joint_data.flip();
+        }
 
         let rb1 = &bodies[handle1];
         let rb2 = &bodies[handle2];
 
-        let frame1 = rb1.pos.position * local_frame1;
+        let frame1 = rb1.pos.position * joint_data.local_frame1;
 
         let starting_j_id = *j_id;
 
@@ -394,11 +392,10 @@ impl JointGenericVelocityOneBodyExternalConstraintBuilder {
             body1,
             link2,
             joint_id,
-            joint: joint.data,
+            joint: joint_data,
             j_id: starting_j_id,
             frame1,
             local_body2,
-            flipped,
             constraint_id: *out_constraint_id,
         });
 
@@ -417,12 +414,7 @@ impl JointGenericVelocityOneBodyExternalConstraintBuilder {
         //       constraints. Could we make this more incremental?
         let mb2 = &multibodies[self.link2.multibody];
         let pos2 = &mb2.link(self.link2.id).unwrap().local_to_world;
-        let frame2 = pos2
-            * if self.flipped {
-                self.joint.local_frame1
-            } else {
-                self.joint.local_frame2
-            };
+        let frame2 = pos2 * self.joint.local_frame2;
 
         let joint_body2 = JointSolverBody {
             world_com: pos2 * self.local_body2.world_com, // the world_com was stored in local-space.
diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
index e184e3d..60c42d3 100644
--- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
@@ -217,28 +217,26 @@ impl JointTwoBodyConstraint<Real, 1> {
         }
 
         if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
-            // if (motor_axes & !coupled_axes) & (1 << first_coupled_lin_axis_id) != 0 {
-            //     let limits = if limit_axes & (1 << first_coupled_lin_axis_id) != 0 {
-            //         Some([
-            //             joint.limits[first_coupled_lin_axis_id].min,
-            //             joint.limits[first_coupled_lin_axis_id].max,
-            //         ])
-            //     } else {
-            //         None
-            //     };
-            //
-            //     out[len] = builder.motor_linear_coupled
-            //         params,
-            //         [joint_id],
-            //         body1,
-            //         body2,
-            //         coupled_axes,
-            //         &joint.motors[first_coupled_lin_axis_id].motor_params(params.dt),
-            //         limits,
-            //         WritebackId::Motor(first_coupled_lin_axis_id),
-            //     );
-            //     len += 1;
-            // }
+            let limits = if (limit_axes & (1 << first_coupled_lin_axis_id)) != 0 {
+                Some([
+                    joint.limits[first_coupled_lin_axis_id].min,
+                    joint.limits[first_coupled_lin_axis_id].max,
+                ])
+            } else {
+                None
+            };
+
+            out[len] = builder.motor_linear_coupled(
+                params,
+                [joint_id],
+                body1,
+                body2,
+                coupled_axes,
+                &joint.motors[first_coupled_lin_axis_id].motor_params(params.dt),
+                limits,
+                WritebackId::Motor(first_coupled_lin_axis_id),
+            );
+            len += 1;
         }
 
         JointTwoBodyConstraintHelper::finalize_constraints(&mut out[start..len]);
@@ -350,6 +348,7 @@ impl JointTwoBodyConstraint<Real, 1> {
         }
     }
 }
+
 #[cfg(feature = "simd-is-enabled")]
 impl JointTwoBodyConstraint<SimdReal, SIMD_WIDTH> {
     pub fn lock_axes(
diff --git a/src/dynamics/solver/solver_constraints_set.rs b/src/dynamics/solver/solver_constraints_set.rs
index 4404241..1161c55 100644
--- a/src/dynamics/solver/solver_constraints_set.rs
+++ b/src/dynamics/solver/solver_constraints_set.rs
@@ -25,6 +25,7 @@ pub(crate) trait ConstraintTypes {
     type SimdBuilderTwoBodies;
 }
 
+#[derive(Debug)]
 pub enum AnyConstraintMut<'a, Constraints: ConstraintTypes> {
     OneBody(&'a mut Constraints::OneBody),
     TwoBodies(&'a mut Constraints::TwoBodies),
@@ -95,7 +96,7 @@ impl<Constraints: ConstraintTypes> SolverConstraintsSet<Constraints> {
         }
     }
 
-    #[allow(dead_code)] // Useful for debuging.
+    #[allow(dead_code)] // Useful for debugging.
     pub fn print_counts(&self) {
         println!("Solver constraints:");
         println!(
diff --git a/src/dynamics/solver/solver_vel.rs b/src/dynamics/solver/solver_vel.rs
index 48c76b8..da69fb8 100644
--- a/src/dynamics/solver/solver_vel.rs
+++ b/src/dynamics/solver/solver_vel.rs
@@ -1,7 +1,7 @@
 use crate::math::{AngVector, Vector, SPATIAL_DIM};
 use crate::utils::SimdRealCopy;
 use na::{DVectorView, DVectorViewMut, Scalar};
-use std::ops::{AddAssign, Sub};
+use std::ops::{AddAssign, Sub, SubAssign};
 
 #[derive(Copy, Clone, Debug, Default)]
 #[repr(C)]
@@ -47,6 +47,13 @@ impl<N: SimdRealCopy> AddAssign for SolverVel<N> {
     }
 }
 
+impl<N: SimdRealCopy> SubAssign for SolverVel<N> {
+    fn sub_assign(&mut self, rhs: Self) {
+        self.linear -= rhs.linear;
+        self.angular -= rhs.angular;
+    }
+}
+
 impl<N: SimdRealCopy> Sub for SolverVel<N> {
     type Output = Self;
 
diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs
index 928b427..a4e03b2 100644
--- a/src/dynamics/solver/velocity_solver.rs
+++ b/src/dynamics/solver/velocity_solver.rs
@@ -178,6 +178,10 @@ impl VelocitySolver {
             joint_constraints.update(params, multibodies, &self.solver_bodies);
             contact_constraints.update(params, substep_id, multibodies, &self.solver_bodies);
 
+            if params.warmstart_coefficient != 0.0 {
+                contact_constraints.warmstart(&mut self.solver_vels, &mut self.generic_solver_vels);
+            }
+
             for _ in 0..params.num_internal_pgs_iterations {
                 joint_constraints.solve(&mut self.solver_vels, &mut self.generic_solver_vels);
                 contact_constraints
@@ -201,9 +205,19 @@ impl VelocitySolver {
             /*
              * Resolution without bias.
              */
-            joint_constraints.solve_wo_bias(&mut self.solver_vels, &mut self.generic_solver_vels);
-            contact_constraints
-                .solve_restitution_wo_bias(&mut self.solver_vels, &mut self.generic_solver_vels);
+            if params.num_internal_stabilization_iterations > 0 {
+                for _ in 0..params.num_internal_stabilization_iterations {
+                    joint_constraints
+                        .solve_wo_bias(&mut self.solver_vels, &mut self.generic_solver_vels);
+                    contact_constraints.solve_restitution_wo_bias(
+                        &mut self.solver_vels,
+                        &mut self.generic_solver_vels,
+                    );
+                }
+
+                contact_constraints
+                    .solve_friction(&mut self.solver_vels, &mut self.generic_solver_vels);
+            }
         }
     }
 
@@ -239,8 +253,9 @@ impl VelocitySolver {
                 .rows(multibody.solver_id, multibody.ndofs());
             multibody.velocities.copy_from(&solver_vels);
             multibody.integrate(params.dt);
-            // PERF: we could have a mode where it doesn’t write back to the `bodies` yet.
-            multibody.forward_kinematics(bodies, !is_last_substep);
+            // PERF: don’t write back to the rigid-body poses `bodies` before the last step?
+            multibody.forward_kinematics(bodies, false);
+            multibody.update_rigid_bodies_internal(bodies, !is_last_substep, true, false);
 
             if !is_last_substep {
                 // These are very expensive and not needed if we don’t
diff --git a/src/geometry/broad_phase.rs b/src/geometry/broad_phase.rs
new file mode 100644
index 0000000..06164a1
--- /dev/null
+++ b/src/geometry/broad_phase.rs
@@ -0,0 +1,50 @@
+use crate::dynamics::RigidBodySet;
+use crate::geometry::{BroadPhasePairEvent, ColliderHandle, ColliderSet};
+use parry::math::Real;
+
+/// An internal index stored in colliders by some broad-phase algorithms.
+pub type BroadPhaseProxyIndex = u32;
+
+/// Trait implemented by broad-phase algorithms supported by Rapier.
+///
+/// The task of a broad-phase algorithm is to detect potential collision pairs, usually based on
+/// bounding volumes. The pairs must be conservative: it is OK to create a collision pair if
+/// two objects don’t actually touch, but it is incorrect to remove a pair between two objects
+/// that are still touching. In other words, it can have false-positive (though these induce
+/// some computational overhead on the narrow-phase), but cannot have false-negative.
+pub trait BroadPhase: Send + Sync + 'static {
+    /// Updates the broad-phase.
+    ///
+    /// The results must be output through the `events` struct. The broad-phase algorithm is only
+    /// required to generate new events (i.e. no need to re-send an `AddPair` event if it was already
+    /// sent previously and no `RemovePair` happened since then). Sending redundant events is allowed
+    /// but can result in a slight computational overhead.
+    ///
+    /// The `colliders` set is mutable only to provide access to
+    /// [`collider.set_internal_broad_phase_proxy_index`]. Other properties of the collider should
+    /// **not** be modified during the broad-phase update.
+    ///
+    /// # Parameters
+    /// - `prediction_distance`: colliders that are not exactly touching, but closer to this
+    ///   distance must form a collision pair.
+    /// - `colliders`: the set of colliders. Change detection with `collider.needs_broad_phase_update()`
+    ///   can be relied on at this stage.
+    /// - `modified_colliders`: colliders that are know to be modified since the last update.
+    /// - `removed_colliders`: colliders that got removed since the last update. Any associated data
+    ///   in the broad-phase should be removed by this call to `update`.
+    /// - `events`: the broad-phase’s output. They indicate what collision pairs need to be created
+    ///   and what pairs need to be removed. It is OK to create pairs for colliders that don’t
+    ///   actually collide (though this can increase computational overhead in the narrow-phase)
+    ///   but it is important not to indicate removal of a collision pair if the underlying colliders
+    ///   are still touching or closer than `prediction_distance`.
+    fn update(
+        &mut self,
+        dt: Real,
+        prediction_distance: Real,
+        colliders: &mut ColliderSet,
+        bodies: &RigidBodySet,
+        modified_colliders: &[ColliderHandle],
+        removed_colliders: &[ColliderHandle],
+        events: &mut Vec<BroadPhasePairEvent>,
+    );
+}
diff --git a/src/geometry/broad_phase_multi_sap/broad_phase.rs b/src/geometry/broad_phase_multi_sap/broad_phase_multi_sap.rs
similarity index 93%
rename from src/geometry/broad_phase_multi_sap/broad_phase.rs
rename to src/geometry/broad_phase_multi_sap/broad_phase_multi_sap.rs
index 9e1bc06..d382258 100644
--- a/src/geometry/broad_phase_multi_sap/broad_phase.rs
+++ b/src/geometry/broad_phase_multi_sap/broad_phase_multi_sap.rs
@@ -1,12 +1,12 @@
 use super::{
     BroadPhasePairEvent, ColliderPair, SAPLayer, SAPProxies, SAPProxy, SAPProxyData, SAPRegionPool,
 };
-use crate::geometry::broad_phase_multi_sap::SAPProxyIndex;
 use crate::geometry::{
-    ColliderBroadPhaseData, ColliderChanges, ColliderHandle, ColliderPosition, ColliderSet,
-    ColliderShape,
+    BroadPhaseProxyIndex, Collider, ColliderBroadPhaseData, ColliderChanges, ColliderHandle,
+    ColliderSet,
 };
-use crate::math::Real;
+use crate::math::{Isometry, Real};
+use crate::prelude::{BroadPhase, RigidBodySet};
 use crate::utils::IndexMut2;
 use parry::bounding_volume::BoundingVolume;
 use parry::utils::hashmap::HashMap;
@@ -74,7 +74,7 @@ use parry::utils::hashmap::HashMap;
 ///   broad-phase, as well as the Aabbs of all the regions part of this broad-phase.
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone)]
-pub struct BroadPhase {
+pub struct BroadPhaseMultiSap {
     proxies: SAPProxies,
     layers: Vec<SAPLayer>,
     smallest_layer: u8,
@@ -90,7 +90,7 @@ pub struct BroadPhase {
     //       Another alternative would be to remove ColliderProxyId and
     //       just use a Coarena. But this seems like it could use too
     //       much memory.
-    colliders_proxy_ids: HashMap<ColliderHandle, SAPProxyIndex>,
+    colliders_proxy_ids: HashMap<ColliderHandle, BroadPhaseProxyIndex>,
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
     region_pool: SAPRegionPool, // To avoid repeated allocations.
     // We could think serializing this workspace is useless.
@@ -114,16 +114,16 @@ pub struct BroadPhase {
     reporting: HashMap<(u32, u32), bool>, // Workspace
 }
 
-impl Default for BroadPhase {
+impl Default for BroadPhaseMultiSap {
     fn default() -> Self {
         Self::new()
     }
 }
 
-impl BroadPhase {
+impl BroadPhaseMultiSap {
     /// Create a new empty broad-phase.
     pub fn new() -> Self {
-        BroadPhase {
+        BroadPhaseMultiSap {
             proxies: SAPProxies::new(),
             layers: Vec::new(),
             smallest_layer: 0,
@@ -138,7 +138,7 @@ impl BroadPhase {
     ///
     /// For each colliders marked as removed, we make their containing layer mark
     /// its proxy as pre-deleted. The actual proxy removal will happen at the end
-    /// of the `BroadPhase::update`.
+    /// of the `BroadPhaseMultiSap::update`.
     fn handle_removed_colliders(&mut self, removed_colliders: &[ColliderHandle]) {
         // For each removed collider, remove the corresponding proxy.
         for removed in removed_colliders {
@@ -156,7 +156,7 @@ impl BroadPhase {
     /// remove, the `complete_removal` method MUST be called to
     /// complete the removal of these proxies, by actually removing them
     /// from all the relevant layers/regions/axes.
-    fn predelete_proxy(&mut self, proxy_index: SAPProxyIndex) {
+    fn predelete_proxy(&mut self, proxy_index: BroadPhaseProxyIndex) {
         if proxy_index == crate::INVALID_U32 {
             // This collider has not been added to the broad-phase yet.
             return;
@@ -353,13 +353,18 @@ impl BroadPhase {
         prediction_distance: Real,
         handle: ColliderHandle,
         proxy_index: &mut u32,
-        collider: (&ColliderPosition, &ColliderShape, &ColliderChanges),
+        collider: &Collider,
+        next_position: Option<&Isometry<Real>>,
     ) -> bool {
-        let (co_pos, co_shape, co_changes) = collider;
+        let mut aabb = collider.compute_collision_aabb(prediction_distance / 2.0);
 
-        let mut aabb = co_shape
-            .compute_aabb(co_pos)
-            .loosened(prediction_distance / 2.0);
+        if let Some(next_position) = next_position {
+            let next_aabb = collider
+                .shape
+                .compute_aabb(next_position)
+                .loosened(collider.contact_skin() + prediction_distance / 2.0);
+            aabb.merge(&next_aabb);
+        }
 
         if aabb.mins.coords.iter().any(|e| !e.is_finite())
             || aabb.maxs.coords.iter().any(|e| !e.is_finite())
@@ -378,7 +383,7 @@ impl BroadPhase {
             prev_aabb = proxy.aabb;
             proxy.aabb = aabb;
 
-            if co_changes.contains(ColliderChanges::SHAPE) {
+            if collider.changes.contains(ColliderChanges::SHAPE) {
                 // If the shape was changed, then we need to see if this proxy should be
                 // migrated to a larger layer. Indeed, if the shape was replaced by
                 // a much larger shape, we need to promote the proxy to a bigger layer
@@ -449,65 +454,6 @@ impl BroadPhase {
         !layer.created_regions.is_empty()
     }
 
-    /// Updates the broad-phase, taking into account the new collider positions.
-    pub fn update(
-        &mut self,
-        prediction_distance: Real,
-        colliders: &mut ColliderSet,
-        modified_colliders: &[ColliderHandle],
-        removed_colliders: &[ColliderHandle],
-        events: &mut Vec<BroadPhasePairEvent>,
-    ) {
-        // Phase 1: pre-delete the collisions that have been deleted.
-        self.handle_removed_colliders(removed_colliders);
-
-        let mut need_region_propagation = false;
-
-        // Phase 2: pre-delete the collisions that have been deleted.
-        for handle in modified_colliders {
-            // NOTE: we use `get` because the collider may no longer
-            //       exist if it has been removed.
-            if let Some(co) = colliders.get_mut_internal(*handle) {
-                if !co.is_enabled() || !co.changes.needs_broad_phase_update() {
-                    continue;
-                }
-
-                let mut new_proxy_id = co.bf_data.proxy_index;
-
-                if self.handle_modified_collider(
-                    prediction_distance,
-                    *handle,
-                    &mut new_proxy_id,
-                    (&co.pos, &co.shape, &co.changes),
-                ) {
-                    need_region_propagation = true;
-                }
-
-                if co.bf_data.proxy_index != new_proxy_id {
-                    self.colliders_proxy_ids.insert(*handle, new_proxy_id);
-
-                    // Make sure we have the new proxy index in case
-                    // the collider was added for the first time.
-                    co.bf_data = ColliderBroadPhaseData {
-                        proxy_index: new_proxy_id,
-                    };
-                }
-            }
-        }
-
-        // Phase 3: bottom-up pass to propagate new regions from smaller layers to larger layers.
-        if need_region_propagation {
-            self.propagate_created_regions();
-        }
-
-        // Phase 4: top-down pass to propagate proxies from larger layers to smaller layers.
-        self.update_layers_and_find_pairs(events);
-
-        // Phase 5: bottom-up pass to remove proxies, and propagate region removed from smaller
-        // layers to possible remove regions from larger layers that would become empty that way.
-        self.complete_removals(colliders, removed_colliders);
-    }
-
     /// Propagate regions from the smallest layers up to the larger layers.
     ///
     /// Whenever a region is created on a layer `n`, then its Aabb must be
@@ -618,16 +564,90 @@ impl BroadPhase {
     }
 }
 
+impl BroadPhase for BroadPhaseMultiSap {
+    /// Updates the broad-phase, taking into account the new collider positions.
+    fn update(
+        &mut self,
+        dt: Real,
+        prediction_distance: Real,
+        colliders: &mut ColliderSet,
+        bodies: &RigidBodySet,
+        modified_colliders: &[ColliderHandle],
+        removed_colliders: &[ColliderHandle],
+        events: &mut Vec<BroadPhasePairEvent>,
+    ) {
+        // Phase 1: pre-delete the collisions that have been deleted.
+        self.handle_removed_colliders(removed_colliders);
+
+        let mut need_region_propagation = false;
+
+        // Phase 2: pre-delete the collisions that have been deleted.
+        for handle in modified_colliders {
+            // NOTE: we use `get` because the collider may no longer
+            //       exist if it has been removed.
+            if let Some(co) = colliders.get_mut_internal(*handle) {
+                if !co.is_enabled() || !co.changes.needs_broad_phase_update() {
+                    continue;
+                }
+
+                let mut new_proxy_id = co.bf_data.proxy_index;
+
+                let next_pos = co.parent.and_then(|p| {
+                    let parent = bodies.get(p.handle)?;
+                    (parent.soft_ccd_prediction() > 0.0).then(|| {
+                        parent.predict_position_using_velocity_and_forces_with_max_dist(
+                            dt,
+                            parent.soft_ccd_prediction(),
+                        ) * p.pos_wrt_parent
+                    })
+                });
+
+                if self.handle_modified_collider(
+                    prediction_distance,
+                    *handle,
+                    &mut new_proxy_id,
+                    co,
+                    next_pos.as_ref(),
+                ) {
+                    need_region_propagation = true;
+                }
+
+                if co.bf_data.proxy_index != new_proxy_id {
+                    self.colliders_proxy_ids.insert(*handle, new_proxy_id);
+
+                    // Make sure we have the new proxy index in case
+                    // the collider was added for the first time.
+                    co.bf_data = ColliderBroadPhaseData {
+                        proxy_index: new_proxy_id,
+                    };
+                }
+            }
+        }
+
+        // Phase 3: bottom-up pass to propagate new regions from smaller layers to larger layers.
+        if need_region_propagation {
+            self.propagate_created_regions();
+        }
+
+        // Phase 4: top-down pass to propagate proxies from larger layers to smaller layers.
+        self.update_layers_and_find_pairs(events);
+
+        // Phase 5: bottom-up pass to remove proxies, and propagate region removed from smaller
+        // layers to possible remove regions from larger layers that would become empty that way.
+        self.complete_removals(colliders, removed_colliders);
+    }
+}
+
 #[cfg(test)]
 mod test {
     use crate::dynamics::{
         ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBodyBuilder, RigidBodySet,
     };
-    use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet};
+    use crate::geometry::{BroadPhase, BroadPhaseMultiSap, ColliderBuilder, ColliderSet};
 
     #[test]
     fn test_add_update_remove() {
-        let mut broad_phase = BroadPhase::new();
+        let mut broad_phase = BroadPhaseMultiSap::new();
         let mut bodies = RigidBodySet::new();
         let mut colliders = ColliderSet::new();
         let mut impulse_joints = ImpulseJointSet::new();
@@ -640,7 +660,7 @@ mod test {
         let coh = colliders.insert_with_parent(co, hrb, &mut bodies);
 
         let mut events = Vec::new();
-        broad_phase.update(0.0, &mut colliders, &[coh], &[], &mut events);
+        broad_phase.update(0.0, 0.0, &mut colliders, &bodies, &[coh], &[], &mut events);
 
         bodies.remove(
             hrb,
@@ -650,7 +670,7 @@ mod test {
             &mut multibody_joints,
             true,
         );
-        broad_phase.update(0.0, &mut colliders, &[], &[coh], &mut events);
+        broad_phase.update(0.0, 0.0, &mut colliders, &bodies, &[], &[coh], &mut events);
 
         // Create another body.
         let rb = RigidBodyBuilder::dynamic().build();
@@ -659,6 +679,6 @@ mod test {
         let coh = colliders.insert_with_parent(co, hrb, &mut bodies);
 
         // Make sure the proxy handles is recycled properly.
-        broad_phase.update(0.0, &mut colliders, &[coh], &[], &mut events);
+        broad_phase.update(0.0, 0.0, &mut colliders, &bodies, &[coh], &[], &mut events);
     }
 }
diff --git a/src/geometry/broad_phase_multi_sap/mod.rs b/src/geometry/broad_phase_multi_sap/mod.rs
index 0f85e96..b9b3097 100644
--- a/src/geometry/broad_phase_multi_sap/mod.rs
+++ b/src/geometry/broad_phase_multi_sap/mod.rs
@@ -1,6 +1,5 @@
-pub use self::broad_phase::BroadPhase;
+pub use self::broad_phase_multi_sap::BroadPhaseMultiSap;
 pub use self::broad_phase_pair_event::{BroadPhasePairEvent, ColliderPair};
-pub use self::sap_proxy::SAPProxyIndex;
 
 use self::sap_axis::*;
 use self::sap_endpoint::*;
@@ -9,7 +8,7 @@ use self::sap_proxy::*;
 use self::sap_region::*;
 use self::sap_utils::*;
 
-mod broad_phase;
+mod broad_phase_multi_sap;
 mod broad_phase_pair_event;
 mod sap_axis;
 mod sap_endpoint;
diff --git a/src/geometry/broad_phase_multi_sap/sap_axis.rs b/src/geometry/broad_phase_multi_sap/sap_axis.rs
index 2452148..f1afdee 100644
--- a/src/geometry/broad_phase_multi_sap/sap_axis.rs
+++ b/src/geometry/broad_phase_multi_sap/sap_axis.rs
@@ -1,6 +1,6 @@
 use super::{SAPEndpoint, SAPProxies, NUM_SENTINELS};
 use crate::geometry::broad_phase_multi_sap::DELETED_AABB_VALUE;
-use crate::geometry::SAPProxyIndex;
+use crate::geometry::BroadPhaseProxyIndex;
 use crate::math::Real;
 use bit_vec::BitVec;
 use parry::bounding_volume::BoundingVolume;
@@ -39,7 +39,7 @@ impl SAPAxis {
     pub fn batch_insert(
         &mut self,
         dim: usize,
-        new_proxies: &[SAPProxyIndex],
+        new_proxies: &[BroadPhaseProxyIndex],
         proxies: &SAPProxies,
         reporting: Option<&mut HashMap<(u32, u32), bool>>,
     ) {
diff --git a/src/geometry/broad_phase_multi_sap/sap_layer.rs b/src/geometry/broad_phase_multi_sap/sap_layer.rs
index 2266d56..f1a3a31 100644
--- a/src/geometry/broad_phase_multi_sap/sap_layer.rs
+++ b/src/geometry/broad_phase_multi_sap/sap_layer.rs
@@ -1,6 +1,6 @@
 use super::{SAPProxies, SAPProxy, SAPRegion, SAPRegionPool};
 use crate::geometry::broad_phase_multi_sap::DELETED_AABB_VALUE;
-use crate::geometry::{Aabb, SAPProxyIndex};
+use crate::geometry::{Aabb, BroadPhaseProxyIndex};
 use crate::math::{Point, Real};
 use parry::bounding_volume::BoundingVolume;
 use parry::utils::hashmap::{Entry, HashMap};
@@ -13,11 +13,11 @@ pub(crate) struct SAPLayer {
     pub smaller_layer: Option<u8>,
     pub larger_layer: Option<u8>,
     region_width: Real,
-    pub regions: HashMap<Point<i32>, SAPProxyIndex>,
+    pub regions: HashMap<Point<i32>, BroadPhaseProxyIndex>,
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
     regions_to_potentially_remove: Vec<Point<i32>>, // Workspace
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
-    pub created_regions: Vec<SAPProxyIndex>,
+    pub created_regions: Vec<BroadPhaseProxyIndex>,
 }
 
 impl SAPLayer {
@@ -71,7 +71,7 @@ impl SAPLayer {
     ///
     /// This method must be called in a bottom-up loop, propagating new regions from the
     /// smallest layer, up to the largest layer. That loop is done by the Phase 3 of the
-    /// BroadPhase::update.
+    /// BroadPhaseMultiSap::update.
     pub fn propagate_created_regions(
         &mut self,
         larger_layer: &mut Self,
@@ -103,7 +103,7 @@ impl SAPLayer {
     /// one region on its parent "larger" layer.
     fn register_subregion(
         &mut self,
-        proxy_id: SAPProxyIndex,
+        proxy_id: BroadPhaseProxyIndex,
         proxies: &mut SAPProxies,
         pool: &mut SAPRegionPool,
     ) {
@@ -140,7 +140,7 @@ impl SAPLayer {
 
     fn unregister_subregion(
         &mut self,
-        proxy_id: SAPProxyIndex,
+        proxy_id: BroadPhaseProxyIndex,
         proxy_region: &SAPRegion,
         proxies: &mut SAPProxies,
     ) {
@@ -182,7 +182,7 @@ impl SAPLayer {
     /// If the region with the given region key does not exist yet, it is created.
     /// When a region is created, it creates a new proxy for that region, and its
     /// proxy ID is added to `self.created_region` so it can be propagated during
-    /// the Phase 3 of `BroadPhase::update`.
+    /// the Phase 3 of `BroadPhaseMultiSap::update`.
     ///
     /// This returns the proxy ID of the already existing region if it existed, or
     /// of the new region if it did not exist and has been created by this method.
@@ -191,7 +191,7 @@ impl SAPLayer {
         region_key: Point<i32>,
         proxies: &mut SAPProxies,
         pool: &mut SAPRegionPool,
-    ) -> SAPProxyIndex {
+    ) -> BroadPhaseProxyIndex {
         match self.regions.entry(region_key) {
             // Yay, the region already exists!
             Entry::Occupied(occupied) => *occupied.get(),
@@ -266,7 +266,7 @@ impl SAPLayer {
         }
     }
 
-    pub fn predelete_proxy(&mut self, proxies: &mut SAPProxies, proxy_index: SAPProxyIndex) {
+    pub fn predelete_proxy(&mut self, proxies: &mut SAPProxies, proxy_index: BroadPhaseProxyIndex) {
         // Discretize the Aabb to find the regions that need to be invalidated.
         let proxy_aabb = &mut proxies[proxy_index].aabb;
         let start = super::point_key(proxy_aabb.mins, self.region_width);
@@ -379,7 +379,7 @@ impl SAPLayer {
     pub fn proper_proxy_moved_to_bigger_layer(
         &mut self,
         proxies: &mut SAPProxies,
-        proxy_id: SAPProxyIndex,
+        proxy_id: BroadPhaseProxyIndex,
     ) {
         for (point, region_id) in &self.regions {
             let region = &mut proxies[*region_id].data.as_region_mut();
diff --git a/src/geometry/broad_phase_multi_sap/sap_proxy.rs b/src/geometry/broad_phase_multi_sap/sap_proxy.rs
index 4d5d79e..ccc172f 100644
--- a/src/geometry/broad_phase_multi_sap/sap_proxy.rs
+++ b/src/geometry/broad_phase_multi_sap/sap_proxy.rs
@@ -1,11 +1,9 @@
 use super::NEXT_FREE_SENTINEL;
 use crate::geometry::broad_phase_multi_sap::SAPRegion;
-use crate::geometry::ColliderHandle;
+use crate::geometry::{BroadPhaseProxyIndex, ColliderHandle};
 use parry::bounding_volume::Aabb;
 use std::ops::{Index, IndexMut};
 
-pub type SAPProxyIndex = u32;
-
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone)]
 pub enum SAPProxyData {
@@ -49,7 +47,7 @@ impl SAPProxyData {
 pub struct SAPProxy {
     pub data: SAPProxyData,
     pub aabb: Aabb,
-    pub next_free: SAPProxyIndex,
+    pub next_free: BroadPhaseProxyIndex,
     // TODO: pack the layer_id and layer_depth into a single u16?
     pub layer_id: u8,
     pub layer_depth: i8,
@@ -81,7 +79,7 @@ impl SAPProxy {
 #[derive(Clone)]
 pub struct SAPProxies {
     pub elements: Vec<SAPProxy>,
-    pub first_free: SAPProxyIndex,
+    pub first_free: BroadPhaseProxyIndex,
 }
 
 impl Default for SAPProxies {
@@ -98,7 +96,7 @@ impl SAPProxies {
         }
     }
 
-    pub fn insert(&mut self, proxy: SAPProxy) -> SAPProxyIndex {
+    pub fn insert(&mut self, proxy: SAPProxy) -> BroadPhaseProxyIndex {
         if self.first_free != NEXT_FREE_SENTINEL {
             let proxy_id = self.first_free;
             self.first_free = self.elements[proxy_id as usize].next_free;
@@ -110,31 +108,31 @@ impl SAPProxies {
         }
     }
 
-    pub fn remove(&mut self, proxy_id: SAPProxyIndex) {
+    pub fn remove(&mut self, proxy_id: BroadPhaseProxyIndex) {
         let proxy = &mut self.elements[proxy_id as usize];
         proxy.next_free = self.first_free;
         self.first_free = proxy_id;
     }
 
     // NOTE: this must not take holes into account.
-    pub fn get_mut(&mut self, i: SAPProxyIndex) -> Option<&mut SAPProxy> {
+    pub fn get_mut(&mut self, i: BroadPhaseProxyIndex) -> Option<&mut SAPProxy> {
         self.elements.get_mut(i as usize)
     }
     // NOTE: this must not take holes into account.
-    pub fn get(&self, i: SAPProxyIndex) -> Option<&SAPProxy> {
+    pub fn get(&self, i: BroadPhaseProxyIndex) -> Option<&SAPProxy> {
         self.elements.get(i as usize)
     }
 }
 
-impl Index<SAPProxyIndex> for SAPProxies {
+impl Index<BroadPhaseProxyIndex> for SAPProxies {
     type Output = SAPProxy;
-    fn index(&self, i: SAPProxyIndex) -> &SAPProxy {
+    fn index(&self, i: BroadPhaseProxyIndex) -> &SAPProxy {
         self.elements.index(i as usize)
     }
 }
 
-impl IndexMut<SAPProxyIndex> for SAPProxies {
-    fn index_mut(&mut self, i: SAPProxyIndex) -> &mut SAPProxy {
+impl IndexMut<BroadPhaseProxyIndex> for SAPProxies {
+    fn index_mut(&mut self, i: BroadPhaseProxyIndex) -> &mut SAPProxy {
         self.elements.index_mut(i as usize)
     }
 }
diff --git a/src/geometry/broad_phase_multi_sap/sap_region.rs b/src/geometry/broad_phase_multi_sap/sap_region.rs
index 21ebca5..7e38eaa 100644
--- a/src/geometry/broad_phase_multi_sap/sap_region.rs
+++ b/src/geometry/broad_phase_multi_sap/sap_region.rs
@@ -1,5 +1,5 @@
 use super::{SAPAxis, SAPProxies};
-use crate::geometry::SAPProxyIndex;
+use crate::geometry::BroadPhaseProxyIndex;
 use crate::math::DIM;
 use bit_vec::BitVec;
 use parry::bounding_volume::Aabb;
@@ -13,8 +13,8 @@ pub struct SAPRegion {
     pub axes: [SAPAxis; DIM],
     pub existing_proxies: BitVec,
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
-    pub to_insert: Vec<SAPProxyIndex>, // Workspace
-    pub subregions: Vec<SAPProxyIndex>,
+    pub to_insert: Vec<BroadPhaseProxyIndex>, // Workspace
+    pub subregions: Vec<BroadPhaseProxyIndex>,
     pub id_in_parent_subregion: u32,
     pub update_count: u8,
     pub needs_update_after_subregion_removal: bool,
@@ -90,7 +90,7 @@ impl SAPRegion {
     /// If this region contains the given proxy, this will decrement this region's proxy count.
     ///
     /// Returns `true` if this region contained the proxy. Returns `false` otherwise.
-    pub fn proper_proxy_moved_to_a_bigger_layer(&mut self, proxy_id: SAPProxyIndex) -> bool {
+    pub fn proper_proxy_moved_to_a_bigger_layer(&mut self, proxy_id: BroadPhaseProxyIndex) -> bool {
         if self.existing_proxies.get(proxy_id as usize) == Some(true) {
             // NOTE: we are just registering the fact that that proxy isn't a
             //       subproper proxy anymore. But it is still part of this region
@@ -142,7 +142,7 @@ impl SAPRegion {
         self.subproper_proxy_count -= num_deleted_subregion_endpoints[0] / 2;
     }
 
-    pub fn predelete_proxy(&mut self, _proxy_id: SAPProxyIndex) {
+    pub fn predelete_proxy(&mut self, _proxy_id: BroadPhaseProxyIndex) {
         // We keep the proxy_id as argument for uniformity with the "preupdate"
         // method. However we don't actually need it because the deletion will be
         // handled transparently during the next update.
@@ -153,14 +153,18 @@ impl SAPRegion {
         self.update_count = self.update_count.max(1);
     }
 
-    pub fn register_subregion(&mut self, proxy_id: SAPProxyIndex) -> usize {
+    pub fn register_subregion(&mut self, proxy_id: BroadPhaseProxyIndex) -> usize {
         let subregion_index = self.subregions.len();
         self.subregions.push(proxy_id);
         self.preupdate_proxy(proxy_id, true);
         subregion_index
     }
 
-    pub fn preupdate_proxy(&mut self, proxy_id: SAPProxyIndex, is_subproper_proxy: bool) -> bool {
+    pub fn preupdate_proxy(
+        &mut self,
+        proxy_id: BroadPhaseProxyIndex,
+        is_subproper_proxy: bool,
+    ) -> bool {
         let mask_len = self.existing_proxies.len();
         if proxy_id as usize >= mask_len {
             self.existing_proxies
diff --git a/src/geometry/broad_phase_qbvh.rs b/src/geometry/broad_phase_qbvh.rs
index 22ca562..5fd6bcf 100644
--- a/src/geometry/broad_phase_qbvh.rs
+++ b/src/geometry/broad_phase_qbvh.rs
@@ -1,5 +1,4 @@
 use crate::geometry::{BroadPhasePairEvent, ColliderHandle, ColliderPair, ColliderSet};
-use parry::bounding_volume::BoundingVolume;
 use parry::math::Real;
 use parry::partitioning::Qbvh;
 use parry::partitioning::QbvhUpdateWorkspace;
@@ -7,20 +6,20 @@ use parry::query::visitors::BoundingVolumeIntersectionsSimultaneousVisitor;
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone)]
-pub struct BroadPhase {
+pub struct BroadPhaseQbvh {
     qbvh: Qbvh<ColliderHandle>,
     stack: Vec<(u32, u32)>,
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
     workspace: QbvhUpdateWorkspace,
 }
 
-impl Default for BroadPhase {
+impl Default for BroadPhaseQbvh {
     fn default() -> Self {
         Self::new()
     }
 }
 
-impl BroadPhase {
+impl BroadPhaseQbvh {
     pub fn new() -> Self {
         Self {
             qbvh: Qbvh::new(),
@@ -59,7 +58,7 @@ impl BroadPhase {
                 colliders.iter().map(|(handle, collider)| {
                     (
                         handle,
-                        collider.compute_aabb().loosened(prediction_distance / 2.0),
+                        collider.compute_collision_aabb(prediction_distance / 2.0),
                     )
                 }),
                 margin,
@@ -76,9 +75,7 @@ impl BroadPhase {
             }
 
             let _ = self.qbvh.refit(margin, &mut self.workspace, |handle| {
-                colliders[*handle]
-                    .compute_aabb()
-                    .loosened(prediction_distance / 2.0)
+                colliders[*handle].compute_collision_aabb(prediction_distance / 2.0)
             });
             self.qbvh
                 .traverse_modified_bvtt_with_stack(&self.qbvh, &mut visitor, &mut self.stack);
diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index 261de46..565d6e2 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -1,17 +1,20 @@
 use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle};
 use crate::geometry::{
-    ActiveCollisionTypes, ColliderBroadPhaseData, ColliderChanges, ColliderFlags,
-    ColliderMassProps, ColliderMaterial, ColliderParent, ColliderPosition, ColliderShape,
-    ColliderType, InteractionGroups, SharedShape,
+    ActiveCollisionTypes, BroadPhaseProxyIndex, ColliderBroadPhaseData, ColliderChanges,
+    ColliderFlags, ColliderMassProps, ColliderMaterial, ColliderParent, ColliderPosition,
+    ColliderShape, ColliderType, InteractionGroups, SharedShape,
 };
 use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector, DIM};
 use crate::parry::transformation::vhacd::VHACDParameters;
 use crate::pipeline::{ActiveEvents, ActiveHooks};
 use crate::prelude::ColliderEnabled;
 use na::Unit;
-use parry::bounding_volume::Aabb;
+use parry::bounding_volume::{Aabb, BoundingVolume};
 use parry::shape::{Shape, TriMeshFlags};
 
+#[cfg(feature = "dim3")]
+use crate::geometry::HeightFieldFlags;
+
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone)]
 /// A geometric entity that can be attached to a body so it can be affected by contacts and proximity queries.
@@ -27,6 +30,7 @@ pub struct Collider {
     pub(crate) material: ColliderMaterial,
     pub(crate) flags: ColliderFlags,
     pub(crate) bf_data: ColliderBroadPhaseData,
+    contact_skin: Real,
     contact_force_event_threshold: Real,
     /// User-defined data associated to this collider.
     pub user_data: u128,
@@ -50,6 +54,21 @@ impl Collider {
         }
     }
 
+    /// An internal index associated to this collider by the broad-phase algorithm.
+    pub fn internal_broad_phase_proxy_index(&self) -> BroadPhaseProxyIndex {
+        self.bf_data.proxy_index
+    }
+
+    /// Sets the internal index associated to this collider by the broad-phase algorithm.
+    ///
+    /// This must **not** be called, unless you are implementing your own custom broad-phase
+    /// that require storing an index in the collider struct.
+    /// Modifying that index outside of a custom broad-phase code will most certainly break
+    /// the physics engine.
+    pub fn set_internal_broad_phase_proxy_index(&mut self, id: BroadPhaseProxyIndex) {
+        self.bf_data.proxy_index = id;
+    }
+
     /// The rigid body this collider is attached to.
     pub fn parent(&self) -> Option<RigidBodyHandle> {
         self.parent.map(|parent| parent.handle)
@@ -60,6 +79,55 @@ impl Collider {
         self.coll_type.is_sensor()
     }
 
+    /// Copy all the characteristics from `other` to `self`.
+    ///
+    /// If you have a mutable reference to a collider `collider: &mut Collider`, attempting to
+    /// assign it a whole new collider instance, e.g., `*collider = ColliderBuilder::ball(0.5).build()`,
+    /// will crash due to some internal indices being overwritten. Instead, use
+    /// `collider.copy_from(&ColliderBuilder::ball(0.5).build())`.
+    ///
+    /// This method will allow you to set most characteristics of this collider from another
+    /// collider instance without causing any breakage.
+    ///
+    /// This method **cannot** be used for reparenting a collider. Therefore, the parent of the
+    /// `other` (if any), as well as its relative position to that parent will not be copied into
+    /// `self`.
+    ///
+    /// The pose of `other` will only copied into `self` if `self` doesn’t have a parent (if it has
+    /// a parent, its position is directly controlled by the parent rigid-body).
+    pub fn copy_from(&mut self, other: &Collider) {
+        // NOTE: we deconstruct the collider struct to be sure we don’t forget to
+        //       add some copies here if we add more field to Collider in the future.
+        let Collider {
+            coll_type,
+            shape,
+            mprops,
+            changes: _changes, // Will be set to ALL.
+            parent: _parent,   // This function cannot be used to reparent the collider.
+            pos,
+            material,
+            flags,
+            bf_data: _bf_data, // Internal ids must not be overwritten.
+            contact_force_event_threshold,
+            user_data,
+            contact_skin,
+        } = other;
+
+        if self.parent.is_none() {
+            self.pos = *pos;
+        }
+
+        self.coll_type = *coll_type;
+        self.shape = shape.clone();
+        self.mprops = mprops.clone();
+        self.material = *material;
+        self.contact_force_event_threshold = *contact_force_event_threshold;
+        self.user_data = *user_data;
+        self.flags = *flags;
+        self.changes = ColliderChanges::all();
+        self.contact_skin = *contact_skin;
+    }
+
     /// The physics hooks enabled for this collider.
     pub fn active_hooks(&self) -> ActiveHooks {
         self.flags.active_hooks
@@ -90,6 +158,20 @@ impl Collider {
         self.flags.active_collision_types = active_collision_types;
     }
 
+    /// The contact skin of this collider.
+    ///
+    /// See the documentation of [`ColliderBuilder::contact_skin`] for details.
+    pub fn contact_skin(&self) -> Real {
+        self.contact_skin
+    }
+
+    /// Sets the contact skin of this collider.
+    ///
+    /// See the documentation of [`ColliderBuilder::contact_skin`] for details.
+    pub fn set_contact_skin(&mut self, skin_thickness: Real) {
+        self.contact_skin = skin_thickness;
+    }
+
     /// The friction coefficient of this collider.
     pub fn friction(&self) -> Real {
         self.material.friction
@@ -224,7 +306,7 @@ impl Collider {
         }
     }
 
-    /// Sets the rotational part of this collider's rotaiton relative to its parent rigid-body.
+    /// Sets the rotational part of this collider's rotation relative to its parent rigid-body.
     pub fn set_rotation_wrt_parent(&mut self, rotation: AngVector<Real>) {
         if let Some(parent) = self.parent.as_mut() {
             self.changes.insert(ColliderChanges::PARENT);
@@ -372,10 +454,21 @@ impl Collider {
     }
 
     /// Compute the axis-aligned bounding box of this collider.
+    ///
+    /// This AABB doesn’t take into account the collider’s contact skin.
+    /// [`Collider::contact_skin`].
     pub fn compute_aabb(&self) -> Aabb {
         self.shape.compute_aabb(&self.pos)
     }
 
+    /// Compute the axis-aligned bounding box of this collider, taking into account the
+    /// [`Collider::contact_skin`] and prediction distance.
+    pub fn compute_collision_aabb(&self, prediction: Real) -> Aabb {
+        self.shape
+            .compute_aabb(&self.pos)
+            .loosened(self.contact_skin + prediction)
+    }
+
     /// Compute the axis-aligned bounding box of this collider moving from its current position
     /// to the given `next_position`
     pub fn compute_swept_aabb(&self, next_position: &Isometry<Real>) -> Aabb {
@@ -430,6 +523,8 @@ pub struct ColliderBuilder {
     pub enabled: bool,
     /// The total force magnitude beyond which a contact force event can be emitted.
     pub contact_force_event_threshold: Real,
+    /// An extra thickness around the collider shape to keep them further apart when colliding.
+    pub contact_skin: Real,
 }
 
 impl ColliderBuilder {
@@ -452,6 +547,7 @@ impl ColliderBuilder {
             active_events: ActiveEvents::empty(),
             enabled: true,
             contact_force_event_threshold: 0.0,
+            contact_skin: 0.0,
         }
     }
 
@@ -518,6 +614,15 @@ impl ColliderBuilder {
         Self::new(SharedShape::round_cuboid(hx, hy, border_radius))
     }
 
+    /// Initialize a new collider builder with a capsule defined from its endpoints.
+    ///
+    /// See also [`ColliderBuilder::capsule_x`], [`ColliderBuilder::capsule_y`], and
+    /// [`ColliderBuilder::capsule_z`], for a simpler way to build capsules with common
+    /// orientations.
+    pub fn capsule_from_endpoints(a: Point<Real>, b: Point<Real>, radius: Real) -> Self {
+        Self::new(SharedShape::capsule(a, b, radius))
+    }
+
     /// Initialize a new collider builder with a capsule shape aligned with the `x` axis.
     pub fn capsule_x(half_height: Real, radius: Real) -> Self {
         Self::new(SharedShape::capsule_x(half_height, radius))
@@ -698,6 +803,17 @@ impl ColliderBuilder {
         Self::new(SharedShape::heightfield(heights, scale))
     }
 
+    /// Initializes a collider builder with a heightfield shape defined by its set of height and a scale
+    /// factor along each coordinate axis.
+    #[cfg(feature = "dim3")]
+    pub fn heightfield_with_flags(
+        heights: na::DMatrix<Real>,
+        scale: Vector<Real>,
+        flags: HeightFieldFlags,
+    ) -> Self {
+        Self::new(SharedShape::heightfield_with_flags(heights, scale, flags))
+    }
+
     /// The default friction coefficient used by the collider builder.
     pub fn default_friction() -> Real {
         0.5
@@ -705,7 +821,7 @@ impl ColliderBuilder {
 
     /// The default density used by the collider builder.
     pub fn default_density() -> Real {
-        1.0
+        100.0
     }
 
     /// Sets an arbitrary user-defined 128-bit integer associated to the colliders built by this builder.
@@ -861,6 +977,20 @@ impl ColliderBuilder {
         self
     }
 
+    /// Sets the contact skin of the collider.
+    ///
+    /// The contact skin acts as if the collider was enlarged with a skin of width `skin_thickness`
+    /// around it, keeping objects further apart when colliding.
+    ///
+    /// A non-zero contact skin can increase performance, and in some cases, stability. However
+    /// it creates a small gap between colliding object (equal to the sum of their skin). If the
+    /// skin is sufficiently small, this might not be visually significant or can be hidden by the
+    /// rendering assets.
+    pub fn contact_skin(mut self, skin_thickness: Real) -> Self {
+        self.contact_skin = skin_thickness;
+        self
+    }
+
     /// Enable or disable the collider after its creation.
     pub fn enabled(mut self, enabled: bool) -> Self {
         self.enabled = enabled;
@@ -908,6 +1038,7 @@ impl ColliderBuilder {
             flags,
             coll_type,
             contact_force_event_threshold: self.contact_force_event_threshold,
+            contact_skin: self.contact_skin,
             user_data: self.user_data,
         }
     }
diff --git a/src/geometry/collider_components.rs b/src/geometry/collider_components.rs
index 5b907c1..123e32c 100644
--- a/src/geometry/collider_components.rs
+++ b/src/geometry/collider_components.rs
@@ -1,5 +1,5 @@
 use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle, RigidBodyType};
-use crate::geometry::{InteractionGroups, SAPProxyIndex, Shape, SharedShape};
+use crate::geometry::{BroadPhaseProxyIndex, InteractionGroups, Shape, SharedShape};
 use crate::math::{Isometry, Real};
 use crate::parry::partitioning::IndexedData;
 use crate::pipeline::{ActiveEvents, ActiveHooks};
@@ -118,7 +118,7 @@ impl ColliderType {
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 /// Data associated to a collider that takes part to a broad-phase algorithm.
 pub struct ColliderBroadPhaseData {
-    pub(crate) proxy_index: SAPProxyIndex,
+    pub(crate) proxy_index: BroadPhaseProxyIndex,
 }
 
 impl Default for ColliderBroadPhaseData {
diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs
index 44730a0..c57f98a 100644
--- a/src/geometry/contact_pair.rs
+++ b/src/geometry/contact_pair.rs
@@ -1,6 +1,6 @@
 use crate::dynamics::{RigidBodyHandle, RigidBodySet};
 use crate::geometry::{ColliderHandle, ColliderSet, Contact, ContactManifold};
-use crate::math::{Point, Real, Vector};
+use crate::math::{Point, Real, TangentImpulse, Vector};
 use crate::pipeline::EventHandler;
 use crate::prelude::CollisionEventFlags;
 use parry::query::ContactManifoldsWorkspace;
@@ -33,12 +33,11 @@ pub struct ContactData {
     pub impulse: Real,
     /// The friction impulse along the vector orthonormal to the contact normal, applied to the first
     /// collider's rigid-body.
-    #[cfg(feature = "dim2")]
-    pub tangent_impulse: Real,
-    /// The friction impulses along the basis orthonormal to the contact normal, applied to the first
-    /// collider's rigid-body.
-    #[cfg(feature = "dim3")]
-    pub tangent_impulse: na::Vector2<Real>,
+    pub tangent_impulse: TangentImpulse<Real>,
+    /// The impulse retained for warmstarting the next simulation step.
+    pub warmstart_impulse: Real,
+    /// The friction impulse retained for warmstarting the next simulation step.
+    pub warmstart_tangent_impulse: TangentImpulse<Real>,
 }
 
 impl Default for ContactData {
@@ -46,6 +45,8 @@ impl Default for ContactData {
         Self {
             impulse: 0.0,
             tangent_impulse: na::zero(),
+            warmstart_impulse: 0.0,
+            warmstart_tangent_impulse: na::zero(),
         }
     }
 }
@@ -57,14 +58,14 @@ pub struct IntersectionPair {
     /// Are the colliders intersecting?
     pub intersecting: bool,
     /// Was a `CollisionEvent::Started` emitted for this collider?
-    pub(crate) start_event_emited: bool,
+    pub(crate) start_event_emitted: bool,
 }
 
 impl IntersectionPair {
     pub(crate) fn new() -> Self {
         Self {
             intersecting: false,
-            start_event_emited: false,
+            start_event_emitted: false,
         }
     }
 
@@ -76,7 +77,7 @@ impl IntersectionPair {
         collider2: ColliderHandle,
         events: &dyn EventHandler,
     ) {
-        self.start_event_emited = true;
+        self.start_event_emitted = true;
         events.handle_collision_event(
             bodies,
             colliders,
@@ -93,7 +94,7 @@ impl IntersectionPair {
         collider2: ColliderHandle,
         events: &dyn EventHandler,
     ) {
-        self.start_event_emited = false;
+        self.start_event_emitted = false;
         events.handle_collision_event(
             bodies,
             colliders,
@@ -114,11 +115,14 @@ pub struct ContactPair {
     /// The set of contact manifolds between the two colliders.
     ///
     /// All contact manifold contain themselves contact points between the colliders.
+    /// Note that contact points in the contact manifold do not take into account the
+    /// [`Collider::contact_skin`] which only affects the constraint solver and the
+    /// [`SolverContact`].
     pub manifolds: Vec<ContactManifold>,
     /// Is there any active contact in this contact pair?
     pub has_any_active_contact: bool,
     /// Was a `CollisionEvent::Started` emitted for this collider?
-    pub(crate) start_event_emited: bool,
+    pub(crate) start_event_emitted: bool,
     pub(crate) workspace: Option<ContactManifoldsWorkspace>,
 }
 
@@ -129,7 +133,7 @@ impl ContactPair {
             collider2,
             has_any_active_contact: false,
             manifolds: Vec::new(),
-            start_event_emited: false,
+            start_event_emitted: false,
             workspace: None,
         }
     }
@@ -206,7 +210,7 @@ impl ContactPair {
         colliders: &ColliderSet,
         events: &dyn EventHandler,
     ) {
-        self.start_event_emited = true;
+        self.start_event_emitted = true;
 
         events.handle_collision_event(
             bodies,
@@ -222,7 +226,7 @@ impl ContactPair {
         colliders: &ColliderSet,
         events: &dyn EventHandler,
     ) {
-        self.start_event_emited = false;
+        self.start_event_emitted = false;
 
         events.handle_collision_event(
             bodies,
@@ -299,6 +303,10 @@ pub struct SolverContact {
     pub tangent_velocity: Vector<Real>,
     /// Whether or not this contact existed during the last timestep.
     pub is_new: bool,
+    /// Impulse used to warmstart the solve for the normal constraint.
+    pub warmstart_impulse: Real,
+    /// Impulse used to warmstart the solve for the friction constraints.
+    pub warmstart_tangent_impulse: TangentImpulse<Real>,
 }
 
 impl SolverContact {
@@ -351,16 +359,10 @@ impl ContactManifoldData {
 pub trait ContactManifoldExt {
     /// Computes the sum of all the impulses applied by contacts from this contact manifold.
     fn total_impulse(&self) -> Real;
-    /// Computes the maximum impulse applied by contacts from this contact manifold.
-    fn max_impulse(&self) -> Real;
 }
 
 impl ContactManifoldExt for ContactManifold {
     fn total_impulse(&self) -> Real {
         self.points.iter().map(|pt| pt.data.impulse).sum()
     }
-
-    fn max_impulse(&self) -> Real {
-        self.points.iter().fold(0.0, |a, pt| a.max(pt.data.impulse))
-    }
 }
diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs
index 1525211..1eaad79 100644
--- a/src/geometry/mod.rs
+++ b/src/geometry/mod.rs
@@ -1,9 +1,7 @@
 //! Structures related to geometry: colliders, shapes, etc.
 
-pub use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair};
-
-pub use self::broad_phase_multi_sap::BroadPhase;
-// pub use self::broad_phase_qbvh::BroadPhase;
+pub use self::broad_phase::BroadPhase;
+pub use self::broad_phase_multi_sap::{BroadPhaseMultiSap, BroadPhasePairEvent, ColliderPair};
 pub use self::collider_components::*;
 pub use self::contact_pair::{
     ContactData, ContactManifoldData, ContactPair, IntersectionPair, SolverContact, SolverFlags,
@@ -51,10 +49,12 @@ pub type Aabb = parry::bounding_volume::Aabb;
 pub type Ray = parry::query::Ray;
 /// The intersection between a ray and a  collider.
 pub type RayIntersection = parry::query::RayIntersection;
-/// The the projection of a point on a collider.
+/// The projection of a point on a collider.
 pub type PointProjection = parry::query::PointProjection;
-/// The the time of impact between two shapes.
-pub type TOI = parry::query::TOI;
+/// The result of a shape-cast between two shapes.
+pub type ShapeCastHit = parry::query::ShapeCastHit;
+/// The default broad-phase implementation recommended for general-purpose usage.
+pub type DefaultBroadPhase = BroadPhaseMultiSap;
 
 bitflags::bitflags! {
     /// Flags providing more information regarding a collision event.
@@ -180,7 +180,7 @@ impl ContactForceEvent {
     }
 }
 
-pub(crate) use self::broad_phase_multi_sap::SAPProxyIndex;
+pub(crate) use self::broad_phase::BroadPhaseProxyIndex;
 pub(crate) use self::narrow_phase::ContactManifoldIndex;
 pub(crate) use parry::partitioning::Qbvh;
 pub use parry::shape::*;
@@ -203,6 +203,7 @@ mod interaction_graph;
 mod interaction_groups;
 mod narrow_phase;
 
+mod broad_phase;
 mod broad_phase_qbvh;
 mod collider;
 mod collider_set;
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index c644b4d..1eddd73 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -8,9 +8,10 @@ use crate::dynamics::{
     RigidBodyType,
 };
 use crate::geometry::{
-    BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderPair,
-    ColliderSet, CollisionEvent, ContactData, ContactManifold, ContactManifoldData, ContactPair,
-    InteractionGraph, IntersectionPair, SolverContact, SolverFlags, TemporaryInteractionIndex,
+    BoundingVolume, BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle,
+    ColliderPair, ColliderSet, CollisionEvent, ContactData, ContactManifold, ContactManifoldData,
+    ContactPair, InteractionGraph, IntersectionPair, SolverContact, SolverFlags,
+    TemporaryInteractionIndex,
 };
 use crate::math::{Real, Vector};
 use crate::pipeline::{
@@ -342,7 +343,7 @@ impl NarrowPhase {
                     islands.wake_up(bodies, parent.handle, true)
                 }
 
-                if pair.start_event_emited {
+                if pair.start_event_emitted {
                     events.handle_collision_event(
                         bodies,
                         colliders,
@@ -354,7 +355,7 @@ impl NarrowPhase {
         } else {
             // If there is no island, don’t wake-up bodies, but do send the Stopped collision event.
             for (a, b, pair) in self.contact_graph.interactions_with(contact_graph_id) {
-                if pair.start_event_emited {
+                if pair.start_event_emitted {
                     events.handle_collision_event(
                         bodies,
                         colliders,
@@ -370,7 +371,7 @@ impl NarrowPhase {
             .intersection_graph
             .interactions_with(intersection_graph_id)
         {
-            if pair.start_event_emited {
+            if pair.start_event_emitted {
                 events.handle_collision_event(
                     bodies,
                     colliders,
@@ -709,7 +710,6 @@ impl NarrowPhase {
             let co1 = &colliders[handle1];
             let co2 = &colliders[handle2];
 
-            // TODO: remove the `loop` once labels on blocks is stabilized.
             'emit_events: {
                 if !co1.changes.needs_narrow_phase_update()
                     && !co2.changes.needs_narrow_phase_update()
@@ -767,7 +767,6 @@ impl NarrowPhase {
                 edge.weight.intersecting = query_dispatcher
                     .intersection_test(&pos12, &*co1.shape, &*co2.shape)
                     .unwrap_or(false);
-                break 'emit_events;
             }
 
             let active_events = co1.flags.active_events | co2.flags.active_events;
@@ -789,6 +788,7 @@ impl NarrowPhase {
     pub(crate) fn compute_contacts(
         &mut self,
         prediction_distance: Real,
+        dt: Real,
         bodies: &RigidBodySet,
         colliders: &ColliderSet,
         impulse_joints: &ImpulseJointSet,
@@ -810,7 +810,6 @@ impl NarrowPhase {
             let co1 = &colliders[pair.collider1];
             let co2 = &colliders[pair.collider2];
 
-            // TODO: remove the `loop` once labels on blocks are supported.
             'emit_events: {
                 if !co1.changes.needs_narrow_phase_update()
                     && !co2.changes.needs_narrow_phase_update()
@@ -819,17 +818,11 @@ impl NarrowPhase {
                     return;
                 }
 
-                // TODO: avoid lookup into bodies.
-                let mut rb_type1 = RigidBodyType::Fixed;
-                let mut rb_type2 = RigidBodyType::Fixed;
+                let rb1 = co1.parent.map(|co_parent1| &bodies[co_parent1.handle]);
+                let rb2 = co2.parent.map(|co_parent2| &bodies[co_parent2.handle]);
 
-                if let Some(co_parent1) = &co1.parent {
-                    rb_type1 = bodies[co_parent1.handle].body_type;
-                }
-
-                if let Some(co_parent2) = &co2.parent {
-                    rb_type2 = bodies[co_parent2.handle].body_type;
-                }
+                let rb_type1 = rb1.map(|rb| rb.body_type).unwrap_or(RigidBodyType::Fixed);
+                let rb_type2 = rb2.map(|rb| rb.body_type).unwrap_or(RigidBodyType::Fixed);
 
                 // Deal with contacts disabled between bodies attached by joints.
                 if let (Some(co_parent1), Some(co_parent2)) = (&co1.parent, &co2.parent) {
@@ -901,11 +894,37 @@ impl NarrowPhase {
                 }
 
                 let pos12 = co1.pos.inv_mul(&co2.pos);
+
+                let contact_skin_sum = co1.contact_skin() + co2.contact_skin();
+                let soft_ccd_prediction1 = rb1.map(|rb| rb.soft_ccd_prediction()).unwrap_or(0.0);
+                let soft_ccd_prediction2 = rb2.map(|rb| rb.soft_ccd_prediction()).unwrap_or(0.0);
+                let effective_prediction_distance = if soft_ccd_prediction1 > 0.0 || soft_ccd_prediction2 > 0.0 {
+                        let aabb1 = co1.compute_collision_aabb(0.0);
+                        let aabb2 = co2.compute_collision_aabb(0.0);
+                        let inv_dt = crate::utils::inv(dt);
+
+                        let linvel1 = rb1.map(|rb| rb.linvel()
+                            .cap_magnitude(soft_ccd_prediction1 * inv_dt)).unwrap_or_default();
+                        let linvel2 = rb2.map(|rb| rb.linvel()
+                            .cap_magnitude(soft_ccd_prediction2 * inv_dt)).unwrap_or_default();
+
+                        if !aabb1.intersects(&aabb2) && !aabb1.intersects_moving_aabb(&aabb2, linvel2 - linvel1) {
+                            pair.clear();
+                            break 'emit_events;
+                        }
+
+
+                    prediction_distance.max(
+                        dt * (linvel1 - linvel2).norm()) + contact_skin_sum
+                } else {
+                    prediction_distance + contact_skin_sum
+                };
+
                 let _ = query_dispatcher.contact_manifolds(
                     &pos12,
                     &*co1.shape,
                     &*co2.shape,
-                    prediction_distance,
+                    effective_prediction_distance,
                     &mut pair.manifolds,
                     &mut pair.workspace,
                 );
@@ -924,14 +943,8 @@ impl NarrowPhase {
                 );
 
                 let zero = RigidBodyDominance(0); // The value doesn't matter, it will be MAX because of the effective groups.
-                let dominance1 = co1
-                    .parent
-                    .map(|p1| bodies[p1.handle].dominance)
-                    .unwrap_or(zero);
-                let dominance2 = co2
-                    .parent
-                    .map(|p2| bodies[p2.handle].dominance)
-                    .unwrap_or(zero);
+                let dominance1 = rb1.map(|rb| rb.dominance).unwrap_or(zero);
+                let dominance2 = rb2.map(|rb| rb.dominance).unwrap_or(zero);
 
                 pair.has_any_active_contact = false;
 
@@ -948,12 +961,22 @@ impl NarrowPhase {
 
                     // Generate solver contacts.
                     for (contact_id, contact) in manifold.points.iter().enumerate() {
-                        assert!(
-                            contact_id <= u8::MAX as usize,
-                            "A contact manifold cannot contain more than 255 contacts currently."
-                        );
+                        if contact_id > u8::MAX as usize {
+                            log::warn!("A contact manifold cannot contain more than 255 contacts currently, dropping contact in excess.");
+                            break;
+                        }
 
-                        if contact.dist < prediction_distance {
+                        let effective_contact_dist = contact.dist - co1.contact_skin() - co2.contact_skin();
+
+                        let keep_solver_contact = effective_contact_dist < prediction_distance || {
+                            let world_pt1 = world_pos1 * contact.local_p1;
+                            let world_pt2 = world_pos2 * contact.local_p2;
+                            let vel1 = rb1.map(|rb| rb.velocity_at_point(&world_pt1)).unwrap_or_default();
+                            let vel2 = rb2.map(|rb| rb.velocity_at_point(&world_pt2)).unwrap_or_default();
+                            effective_contact_dist + (vel2 - vel1).dot(&manifold.data.normal) * dt < prediction_distance
+                        };
+
+                        if keep_solver_contact {
                             // Generate the solver contact.
                             let world_pt1 = world_pos1 * contact.local_p1;
                             let world_pt2 = world_pos2 * contact.local_p2;
@@ -962,11 +985,13 @@ impl NarrowPhase {
                             let solver_contact = SolverContact {
                                 contact_id: contact_id as u8,
                                 point: effective_point,
-                                dist: contact.dist,
+                                dist: effective_contact_dist,
                                 friction,
                                 restitution,
                                 tangent_velocity: Vector::zeros(),
                                 is_new: contact.data.impulse == 0.0,
+                                warmstart_impulse: contact.data.warmstart_impulse,
+                                warmstart_tangent_impulse: contact.data.warmstart_tangent_impulse,
                             };
 
                             manifold.data.solver_contacts.push(solver_contact);
@@ -1000,9 +1025,36 @@ impl NarrowPhase {
                         manifold.data.normal = modifiable_normal;
                         manifold.data.user_data = modifiable_user_data;
                     }
-                }
 
-                break 'emit_events;
+                    /*
+                     * TODO: When using the block solver in 3D, I’d expect this sort to help, but
+                     *       it makes the domino demo worse. Needs more investigation.
+                    fn sort_solver_contacts(mut contacts: &mut [SolverContact]) {
+                        while contacts.len() > 2 {
+                            let first = contacts[0];
+                            let mut furthest_id = 1;
+                            let mut furthest_dist = na::distance(&first.point, &contacts[1].point);
+
+                            for (candidate_id, candidate) in contacts.iter().enumerate().skip(2) {
+                                let candidate_dist = na::distance(&first.point, &candidate.point);
+
+                                if candidate_dist > furthest_dist {
+                                    furthest_dist = candidate_dist;
+                                    furthest_id = candidate_id;
+                                }
+                            }
+
+                            if furthest_id > 1 {
+                                contacts.swap(1, furthest_id);
+                            }
+
+                            contacts = &mut contacts[2..];
+                        }
+                    }
+
+                    sort_solver_contacts(&mut manifold.data.solver_contacts);
+                    */
+                }
             }
 
             let active_events = co1.flags.active_events | co2.flags.active_events;
diff --git a/src/lib.rs b/src/lib.rs
index 304c0c0..5c660ba 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -15,6 +15,7 @@
 #![allow(clippy::too_many_arguments)]
 #![allow(clippy::needless_range_loop)] // TODO: remove this? I find that in the math code using indices adds clarity.
 #![allow(clippy::module_inception)]
+#![allow(unexpected_cfgs)] // This happens due to the dim2/dim3/f32/f64 cfg.
 
 #[cfg(all(feature = "dim2", feature = "f32"))]
 pub extern crate parry2d as parry;
@@ -166,6 +167,10 @@ pub mod math {
     #[cfg(feature = "dim2")]
     pub type JacobianViewMut<'a, N> = na::MatrixViewMut3xX<'a, N>;
 
+    /// The type of impulse applied for friction constraints.
+    #[cfg(feature = "dim2")]
+    pub type TangentImpulse<N> = na::Vector1<N>;
+
     /// The maximum number of possible rotations and translations of a rigid body.
     #[cfg(feature = "dim2")]
     pub const SPATIAL_DIM: usize = 3;
@@ -195,6 +200,10 @@ pub mod math {
     #[cfg(feature = "dim3")]
     pub type JacobianViewMut<'a, N> = na::MatrixViewMut6xX<'a, N>;
 
+    /// The type of impulse applied for friction constraints.
+    #[cfg(feature = "dim3")]
+    pub type TangentImpulse<N> = na::Vector2<N>;
+
     /// The maximum number of possible rotations and translations of a rigid body.
     #[cfg(feature = "dim3")]
     pub const SPATIAL_DIM: usize = 6;
diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs
index 03396ed..9004946 100644
--- a/src/pipeline/collision_pipeline.rs
+++ b/src/pipeline/collision_pipeline.rs
@@ -43,7 +43,7 @@ impl CollisionPipeline {
     fn detect_collisions(
         &mut self,
         prediction_distance: Real,
-        broad_phase: &mut BroadPhase,
+        broad_phase: &mut dyn BroadPhase,
         narrow_phase: &mut NarrowPhase,
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
@@ -58,8 +58,10 @@ impl CollisionPipeline {
         self.broadphase_collider_pairs.clear();
 
         broad_phase.update(
+            0.0,
             prediction_distance,
             colliders,
+            bodies,
             modified_colliders,
             removed_colliders,
             &mut self.broad_phase_events,
@@ -80,6 +82,7 @@ impl CollisionPipeline {
         narrow_phase.register_pairs(None, colliders, bodies, &self.broad_phase_events, events);
         narrow_phase.compute_contacts(
             prediction_distance,
+            0.0,
             bodies,
             colliders,
             &ImpulseJointSet::new(),
@@ -107,7 +110,7 @@ impl CollisionPipeline {
     pub fn step(
         &mut self,
         prediction_distance: Real,
-        broad_phase: &mut BroadPhase,
+        broad_phase: &mut dyn BroadPhase,
         narrow_phase: &mut NarrowPhase,
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
@@ -192,13 +195,13 @@ mod tests {
         let _ = collider_set.insert(collider_b);
 
         let integration_parameters = IntegrationParameters::default();
-        let mut broad_phase = BroadPhase::new();
+        let mut broad_phase = BroadPhaseMultiSap::new();
         let mut narrow_phase = NarrowPhase::new();
         let mut collision_pipeline = CollisionPipeline::new();
         let physics_hooks = ();
 
         collision_pipeline.step(
-            integration_parameters.prediction_distance,
+            integration_parameters.prediction_distance(),
             &mut broad_phase,
             &mut narrow_phase,
             &mut rigid_body_set,
@@ -244,13 +247,13 @@ mod tests {
         let _ = collider_set.insert(collider_b);
 
         let integration_parameters = IntegrationParameters::default();
-        let mut broad_phase = BroadPhase::new();
+        let mut broad_phase = BroadPhaseMultiSap::new();
         let mut narrow_phase = NarrowPhase::new();
         let mut collision_pipeline = CollisionPipeline::new();
         let physics_hooks = ();
 
         collision_pipeline.step(
-            integration_parameters.prediction_distance,
+            integration_parameters.prediction_distance(),
             &mut broad_phase,
             &mut narrow_phase,
             &mut rigid_body_set,
diff --git a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs
index b429c90..041862c 100644
--- a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs
+++ b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs
@@ -9,6 +9,7 @@ use crate::math::{Isometry, Point, Real, Vector, DIM};
 use crate::pipeline::debug_render_pipeline::debug_render_backend::DebugRenderObject;
 use crate::pipeline::debug_render_pipeline::DebugRenderStyle;
 use crate::utils::SimdBasis;
+use parry::utils::IsometryOpt;
 use std::any::TypeId;
 use std::collections::HashMap;
 
@@ -115,16 +116,19 @@ impl DebugRenderPipeline {
                     if backend.filter_object(object) {
                         for manifold in &pair.manifolds {
                             for contact in manifold.contacts() {
+                                let world_subshape_pos1 =
+                                    manifold.subshape_pos1.prepend_to(co1.position());
                                 backend.draw_line(
                                     object,
-                                    co1.position() * contact.local_p1,
-                                    co2.position() * contact.local_p2,
+                                    world_subshape_pos1 * contact.local_p1,
+                                    manifold.subshape_pos2.prepend_to(co2.position())
+                                        * contact.local_p2,
                                     self.style.contact_depth_color,
                                 );
                                 backend.draw_line(
                                     object,
-                                    co1.position() * contact.local_p1,
-                                    co1.position()
+                                    world_subshape_pos1 * contact.local_p1,
+                                    world_subshape_pos1
                                         * (contact.local_p1
                                             + manifold.local_n1 * self.style.contact_normal_length),
                                     self.style.contact_normal_color,
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index 6dbc4e5..95e408f 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -93,7 +93,7 @@ impl PhysicsPipeline {
         &mut self,
         integration_parameters: &IntegrationParameters,
         islands: &mut IslandManager,
-        broad_phase: &mut BroadPhase,
+        broad_phase: &mut dyn BroadPhase,
         narrow_phase: &mut NarrowPhase,
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
@@ -112,8 +112,10 @@ impl PhysicsPipeline {
         self.broad_phase_events.clear();
         self.broadphase_collider_pairs.clear();
         broad_phase.update(
-            integration_parameters.prediction_distance,
+            integration_parameters.dt,
+            integration_parameters.prediction_distance(),
             colliders,
+            bodies,
             modified_colliders,
             removed_colliders,
             &mut self.broad_phase_events,
@@ -141,7 +143,8 @@ impl PhysicsPipeline {
             events,
         );
         narrow_phase.compute_contacts(
-            integration_parameters.prediction_distance,
+            integration_parameters.prediction_distance(),
+            integration_parameters.dt,
             bodies,
             colliders,
             impulse_joints,
@@ -171,6 +174,7 @@ impl PhysicsPipeline {
         self.counters.stages.island_construction_time.resume();
         islands.update_active_set_with_contacts(
             integration_parameters.dt,
+            integration_parameters.length_unit,
             bodies,
             colliders,
             narrow_phase,
@@ -178,7 +182,6 @@ impl PhysicsPipeline {
             multibody_joints,
             integration_parameters.min_island_size,
         );
-        self.counters.stages.island_construction_time.pause();
 
         if self.manifold_indices.len() < islands.num_islands() {
             self.manifold_indices
@@ -203,6 +206,7 @@ impl PhysicsPipeline {
             bodies,
             &mut self.joint_constraint_indices,
         );
+        self.counters.stages.island_construction_time.pause();
 
         self.counters.stages.update_time.resume();
         for handle in islands.active_dynamic_bodies() {
@@ -406,7 +410,7 @@ impl PhysicsPipeline {
         gravity: &Vector<Real>,
         integration_parameters: &IntegrationParameters,
         islands: &mut IslandManager,
-        broad_phase: &mut BroadPhase,
+        broad_phase: &mut dyn BroadPhase,
         narrow_phase: &mut NarrowPhase,
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
@@ -421,6 +425,7 @@ impl PhysicsPipeline {
         self.counters.step_started();
 
         // Apply some of delayed wake-ups.
+        self.counters.stages.user_changes.start();
         for handle in impulse_joints
             .to_wake_up
             .drain(..)
@@ -459,14 +464,15 @@ impl PhysicsPipeline {
                 .copied()
                 .filter(|h| colliders.get(*h).map(|c| !c.is_enabled()).unwrap_or(false)),
         );
+        self.counters.stages.user_changes.pause();
 
         // TODO: do this only on user-change.
         // TODO: do we want some kind of automatic inverse kinematics?
         for multibody in &mut multibody_joints.multibodies {
-            multibody.1.update_root_type(bodies);
-            // FIXME: what should we do here? We should not
-            //        rely on the next state here.
             multibody.1.forward_kinematics(bodies, true);
+            multibody
+                .1
+                .update_rigid_bodies_internal(bodies, true, false, false);
         }
 
         self.detect_collisions(
@@ -486,12 +492,16 @@ impl PhysicsPipeline {
         );
 
         if let Some(queries) = query_pipeline.as_deref_mut() {
+            self.counters.stages.query_pipeline_time.start();
             queries.update_incremental(colliders, &modified_colliders, &removed_colliders, false);
+            self.counters.stages.query_pipeline_time.pause();
         }
 
+        self.counters.stages.user_changes.resume();
         self.clear_modified_colliders(colliders, &mut modified_colliders);
         self.clear_modified_bodies(bodies, &mut modified_bodies);
         removed_colliders.clear();
+        self.counters.stages.user_changes.pause();
 
         let mut remaining_time = integration_parameters.dt;
         let mut integration_parameters = *integration_parameters;
@@ -508,7 +518,7 @@ impl PhysicsPipeline {
             // the timestep into multiple intervals. First, estimate the
             // size of the time slice we will integrate for this substep.
             //
-            // Note that we must do this now, before the constrains resolution
+            // Note that we must do this now, before the constraints resolution
             // because we need to use the correct timestep length for the
             // integration of external forces.
             //
@@ -599,7 +609,9 @@ impl PhysicsPipeline {
                 }
             }
 
+            self.counters.stages.update_time.resume();
             self.advance_to_final_positions(islands, bodies, colliders, &mut modified_colliders);
+            self.counters.stages.update_time.pause();
 
             self.detect_collisions(
                 &integration_parameters,
@@ -618,12 +630,14 @@ impl PhysicsPipeline {
             );
 
             if let Some(queries) = query_pipeline.as_deref_mut() {
+                self.counters.stages.query_pipeline_time.resume();
                 queries.update_incremental(
                     colliders,
                     &modified_colliders,
                     &[],
                     remaining_substeps == 0,
                 );
+                self.counters.stages.query_pipeline_time.pause();
             }
 
             self.clear_modified_colliders(colliders, &mut modified_colliders);
@@ -635,10 +649,12 @@ impl PhysicsPipeline {
         // TODO: avoid updating the world mass properties twice (here, and
         //       at the beginning of the next timestep) for bodies that were
         //       not modified by the user in the mean time.
+        self.counters.stages.update_time.resume();
         for handle in islands.active_dynamic_bodies() {
             let rb = bodies.index_mut_internal(*handle);
             rb.mprops.update_world_mass_properties(&rb.pos.position);
         }
+        self.counters.stages.update_time.pause();
 
         self.counters.step_completed();
     }
@@ -650,10 +666,10 @@ mod test {
         CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, RigidBodyBuilder,
         RigidBodySet,
     };
-    use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet, NarrowPhase};
+    use crate::geometry::{BroadPhaseMultiSap, ColliderBuilder, ColliderSet, NarrowPhase};
     use crate::math::Vector;
     use crate::pipeline::PhysicsPipeline;
-    use crate::prelude::MultibodyJointSet;
+    use crate::prelude::{MultibodyJointSet, RigidBodyType};
 
     #[test]
     fn kinematic_and_fixed_contact_crash() {
@@ -661,7 +677,7 @@ mod test {
         let mut impulse_joints = ImpulseJointSet::new();
         let mut multibody_joints = MultibodyJointSet::new();
         let mut pipeline = PhysicsPipeline::new();
-        let mut bf = BroadPhase::new();
+        let mut bf = BroadPhaseMultiSap::new();
         let mut nf = NarrowPhase::new();
         let mut bodies = RigidBodySet::new();
         let mut islands = IslandManager::new();
@@ -699,7 +715,7 @@ mod test {
         let mut impulse_joints = ImpulseJointSet::new();
         let mut multibody_joints = MultibodyJointSet::new();
         let mut pipeline = PhysicsPipeline::new();
-        let mut bf = BroadPhase::new();
+        let mut bf = BroadPhaseMultiSap::new();
         let mut nf = NarrowPhase::new();
         let mut islands = IslandManager::new();
 
@@ -809,7 +825,7 @@ mod test {
         let mut pipeline = PhysicsPipeline::new();
         let gravity = Vector::y() * -9.81;
         let integration_parameters = IntegrationParameters::default();
-        let mut broad_phase = BroadPhase::new();
+        let mut broad_phase = BroadPhaseMultiSap::new();
         let mut narrow_phase = NarrowPhase::new();
         let mut bodies = RigidBodySet::new();
         let mut colliders = ColliderSet::new();
@@ -852,4 +868,73 @@ mod test {
             );
         }
     }
+
+    #[test]
+    fn rigid_body_type_changed_dynamic_is_in_active_set() {
+        let mut colliders = ColliderSet::new();
+        let mut impulse_joints = ImpulseJointSet::new();
+        let mut multibody_joints = MultibodyJointSet::new();
+        let mut pipeline = PhysicsPipeline::new();
+        let mut bf = BroadPhaseMultiSap::new();
+        let mut nf = NarrowPhase::new();
+        let mut islands = IslandManager::new();
+
+        let mut bodies = RigidBodySet::new();
+
+        // Initialize body as kinematic with mass
+        let rb = RigidBodyBuilder::kinematic_position_based()
+            .additional_mass(1.0)
+            .build();
+        let h = bodies.insert(rb.clone());
+
+        // Step once
+        let gravity = Vector::y() * -9.81;
+        pipeline.step(
+            &gravity,
+            &IntegrationParameters::default(),
+            &mut islands,
+            &mut bf,
+            &mut nf,
+            &mut bodies,
+            &mut colliders,
+            &mut impulse_joints,
+            &mut multibody_joints,
+            &mut CCDSolver::new(),
+            None,
+            &(),
+            &(),
+        );
+
+        // Switch body type to Dynamic
+        bodies
+            .get_mut(h)
+            .unwrap()
+            .set_body_type(RigidBodyType::Dynamic, true);
+
+        // Step again
+        pipeline.step(
+            &gravity,
+            &IntegrationParameters::default(),
+            &mut islands,
+            &mut bf,
+            &mut nf,
+            &mut bodies,
+            &mut colliders,
+            &mut impulse_joints,
+            &mut multibody_joints,
+            &mut CCDSolver::new(),
+            None,
+            &(),
+            &(),
+        );
+
+        let body = bodies.get(h).unwrap();
+        let h_y = body.pos.position.translation.y;
+
+        // Expect gravity to be applied on second step after switching to Dynamic
+        assert!(h_y < 0.0);
+
+        // Expect body to now be in active_dynamic_set
+        assert!(islands.active_dynamic_set.contains(&h));
+    }
 }
diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs
index 4dc5652..f202107 100644
--- a/src/pipeline/query_pipeline.rs
+++ b/src/pipeline/query_pipeline.rs
@@ -6,17 +6,16 @@ use crate::math::{Isometry, Point, Real, Vector};
 use crate::{dynamics::RigidBodySet, geometry::ColliderSet};
 use parry::partitioning::{QbvhDataGenerator, QbvhUpdateWorkspace};
 use parry::query::details::{
-    NonlinearTOICompositeShapeShapeBestFirstVisitor, PointCompositeShapeProjBestFirstVisitor,
-    PointCompositeShapeProjWithFeatureBestFirstVisitor,
+    NonlinearTOICompositeShapeShapeBestFirstVisitor, NormalConstraints,
+    PointCompositeShapeProjBestFirstVisitor, PointCompositeShapeProjWithFeatureBestFirstVisitor,
     RayCompositeShapeToiAndNormalBestFirstVisitor, RayCompositeShapeToiBestFirstVisitor,
-    TOICompositeShapeShapeBestFirstVisitor,
+    ShapeCastOptions, TOICompositeShapeShapeBestFirstVisitor,
 };
 use parry::query::visitors::{
     BoundingVolumeIntersectionsVisitor, PointIntersectionsVisitor, RayIntersectionsVisitor,
 };
-use parry::query::{DefaultQueryDispatcher, NonlinearRigidMotion, QueryDispatcher, TOI};
+use parry::query::{DefaultQueryDispatcher, NonlinearRigidMotion, QueryDispatcher, ShapeCastHit};
 use parry::shape::{FeatureId, Shape, TypedSimdCompositeShape};
-use parry::utils::DefaultStorage;
 use std::sync::Arc;
 
 /// A pipeline for performing queries on all the colliders of a scene.
@@ -101,7 +100,7 @@ impl QueryFilterFlags {
     }
 }
 
-/// A filter tha describes what collider should be included or excluded from a scene query.
+/// A filter that describes what collider should be included or excluded from a scene query.
 #[derive(Copy, Clone, Default)]
 pub struct QueryFilter<'a> {
     /// Flags indicating what particular type of colliders should be excluded from the scene query.
@@ -246,17 +245,21 @@ pub enum QueryPipelineMode {
 
 impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> {
     type PartShape = dyn Shape;
+    type PartNormalConstraints = dyn NormalConstraints;
     type PartId = ColliderHandle;
-    type QbvhStorage = DefaultStorage;
 
     fn map_typed_part_at(
         &self,
         shape_id: Self::PartId,
-        mut f: impl FnMut(Option<&Isometry<Real>>, &Self::PartShape),
+        mut f: impl FnMut(
+            Option<&Isometry<Real>>,
+            &Self::PartShape,
+            Option<&Self::PartNormalConstraints>,
+        ),
     ) {
         if let Some(co) = self.colliders.get(shape_id) {
             if self.filter.test(self.bodies, shape_id, co) {
-                f(Some(&co.pos), &*co.shape)
+                f(Some(&co.pos), &*co.shape, None)
             }
         }
     }
@@ -264,7 +267,7 @@ impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> {
     fn map_untyped_part_at(
         &self,
         shape_id: Self::PartId,
-        f: impl FnMut(Option<&Isometry<Real>>, &Self::PartShape),
+        f: impl FnMut(Option<&Isometry<Real>>, &Self::PartShape, Option<&dyn NormalConstraints>),
     ) {
         self.map_typed_part_at(shape_id, f);
     }
@@ -676,10 +679,9 @@ impl QueryPipeline {
         shape_pos: &Isometry<Real>,
         shape_vel: &Vector<Real>,
         shape: &dyn Shape,
-        max_toi: Real,
-        stop_at_penetration: bool,
+        options: ShapeCastOptions,
         filter: QueryFilter,
-    ) -> Option<(ColliderHandle, TOI)> {
+    ) -> Option<(ColliderHandle, ShapeCastHit)> {
         let pipeline_shape = self.as_composite_shape(bodies, colliders, filter);
         let mut visitor = TOICompositeShapeShapeBestFirstVisitor::new(
             &*self.query_dispatcher,
@@ -687,8 +689,7 @@ impl QueryPipeline {
             shape_vel,
             &pipeline_shape,
             shape,
-            max_toi,
-            stop_at_penetration,
+            options,
         );
         self.qbvh.traverse_best_first(&mut visitor).map(|h| h.1)
     }
@@ -722,7 +723,7 @@ impl QueryPipeline {
         end_time: Real,
         stop_at_penetration: bool,
         filter: QueryFilter,
-    ) -> Option<(ColliderHandle, TOI)> {
+    ) -> Option<(ColliderHandle, ShapeCastHit)> {
         let pipeline_shape = self.as_composite_shape(bodies, colliders, filter);
         let pipeline_motion = NonlinearRigidMotion::identity();
         let mut visitor = NonlinearTOICompositeShapeShapeBestFirstVisitor::new(
diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs
index 7c3f1ac..a2b00a1 100644
--- a/src/pipeline/user_changes.rs
+++ b/src/pipeline/user_changes.rs
@@ -120,9 +120,9 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
                         islands.active_kinematic_set.push(*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 changes.contains(RigidBodyChanges::SLEEP)
+                    // Push the body to the active set if it is not inside the active set yet, and
+                    // is either not longer sleeping or became dynamic.
+                    if (changes.contains(RigidBodyChanges::SLEEP) || changes.contains(RigidBodyChanges::TYPE))
                         && rb.is_enabled()
                         && !rb.activation.sleeping // May happen if the body was put to sleep manually.
                         && rb.is_dynamic() // Only dynamic bodies are in the active dynamic set.
diff --git a/src/utils.rs b/src/utils.rs
index 6521b0f..102c85f 100644
--- a/src/utils.rs
+++ b/src/utils.rs
@@ -1,8 +1,8 @@
 //! Miscellaneous utilities.
 
 use na::{
-    Matrix1, Matrix2, Matrix3, Point2, Point3, RowVector2, Scalar, SimdRealField, UnitComplex,
-    UnitQuaternion, Vector1, Vector2, Vector3,
+    Matrix1, Matrix2, Matrix3, RowVector2, Scalar, SimdRealField, UnitComplex, UnitQuaternion,
+    Vector1, Vector2, Vector3,
 };
 use num::Zero;
 use simba::simd::SimdValue;
@@ -90,35 +90,6 @@ impl SimdSign<SimdReal> for SimdReal {
     }
 }
 
-pub(crate) trait SimdComponent: Sized {
-    type Element;
-
-    fn min_component(self) -> Self::Element;
-    fn max_component(self) -> Self::Element;
-}
-
-impl SimdComponent for Real {
-    type Element = Real;
-
-    fn min_component(self) -> Self::Element {
-        self
-    }
-    fn max_component(self) -> Self::Element {
-        self
-    }
-}
-
-impl SimdComponent for SimdReal {
-    type Element = Real;
-
-    fn min_component(self) -> Self::Element {
-        self.simd_horizontal_min()
-    }
-    fn max_component(self) -> Self::Element {
-        self.simd_horizontal_max()
-    }
-}
-
 /// Trait to compute the orthonormal basis of a vector.
 pub trait SimdBasis: Sized {
     /// The type of the array of orthonormal vectors.
@@ -166,89 +137,6 @@ impl<N: SimdRealCopy + SimdSign<N>> SimdBasis for Vector3<N> {
     }
 }
 
-pub(crate) trait SimdVec: Sized {
-    type Element;
-
-    fn horizontal_inf(&self) -> Self::Element;
-    fn horizontal_sup(&self) -> Self::Element;
-}
-
-impl<N: Scalar + Copy + SimdComponent> SimdVec for Vector2<N>
-where
-    N::Element: Scalar,
-{
-    type Element = Vector2<N::Element>;
-
-    fn horizontal_inf(&self) -> Self::Element {
-        Vector2::new(self.x.min_component(), self.y.min_component())
-    }
-
-    fn horizontal_sup(&self) -> Self::Element {
-        Vector2::new(self.x.max_component(), self.y.max_component())
-    }
-}
-
-impl<N: Scalar + Copy + SimdComponent> SimdVec for Point2<N>
-where
-    N::Element: Scalar,
-{
-    type Element = Point2<N::Element>;
-
-    fn horizontal_inf(&self) -> Self::Element {
-        Point2::new(self.x.min_component(), self.y.min_component())
-    }
-
-    fn horizontal_sup(&self) -> Self::Element {
-        Point2::new(self.x.max_component(), self.y.max_component())
-    }
-}
-
-impl<N: Scalar + Copy + SimdComponent> SimdVec for Vector3<N>
-where
-    N::Element: Scalar,
-{
-    type Element = Vector3<N::Element>;
-
-    fn horizontal_inf(&self) -> Self::Element {
-        Vector3::new(
-            self.x.min_component(),
-            self.y.min_component(),
-            self.z.min_component(),
-        )
-    }
-
-    fn horizontal_sup(&self) -> Self::Element {
-        Vector3::new(
-            self.x.max_component(),
-            self.y.max_component(),
-            self.z.max_component(),
-        )
-    }
-}
-
-impl<N: Scalar + Copy + SimdComponent> SimdVec for Point3<N>
-where
-    N::Element: Scalar,
-{
-    type Element = Point3<N::Element>;
-
-    fn horizontal_inf(&self) -> Self::Element {
-        Point3::new(
-            self.x.min_component(),
-            self.y.min_component(),
-            self.z.min_component(),
-        )
-    }
-
-    fn horizontal_sup(&self) -> Self::Element {
-        Point3::new(
-            self.x.max_component(),
-            self.y.max_component(),
-            self.z.max_component(),
-        )
-    }
-}
-
 pub(crate) trait SimdCrossMatrix: Sized {
     type CrossMat;
     type CrossMatTr;
@@ -463,28 +351,21 @@ impl<N: SimdRealCopy> SimdQuat<N> for UnitQuaternion<N> {
 
 pub(crate) trait SimdAngularInertia<N> {
     type AngVector;
-    type LinVector;
     type AngMatrix;
     fn inverse(&self) -> Self;
-    fn transform_lin_vector(&self, pt: Self::LinVector) -> Self::LinVector;
     fn transform_vector(&self, pt: Self::AngVector) -> Self::AngVector;
     fn squared(&self) -> Self;
-    fn transform_matrix(&self, mat: &Self::AngMatrix) -> Self::AngMatrix;
     fn into_matrix(self) -> Self::AngMatrix;
 }
 
 impl<N: SimdRealCopy> SimdAngularInertia<N> for N {
     type AngVector = N;
-    type LinVector = Vector2<N>;
     type AngMatrix = N;
 
     fn inverse(&self) -> Self {
         simd_inv(*self)
     }
 
-    fn transform_lin_vector(&self, pt: Vector2<N>) -> Vector2<N> {
-        pt * *self
-    }
     fn transform_vector(&self, pt: N) -> N {
         pt * *self
     }
@@ -493,10 +374,6 @@ impl<N: SimdRealCopy> SimdAngularInertia<N> for N {
         *self * *self
     }
 
-    fn transform_matrix(&self, mat: &Self::AngMatrix) -> Self::AngMatrix {
-        *mat * *self
-    }
-
     fn into_matrix(self) -> Self::AngMatrix {
         self
     }
@@ -504,7 +381,6 @@ impl<N: SimdRealCopy> SimdAngularInertia<N> for N {
 
 impl SimdAngularInertia<Real> for SdpMatrix3<Real> {
     type AngVector = Vector3<Real>;
-    type LinVector = Vector3<Real>;
     type AngMatrix = Matrix3<Real>;
 
     fn inverse(&self) -> Self {
@@ -540,10 +416,6 @@ impl SimdAngularInertia<Real> for SdpMatrix3<Real> {
         }
     }
 
-    fn transform_lin_vector(&self, v: Vector3<Real>) -> Vector3<Real> {
-        self.transform_vector(v)
-    }
-
     fn transform_vector(&self, v: Vector3<Real>) -> Vector3<Real> {
         let x = self.m11 * v.x + self.m12 * v.y + self.m13 * v.z;
         let y = self.m12 * v.x + self.m22 * v.y + self.m23 * v.z;
@@ -559,16 +431,10 @@ impl SimdAngularInertia<Real> for SdpMatrix3<Real> {
             self.m13, self.m23, self.m33,
         )
     }
-
-    #[rustfmt::skip]
-    fn transform_matrix(&self, m: &Matrix3<Real>) -> Matrix3<Real> {
-        *self * *m
-    }
 }
 
 impl SimdAngularInertia<SimdReal> for SdpMatrix3<SimdReal> {
     type AngVector = Vector3<SimdReal>;
-    type LinVector = Vector3<SimdReal>;
     type AngMatrix = Matrix3<SimdReal>;
 
     fn inverse(&self) -> Self {
@@ -593,10 +459,6 @@ impl SimdAngularInertia<SimdReal> for SdpMatrix3<SimdReal> {
         }
     }
 
-    fn transform_lin_vector(&self, v: Vector3<SimdReal>) -> Vector3<SimdReal> {
-        self.transform_vector(v)
-    }
-
     fn transform_vector(&self, v: Vector3<SimdReal>) -> Vector3<SimdReal> {
         let x = self.m11 * v.x + self.m12 * v.y + self.m13 * v.z;
         let y = self.m12 * v.x + self.m22 * v.y + self.m23 * v.z;
@@ -623,31 +485,10 @@ impl SimdAngularInertia<SimdReal> for SdpMatrix3<SimdReal> {
             self.m13, self.m23, self.m33,
         )
     }
-
-    #[rustfmt::skip]
-    fn transform_matrix(&self, m: &Matrix3<SimdReal>) -> Matrix3<SimdReal> {
-        let x0 = self.m11 * m.m11 + self.m12 * m.m21 + self.m13 * m.m31;
-        let y0 = self.m12 * m.m11 + self.m22 * m.m21 + self.m23 * m.m31;
-        let z0 = self.m13 * m.m11 + self.m23 * m.m21 + self.m33 * m.m31;
-
-        let x1 = self.m11 * m.m12 + self.m12 * m.m22 + self.m13 * m.m32;
-        let y1 = self.m12 * m.m12 + self.m22 * m.m22 + self.m23 * m.m32;
-        let z1 = self.m13 * m.m12 + self.m23 * m.m22 + self.m33 * m.m32;
-
-        let x2 = self.m11 * m.m13 + self.m12 * m.m23 + self.m13 * m.m33;
-        let y2 = self.m12 * m.m13 + self.m22 * m.m23 + self.m23 * m.m33;
-        let z2 = self.m13 * m.m13 + self.m23 * m.m23 + self.m33 * m.m33;
-
-        Matrix3::new(
-            x0, x1, x2,
-            y0, y1, y2,
-            z0, z1, z2,
-        )
-    }
 }
 
 // This is an RAII structure that enables flushing denormal numbers
-// to zero, and automatically reseting previous flags once it is dropped.
+// to zero, and automatically resetting previous flags once it is dropped.
 #[derive(Clone, Debug, PartialEq, Eq)]
 pub(crate) struct FlushToZeroDenormalsAreZeroFlags {
     original_flags: u32,
diff --git a/src_testbed/camera2d.rs b/src_testbed/camera2d.rs
index 6ba17a1..ae5c163 100644
--- a/src_testbed/camera2d.rs
+++ b/src_testbed/camera2d.rs
@@ -48,7 +48,7 @@ impl OrbitCameraPlugin {
     fn mouse_motion_system(
         _time: Res<Time>,
         mut mouse_motion_events: EventReader<MouseMotion>,
-        mouse_button_input: Res<Input<MouseButton>>,
+        mouse_button_input: Res<ButtonInput<MouseButton>>,
         mut query: Query<(&mut OrbitCamera, &mut Transform, &mut Camera)>,
     ) {
         let mut delta = Vec2::ZERO;
diff --git a/src_testbed/camera3d.rs b/src_testbed/camera3d.rs
index 9c3764e..b2313b7 100644
--- a/src_testbed/camera3d.rs
+++ b/src_testbed/camera3d.rs
@@ -61,7 +61,7 @@ impl OrbitCameraPlugin {
     fn mouse_motion_system(
         time: Res<Time>,
         mut mouse_motion_events: EventReader<MouseMotion>,
-        mouse_button_input: Res<Input<MouseButton>>,
+        mouse_button_input: Res<ButtonInput<MouseButton>>,
         mut query: Query<(&mut OrbitCamera, &mut Transform, &mut Camera)>,
     ) {
         let mut delta = Vec2::ZERO;
diff --git a/src_testbed/debug_render.rs b/src_testbed/debug_render.rs
index 0e0b8b8..3138546 100644
--- a/src_testbed/debug_render.rs
+++ b/src_testbed/debug_render.rs
@@ -30,11 +30,11 @@ impl Plugin for RapierDebugRenderPlugin {
     }
 }
 
-struct BevyLinesRenderBackend<'a> {
-    gizmos: Gizmos<'a>,
+struct BevyLinesRenderBackend<'w, 's> {
+    gizmos: Gizmos<'w, 's>,
 }
 
-impl<'a> DebugRenderBackend for BevyLinesRenderBackend<'a> {
+impl<'w, 's> DebugRenderBackend for BevyLinesRenderBackend<'w, 's> {
     #[cfg(feature = "dim2")]
     fn draw_line(&mut self, _: DebugRenderObject, a: Point<Real>, b: Point<Real>, color: [f32; 4]) {
         self.gizmos.line(
diff --git a/src_testbed/graphics.rs b/src_testbed/graphics.rs
index 552d4c7..226be4d 100644
--- a/src_testbed/graphics.rs
+++ b/src_testbed/graphics.rs
@@ -22,6 +22,9 @@ pub type BevyMaterial = ColorMaterial;
 #[cfg(feature = "dim3")]
 pub type BevyMaterial = StandardMaterial;
 
+pub type InstancedMaterials = HashMap<Point3<usize>, Handle<BevyMaterial>>;
+pub const SELECTED_OBJECT_MATERIAL_KEY: Point3<usize> = point![42, 42, 42];
+
 pub struct GraphicsManager {
     rand: Pcg32,
     b2sn: HashMap<RigidBodyHandle, Vec<EntityWithGraphics>>,
@@ -30,6 +33,7 @@ pub struct GraphicsManager {
     b2wireframe: HashMap<RigidBodyHandle, bool>,
     ground_color: Point3<f32>,
     prefab_meshes: HashMap<ShapeType, Handle<Mesh>>,
+    instanced_materials: InstancedMaterials,
     pub gfx_shift: Vector<Real>,
 }
 
@@ -43,10 +47,15 @@ impl GraphicsManager {
             ground_color: point![0.5, 0.5, 0.5],
             b2wireframe: HashMap::new(),
             prefab_meshes: HashMap::new(),
+            instanced_materials: HashMap::new(),
             gfx_shift: Vector::zeros(),
         }
     }
 
+    pub fn selection_material(&self) -> Handle<BevyMaterial> {
+        self.instanced_materials[&SELECTED_OBJECT_MATERIAL_KEY].clone_weak()
+    }
+
     pub fn clear(&mut self, commands: &mut Commands) {
         for sns in self.b2sn.values_mut() {
             for sn in sns.iter_mut() {
@@ -54,6 +63,7 @@ impl GraphicsManager {
             }
         }
 
+        self.instanced_materials.clear();
         self.b2sn.clear();
         self.c2color.clear();
         self.b2color.clear();
@@ -159,10 +169,11 @@ impl GraphicsManager {
 
     fn gen_color(rng: &mut Pcg32) -> Point3<f32> {
         let mut color: Point3<f32> = rng.gen();
-        color *= 1.5;
-        color.x = color.x.min(1.0);
-        color.y = color.y.min(1.0);
-        color.z = color.z.min(1.0);
+
+        // Quantize the colors a bit to get some amount of auto-instancing from bevy.
+        color.x = (color.x * 5.0).round() / 5.0;
+        color.y = (color.y * 5.0).round() / 5.0;
+        color.z = (color.z * 5.0).round() / 5.0;
         color
     }
 
@@ -191,7 +202,7 @@ impl GraphicsManager {
         commands: &mut Commands,
         meshes: &mut Assets<Mesh>,
         materials: &mut Assets<BevyMaterial>,
-        components: &mut Query<(&mut Transform,)>,
+        components: &mut Query<&mut Transform>,
         handle: RigidBodyHandle,
         bodies: &RigidBodySet,
         colliders: &ColliderSet,
@@ -214,7 +225,7 @@ impl GraphicsManager {
         commands: &mut Commands,
         meshes: &mut Assets<Mesh>,
         materials: &mut Assets<BevyMaterial>,
-        components: &mut Query<(&mut Transform,)>,
+        components: &mut Query<&mut Transform>,
         handle: RigidBodyHandle,
         bodies: &RigidBodySet,
         colliders: &ColliderSet,
@@ -330,6 +341,7 @@ impl GraphicsManager {
                 meshes,
                 materials,
                 &self.prefab_meshes,
+                &mut self.instanced_materials,
                 shape,
                 handle,
                 *pos,
@@ -345,7 +357,7 @@ impl GraphicsManager {
         &mut self,
         _bodies: &RigidBodySet,
         colliders: &ColliderSet,
-        components: &mut Query<(&mut Transform,)>,
+        components: &mut Query<&mut Transform>,
         _materials: &mut Assets<BevyMaterial>,
     ) {
         for (_, ns) in self.b2sn.iter_mut() {
diff --git a/src_testbed/harness/mod.rs b/src_testbed/harness/mod.rs
index e27a03a..9c41472 100644
--- a/src_testbed/harness/mod.rs
+++ b/src_testbed/harness/mod.rs
@@ -9,7 +9,7 @@ use rapier::dynamics::{
     CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet,
     RigidBodySet,
 };
-use rapier::geometry::{BroadPhase, ColliderSet, NarrowPhase};
+use rapier::geometry::{ColliderSet, DefaultBroadPhase, NarrowPhase};
 use rapier::math::{Real, Vector};
 use rapier::pipeline::{ChannelEventCollector, PhysicsHooks, PhysicsPipeline, QueryPipeline};
 
@@ -179,7 +179,7 @@ impl Harness {
         self.physics.hooks = Box::new(hooks);
 
         self.physics.islands = IslandManager::new();
-        self.physics.broad_phase = BroadPhase::new();
+        self.physics.broad_phase = DefaultBroadPhase::new();
         self.physics.narrow_phase = NarrowPhase::new();
         self.state.timestep_id = 0;
         self.state.time = 0.0;
diff --git a/src_testbed/lib.rs b/src_testbed/lib.rs
index cb24d7e..2cb002a 100644
--- a/src_testbed/lib.rs
+++ b/src_testbed/lib.rs
@@ -1,4 +1,5 @@
 #![allow(clippy::too_many_arguments)]
+#![allow(unexpected_cfgs)] // This happens due to the dim2/dim3/f32/f64 cfg.
 
 extern crate nalgebra as na;
 
@@ -25,6 +26,7 @@ mod camera3d;
 mod debug_render;
 mod graphics;
 pub mod harness;
+mod mouse;
 pub mod objects;
 pub mod physics;
 #[cfg(all(feature = "dim3", feature = "other-backends"))]
diff --git a/src_testbed/mouse.rs b/src_testbed/mouse.rs
new file mode 100644
index 0000000..05b69e2
--- /dev/null
+++ b/src_testbed/mouse.rs
@@ -0,0 +1,46 @@
+use crate::math::Point;
+use bevy::prelude::*;
+use bevy::window::PrimaryWindow;
+
+#[derive(Component)]
+pub struct MainCamera;
+
+#[derive(Default, Copy, Clone, Debug, Resource)]
+pub struct SceneMouse {
+    #[cfg(feature = "dim2")]
+    pub point: Option<Point<f32>>,
+    #[cfg(feature = "dim3")]
+    pub ray: Option<(Point<f32>, crate::math::Vector<f32>)>,
+}
+
+pub fn track_mouse_state(
+    mut scene_mouse: ResMut<SceneMouse>,
+    windows: Query<&Window, With<PrimaryWindow>>,
+    camera: Query<(&GlobalTransform, &Camera), With<MainCamera>>,
+) {
+    if let Ok(window) = windows.get_single() {
+        for (camera_transform, camera) in camera.iter() {
+            if let Some(cursor) = window.cursor_position() {
+                let ndc_cursor = ((cursor / Vec2::new(window.width(), window.height()) * 2.0)
+                    - Vec2::ONE)
+                    * Vec2::new(1.0, -1.0);
+                let ndc_to_world =
+                    camera_transform.compute_matrix() * camera.projection_matrix().inverse();
+                let ray_pt1 =
+                    ndc_to_world.project_point3(Vec3::new(ndc_cursor.x, ndc_cursor.y, -1.0));
+
+                #[cfg(feature = "dim2")]
+                {
+                    scene_mouse.point = Some(Point::new(ray_pt1.x, ray_pt1.y));
+                }
+                #[cfg(feature = "dim3")]
+                {
+                    let ray_pt2 =
+                        ndc_to_world.project_point3(Vec3::new(ndc_cursor.x, ndc_cursor.y, 1.0));
+                    let ray_dir = ray_pt2 - ray_pt1;
+                    scene_mouse.ray = Some((na::Vector3::from(ray_pt1).into(), ray_dir.into()));
+                }
+            }
+        }
+    }
+}
diff --git a/src_testbed/objects/node.rs b/src_testbed/objects/node.rs
index cdc042b..58e3a00 100644
--- a/src_testbed/objects/node.rs
+++ b/src_testbed/objects/node.rs
@@ -14,7 +14,7 @@ use rapier::geometry::{ColliderHandle, ColliderSet, Shape, ShapeType};
 use rapier::geometry::{Cone, Cylinder};
 use rapier::math::{Isometry, Real, Vector};
 
-use crate::graphics::BevyMaterial;
+use crate::graphics::{BevyMaterial, InstancedMaterials, SELECTED_OBJECT_MATERIAL_KEY};
 #[cfg(feature = "dim2")]
 use {
     bevy_sprite::MaterialMesh2dBundle,
@@ -30,15 +30,43 @@ pub struct EntityWithGraphics {
     pub collider: Option<ColliderHandle>,
     pub delta: Isometry<Real>,
     pub opacity: f32,
-    material: Handle<BevyMaterial>,
+    pub material: Handle<BevyMaterial>,
 }
 
 impl EntityWithGraphics {
+    pub fn register_selected_object_material(
+        materials: &mut Assets<BevyMaterial>,
+        instanced_materials: &mut InstancedMaterials,
+    ) {
+        if instanced_materials.contains_key(&SELECTED_OBJECT_MATERIAL_KEY) {
+            return; // Already added.
+        }
+
+        #[cfg(feature = "dim2")]
+        let selection_material = ColorMaterial {
+            color: Color::rgb(1.0, 0.0, 0.0),
+            texture: None,
+        };
+        #[cfg(feature = "dim3")]
+        let selection_material = StandardMaterial {
+            metallic: 0.5,
+            perceptual_roughness: 0.5,
+            double_sided: true, // TODO: this doesn't do anything?
+            ..StandardMaterial::from(Color::rgb(1.0, 0.0, 0.0))
+        };
+
+        instanced_materials.insert(
+            SELECTED_OBJECT_MATERIAL_KEY,
+            materials.add(selection_material),
+        );
+    }
+
     pub fn spawn(
         commands: &mut Commands,
         meshes: &mut Assets<Mesh>,
         materials: &mut Assets<BevyMaterial>,
         prefab_meshs: &HashMap<ShapeType, Handle<Mesh>>,
+        instanced_materials: &mut InstancedMaterials,
         shape: &dyn Shape,
         collider: Option<ColliderHandle>,
         collider_pos: Isometry<Real>,
@@ -46,6 +74,8 @@ impl EntityWithGraphics {
         color: Point3<f32>,
         sensor: bool,
     ) -> Self {
+        Self::register_selected_object_material(materials, instanced_materials);
+
         let entity = commands.spawn_empty().id();
 
         let scale = collider_mesh_scale(shape);
@@ -90,21 +120,23 @@ impl EntityWithGraphics {
             double_sided: true, // TODO: this doesn't do anything?
             ..StandardMaterial::from(bevy_color)
         };
-        let material_handle = materials.add(material);
+        let material_handle = instanced_materials
+            .entry(color.coords.map(|c| (c * 255.0) as usize).into())
+            .or_insert_with(|| materials.add(material));
         let material_weak_handle = material_handle.clone_weak();
 
         if let Some(mesh) = mesh {
             #[cfg(feature = "dim2")]
             let bundle = MaterialMesh2dBundle {
                 mesh: mesh.into(),
-                material: material_handle,
+                material: material_handle.clone_weak(),
                 transform,
                 ..Default::default()
             };
             #[cfg(feature = "dim3")]
             let bundle = PbrBundle {
                 mesh,
-                material: material_handle,
+                material: material_handle.clone_weak(),
                 transform,
                 ..Default::default()
             };
@@ -133,28 +165,6 @@ impl EntityWithGraphics {
         commands.entity(self.entity).despawn();
     }
 
-    pub fn select(&mut self, materials: &mut Assets<BevyMaterial>) {
-        // NOTE: we don't just call `self.set_color` because that would
-        //       overwrite self.base_color too.
-        self.color = point![1.0, 0.0, 0.0];
-        if let Some(material) = materials.get_mut(&self.material) {
-            #[cfg(feature = "dim2")]
-            {
-                material.color =
-                    Color::rgba(self.color.x, self.color.y, self.color.z, self.opacity);
-            }
-            #[cfg(feature = "dim3")]
-            {
-                material.base_color =
-                    Color::rgba(self.color.x, self.color.y, self.color.z, self.opacity);
-            }
-        }
-    }
-
-    pub fn unselect(&mut self, materials: &mut Assets<BevyMaterial>) {
-        self.set_color(materials, self.base_color);
-    }
-
     pub fn set_color(&mut self, materials: &mut Assets<BevyMaterial>, color: Point3<f32>) {
         if let Some(material) = materials.get_mut(&self.material) {
             #[cfg(feature = "dim2")]
@@ -173,11 +183,11 @@ impl EntityWithGraphics {
     pub fn update(
         &mut self,
         colliders: &ColliderSet,
-        components: &mut Query<(&mut Transform,)>,
+        components: &mut Query<&mut Transform>,
         gfx_shift: &Vector<Real>,
     ) {
         if let Some(Some(co)) = self.collider.map(|c| colliders.get(c)) {
-            if let Ok(mut pos) = components.get_component_mut::<Transform>(self.entity) {
+            if let Ok(mut pos) = components.get_mut(self.entity) {
                 let co_pos = co.position() * self.delta;
                 pos.translation.x = (co_pos.translation.vector.x + gfx_shift.x) as f32;
                 pos.translation.y = (co_pos.translation.vector.y + gfx_shift.y) as f32;
@@ -230,18 +240,14 @@ impl EntityWithGraphics {
         //
         // Cuboid mesh
         //
-        let cuboid = Mesh::from(shape::Cube { size: 2.0 });
+        let cuboid = Mesh::from(bevy::math::primitives::Cuboid::new(2.0, 2.0, 2.0));
         out.insert(ShapeType::Cuboid, meshes.add(cuboid.clone()));
         out.insert(ShapeType::RoundCuboid, meshes.add(cuboid));
 
         //
         // Ball mesh
         //
-        let ball = Mesh::try_from(shape::Icosphere {
-            subdivisions: 2,
-            radius: 1.0,
-        })
-        .unwrap();
+        let ball = Mesh::from(bevy::math::primitives::Sphere::new(1.0));
         out.insert(ShapeType::Ball, meshes.add(ball));
 
         //
@@ -309,14 +315,14 @@ fn bevy_polyline(buffers: (Vec<Point2<Real>>, Option<Vec<[u32; 2]>>)) -> Mesh {
     let normals: Vec<_> = (0..vertices.len()).map(|_| [0.0, 0.0, 1.0]).collect();
 
     // Generate the mesh
-    let mut mesh = Mesh::new(PrimitiveTopology::LineStrip);
+    let mut mesh = Mesh::new(PrimitiveTopology::LineStrip, Default::default());
     mesh.insert_attribute(
         Mesh::ATTRIBUTE_POSITION,
         VertexAttributeValues::from(vertices),
     );
     mesh.insert_attribute(Mesh::ATTRIBUTE_NORMAL, VertexAttributeValues::from(normals));
     mesh.insert_attribute(Mesh::ATTRIBUTE_UV_0, VertexAttributeValues::from(uvs));
-    mesh.set_indices(Some(Indices::U32(indices)));
+    mesh.insert_indices(Indices::U32(indices));
     mesh
 }
 
@@ -352,14 +358,14 @@ fn bevy_mesh(buffers: (Vec<Point3<Real>>, Vec<[u32; 3]>)) -> Mesh {
     let uvs: Vec<_> = (0..vertices.len()).map(|_| [0.0, 0.0]).collect();
 
     // Generate the mesh
-    let mut mesh = Mesh::new(PrimitiveTopology::TriangleList);
+    let mut mesh = Mesh::new(PrimitiveTopology::TriangleList, Default::default());
     mesh.insert_attribute(
         Mesh::ATTRIBUTE_POSITION,
         VertexAttributeValues::from(vertices),
     );
     mesh.insert_attribute(Mesh::ATTRIBUTE_NORMAL, VertexAttributeValues::from(normals));
     mesh.insert_attribute(Mesh::ATTRIBUTE_UV_0, VertexAttributeValues::from(uvs));
-    mesh.set_indices(Some(Indices::U32(indices)));
+    mesh.insert_indices(Indices::U32(indices));
     mesh
 }
 
diff --git a/src_testbed/physics/mod.rs b/src_testbed/physics/mod.rs
index 26a55c7..3c69f6e 100644
--- a/src_testbed/physics/mod.rs
+++ b/src_testbed/physics/mod.rs
@@ -3,7 +3,9 @@ use rapier::dynamics::{
     CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet,
     RigidBodySet,
 };
-use rapier::geometry::{BroadPhase, ColliderSet, CollisionEvent, ContactForceEvent, NarrowPhase};
+use rapier::geometry::{
+    ColliderSet, CollisionEvent, ContactForceEvent, DefaultBroadPhase, NarrowPhase,
+};
 use rapier::math::{Real, Vector};
 use rapier::pipeline::{PhysicsHooks, PhysicsPipeline, QueryPipeline};
 
@@ -14,65 +16,79 @@ pub struct PhysicsSnapshot {
     bodies: Vec<u8>,
     colliders: Vec<u8>,
     impulse_joints: Vec<u8>,
+    multibody_joints: Vec<u8>,
+    island_manager: Vec<u8>,
+}
+
+pub struct DeserializedPhysicsSnapshot {
+    pub timestep_id: usize,
+    pub broad_phase: DefaultBroadPhase,
+    pub narrow_phase: NarrowPhase,
+    pub island_manager: IslandManager,
+    pub bodies: RigidBodySet,
+    pub colliders: ColliderSet,
+    pub impulse_joints: ImpulseJointSet,
+    pub multibody_joints: MultibodyJointSet,
 }
 
 impl PhysicsSnapshot {
     pub fn new(
         timestep_id: usize,
-        broad_phase: &BroadPhase,
+        broad_phase: &DefaultBroadPhase,
         narrow_phase: &NarrowPhase,
+        island_manager: &IslandManager,
         bodies: &RigidBodySet,
         colliders: &ColliderSet,
         impulse_joints: &ImpulseJointSet,
+        multibody_joints: &MultibodyJointSet,
     ) -> bincode::Result<Self> {
         Ok(Self {
             timestep_id,
             broad_phase: bincode::serialize(broad_phase)?,
             narrow_phase: bincode::serialize(narrow_phase)?,
+            island_manager: bincode::serialize(island_manager)?,
             bodies: bincode::serialize(bodies)?,
             colliders: bincode::serialize(colliders)?,
             impulse_joints: bincode::serialize(impulse_joints)?,
+            multibody_joints: bincode::serialize(multibody_joints)?,
         })
     }
 
-    pub fn restore(
-        &self,
-    ) -> bincode::Result<(
-        usize,
-        BroadPhase,
-        NarrowPhase,
-        RigidBodySet,
-        ColliderSet,
-        ImpulseJointSet,
-    )> {
-        Ok((
-            self.timestep_id,
-            bincode::deserialize(&self.broad_phase)?,
-            bincode::deserialize(&self.narrow_phase)?,
-            bincode::deserialize(&self.bodies)?,
-            bincode::deserialize(&self.colliders)?,
-            bincode::deserialize(&self.impulse_joints)?,
-        ))
+    pub fn restore(&self) -> bincode::Result<DeserializedPhysicsSnapshot> {
+        Ok(DeserializedPhysicsSnapshot {
+            timestep_id: self.timestep_id,
+            broad_phase: bincode::deserialize(&self.broad_phase)?,
+            narrow_phase: bincode::deserialize(&self.narrow_phase)?,
+            island_manager: bincode::deserialize(&self.island_manager)?,
+            bodies: bincode::deserialize(&self.bodies)?,
+            colliders: bincode::deserialize(&self.colliders)?,
+            impulse_joints: bincode::deserialize(&self.impulse_joints)?,
+            multibody_joints: bincode::deserialize(&self.multibody_joints)?,
+        })
     }
 
     pub fn print_snapshot_len(&self) {
         let total = self.broad_phase.len()
             + self.narrow_phase.len()
+            + self.island_manager.len()
             + self.bodies.len()
             + self.colliders.len()
-            + self.impulse_joints.len();
+            + self.impulse_joints.len()
+            + self.multibody_joints.len();
         println!("Snapshot length: {}B", total);
         println!("|_ broad_phase: {}B", self.broad_phase.len());
         println!("|_ narrow_phase: {}B", self.narrow_phase.len());
+        println!("|_ island_manager: {}B", self.island_manager.len());
         println!("|_ bodies: {}B", self.bodies.len());
         println!("|_ colliders: {}B", self.colliders.len());
         println!("|_ impulse_joints: {}B", self.impulse_joints.len());
+        println!("|_ multibody_joints: {}B", self.multibody_joints.len());
     }
 }
 
 pub struct PhysicsState {
     pub islands: IslandManager,
-    pub broad_phase: BroadPhase,
+    pub broad_phase: DefaultBroadPhase,
     pub narrow_phase: NarrowPhase,
     pub bodies: RigidBodySet,
     pub colliders: ColliderSet,
@@ -96,7 +112,7 @@ impl PhysicsState {
     pub fn new() -> Self {
         Self {
             islands: IslandManager::new(),
-            broad_phase: BroadPhase::new(),
+            broad_phase: DefaultBroadPhase::new(),
             narrow_phase: NarrowPhase::new(),
             bodies: RigidBodySet::new(),
             colliders: ColliderSet::new(),
diff --git a/src_testbed/plugin.rs b/src_testbed/plugin.rs
index 7f19aa9..42fdd17 100644
--- a/src_testbed/plugin.rs
+++ b/src_testbed/plugin.rs
@@ -14,7 +14,7 @@ pub trait TestbedPlugin {
         commands: &mut Commands,
         meshes: &mut Assets<Mesh>,
         materials: &mut Assets<BevyMaterial>,
-        components: &mut Query<(&mut Transform,)>,
+        components: &mut Query<&mut Transform>,
         harness: &mut Harness,
     );
     fn clear_graphics(&mut self, graphics: &mut GraphicsManager, commands: &mut Commands);
@@ -26,7 +26,7 @@ pub trait TestbedPlugin {
         commands: &mut Commands,
         meshes: &mut Assets<Mesh>,
         materials: &mut Assets<BevyMaterial>,
-        components: &mut Query<(&mut Transform,)>,
+        components: &mut Query<&mut Transform>,
         harness: &mut Harness,
     );
     fn update_ui(
@@ -37,7 +37,7 @@ pub trait TestbedPlugin {
         commands: &mut Commands,
         meshes: &mut Assets<Mesh>,
         materials: &mut Assets<BevyMaterial>,
-        components: &mut Query<(&mut Transform,)>,
+        components: &mut Query<&mut Transform>,
     );
     fn profiling_string(&self) -> String;
 }
diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs
index e5a5f13..836fc1f 100644
--- a/src_testbed/testbed.rs
+++ b/src_testbed/testbed.rs
@@ -1,4 +1,4 @@
-#![allow(clippy::bad_bit_mask)] // otherwsie clippy complains because of TestbedStateFlags::NONE which is 0.
+#![allow(clippy::bad_bit_mask)] // otherwise clippy complains because of TestbedStateFlags::NONE which is 0.
 
 use std::env;
 use std::mem;
@@ -7,10 +7,10 @@ use std::num::NonZeroUsize;
 use bevy::prelude::*;
 
 use crate::debug_render::{DebugRenderPipelineResource, RapierDebugRenderPlugin};
-use crate::physics::{PhysicsEvents, PhysicsSnapshot, PhysicsState};
+use crate::physics::{DeserializedPhysicsSnapshot, PhysicsEvents, PhysicsSnapshot, PhysicsState};
 use crate::plugin::TestbedPlugin;
-use crate::ui;
 use crate::{graphics::GraphicsManager, harness::RunState};
+use crate::{mouse, ui};
 
 use na::{self, Point2, Point3, Vector3};
 #[cfg(feature = "dim3")]
@@ -24,15 +24,14 @@ use rapier::dynamics::{
 use rapier::geometry::Ray;
 use rapier::geometry::{ColliderHandle, ColliderSet, NarrowPhase};
 use rapier::math::{Real, Vector};
-use rapier::pipeline::{PhysicsHooks, QueryFilter};
+use rapier::pipeline::{PhysicsHooks, QueryFilter, QueryPipeline};
 
 #[cfg(all(feature = "dim2", feature = "other-backends"))]
 use crate::box2d_backend::Box2dWorld;
 use crate::harness::Harness;
 #[cfg(all(feature = "dim3", feature = "other-backends"))]
 use crate::physx_backend::PhysxWorld;
-use bevy::render::camera::Camera;
-use bevy_core_pipeline::prelude::ClearColor;
+use bevy::render::camera::{Camera, ClearColor};
 use bevy_egui::EguiContexts;
 use bevy_pbr::wireframe::WireframePlugin;
 use bevy_pbr::AmbientLight;
@@ -103,10 +102,12 @@ bitflags! {
     }
 }
 
-#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+#[derive(Copy, Clone, Debug, PartialEq, Eq, Default)]
 pub enum RapierSolverType {
-    SmallStepsPgs,
-    StandardPgs,
+    #[default]
+    TgsSoft,
+    TgsSoftNoWarmstart,
+    PgsLegacy,
 }
 
 pub type SimulationBuilders = Vec<(&'static str, fn(&mut Testbed))>;
@@ -134,7 +135,7 @@ pub struct TestbedState {
     pub solver_type: RapierSolverType,
     pub physx_use_two_friction_directions: bool,
     pub snapshot: Option<PhysicsSnapshot>,
-    nsteps: usize,
+    pub nsteps: usize,
     camera_locked: bool, // Used so that the camera can remain the same before and after we change backend or press the restart button.
 }
 
@@ -155,11 +156,12 @@ pub struct TestbedGraphics<'a, 'b, 'c, 'd, 'e, 'f> {
     commands: &'a mut Commands<'d, 'e>,
     meshes: &'a mut Assets<Mesh>,
     materials: &'a mut Assets<BevyMaterial>,
-    components: &'a mut Query<'b, 'f, (&'c mut Transform,)>,
+    components: &'a mut Query<'b, 'f, &'c mut Transform>,
     #[allow(dead_code)] // Dead in 2D but not in 3D.
     camera_transform: GlobalTransform,
     camera: &'a mut OrbitCamera,
-    keys: &'a Input<KeyCode>,
+    keys: &'a ButtonInput<KeyCode>,
+    mouse: &'a SceneMouse,
 }
 
 pub struct Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
@@ -215,7 +217,7 @@ impl TestbedApp {
             example_names: Vec::new(),
             selected_example: 0,
             selected_backend: RAPIER_BACKEND,
-            solver_type: RapierSolverType::SmallStepsPgs,
+            solver_type: RapierSolverType::default(),
             physx_use_two_friction_directions: true,
             nsteps: 1,
             camera_locked: false,
@@ -399,6 +401,7 @@ impl TestbedApp {
                     brightness: 0.3,
                     ..Default::default()
                 })
+                .init_resource::<mouse::SceneMouse>()
                 .add_plugins(DefaultPlugins.set(window_plugin))
                 .add_plugins(OrbitCameraPlugin)
                 .add_plugins(WireframePlugin)
@@ -418,7 +421,9 @@ impl TestbedApp {
                 .insert_resource(self.builders)
                 .insert_non_send_resource(self.plugins)
                 .add_systems(Update, update_testbed)
-                .add_systems(Update, egui_focus);
+                .add_systems(Update, egui_focus)
+                .add_systems(Update, track_mouse_state);
+
             init(&mut app);
             app.run();
         }
@@ -468,9 +473,18 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> TestbedGraphics<'a, 'b, 'c, 'd, 'e, 'f> {
         )
     }
 
-    pub fn keys(&self) -> &Input<KeyCode> {
+    pub fn keys(&self) -> &ButtonInput<KeyCode> {
         self.keys
     }
+
+    pub fn mouse(&self) -> &SceneMouse {
+        self.mouse
+    }
+
+    #[cfg(feature = "dim3")]
+    pub fn camera_fwd_dir(&self) -> Vector<f32> {
+        (self.camera_transform * -Vec3::Z).normalize().into()
+    }
 }
 
 impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
@@ -660,7 +674,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
     }
 
     #[cfg(feature = "dim3")]
-    fn update_vehicle_controller(&mut self, events: &Input<KeyCode>) {
+    fn update_vehicle_controller(&mut self, events: &ButtonInput<KeyCode>) {
         if self.state.running == RunMode::Stop {
             return;
         }
@@ -671,16 +685,16 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
 
             for key in events.get_pressed() {
                 match *key {
-                    KeyCode::Right => {
+                    KeyCode::ArrowRight => {
                         steering_angle += -0.7;
                     }
-                    KeyCode::Left => {
+                    KeyCode::ArrowLeft => {
                         steering_angle += 0.7;
                     }
-                    KeyCode::Up => {
+                    KeyCode::ArrowUp => {
                         engine_force += 30.0;
                     }
-                    KeyCode::Down => {
+                    KeyCode::ArrowDown => {
                         engine_force += -30.0;
                     }
                     _ => {}
@@ -703,7 +717,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
         }
     }
 
-    fn update_character_controller(&mut self, events: &Input<KeyCode>) {
+    fn update_character_controller(&mut self, events: &ButtonInput<KeyCode>) {
         if self.state.running == RunMode::Stop {
             return;
         }
@@ -715,10 +729,10 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
             #[cfg(feature = "dim2")]
             for key in events.get_pressed() {
                 match *key {
-                    KeyCode::Right => {
+                    KeyCode::ArrowRight => {
                         desired_movement += Vector::x();
                     }
-                    KeyCode::Left => {
+                    KeyCode::ArrowLeft => {
                         desired_movement -= Vector::x();
                     }
                     KeyCode::Space => {
@@ -750,16 +764,16 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
 
                 for key in events.get_pressed() {
                     match *key {
-                        KeyCode::Right => {
+                        KeyCode::ArrowRight => {
                             desired_movement += rot_x;
                         }
-                        KeyCode::Left => {
+                        KeyCode::ArrowLeft => {
                             desired_movement -= rot_x;
                         }
-                        KeyCode::Up => {
+                        KeyCode::ArrowUp => {
                             desired_movement -= rot_z;
                         }
-                        KeyCode::Down => {
+                        KeyCode::ArrowDown => {
                             desired_movement += rot_z;
                         }
                         KeyCode::Space => {
@@ -818,22 +832,22 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
         }
     }
 
-    fn handle_common_events(&mut self, events: &Input<KeyCode>) {
+    fn handle_common_events(&mut self, events: &ButtonInput<KeyCode>) {
         for key in events.get_just_released() {
             match *key {
-                KeyCode::T => {
+                KeyCode::KeyT => {
                     if self.state.running == RunMode::Stop {
                         self.state.running = RunMode::Running;
                     } else {
                         self.state.running = RunMode::Stop;
                     }
                 }
-                KeyCode::S => self.state.running = RunMode::Step,
-                KeyCode::R => self
+                KeyCode::KeyS => self.state.running = RunMode::Step,
+                KeyCode::KeyR => self
                     .state
                     .action_flags
                     .set(TestbedActionFlags::EXAMPLE_CHANGED, true),
-                KeyCode::C => {
+                KeyCode::KeyC => {
                     // Delete 1 collider of 10% of the remaining dynamic bodies.
                     let mut colliders: Vec<_> = self
                         .harness
@@ -859,7 +873,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
                         );
                     }
                 }
-                KeyCode::D => {
+                KeyCode::KeyD => {
                     // Delete 10% of the remaining dynamic bodies.
                     let dynamic_bodies: Vec<_> = self
                         .harness
@@ -884,7 +898,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
                         );
                     }
                 }
-                KeyCode::J => {
+                KeyCode::KeyJ => {
                     // Delete 10% of the remaining impulse_joints.
                     let impulse_joints: Vec<_> = self
                         .harness
@@ -898,7 +912,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
                         self.harness.physics.impulse_joints.remove(*to_delete, true);
                     }
                 }
-                KeyCode::A => {
+                KeyCode::KeyA => {
                     // Delete 10% of the remaining multibody_joints.
                     let multibody_joints: Vec<_> = self
                         .harness
@@ -915,7 +929,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
                             .remove(*to_delete, true);
                     }
                 }
-                KeyCode::M => {
+                KeyCode::KeyM => {
                     // Delete one remaining multibody.
                     let to_delete = self
                         .harness
@@ -1013,10 +1027,14 @@ fn draw_contacts(_nf: &NarrowPhase, _colliders: &ColliderSet) {
 
 #[cfg(feature = "dim3")]
 fn setup_graphics_environment(mut commands: Commands) {
+    commands.insert_resource(AmbientLight {
+        brightness: 100.0,
+        ..Default::default()
+    });
+
     commands.spawn(DirectionalLightBundle {
         directional_light: DirectionalLight {
-            illuminance: 10000.0,
-            shadows_enabled: true,
+            shadows_enabled: false,
             ..Default::default()
         },
         transform: Transform {
@@ -1042,7 +1060,8 @@ fn setup_graphics_environment(mut commands: Commands) {
         .insert(OrbitCamera {
             rotate_sensitivity: 0.05,
             ..OrbitCamera::default()
-        });
+        })
+        .insert(MainCamera);
 }
 
 #[cfg(feature = "dim2")]
@@ -1073,7 +1092,8 @@ fn setup_graphics_environment(mut commands: Commands) {
             zoom: 100.0,
             pan_sensitivity: 0.02,
             ..OrbitCamera::default()
-        });
+        })
+        .insert(MainCamera);
 }
 
 fn egui_focus(mut ui_context: EguiContexts, mut cameras: Query<&mut OrbitCamera>) {
@@ -1086,12 +1106,14 @@ fn egui_focus(mut ui_context: EguiContexts, mut cameras: Query<&mut OrbitCamera>
     }
 }
 
+use crate::mouse::{track_mouse_state, MainCamera, SceneMouse};
 use bevy::window::PrimaryWindow;
 
 fn update_testbed(
     mut commands: Commands,
     windows: Query<&Window, With<PrimaryWindow>>,
     // mut pipelines: ResMut<Assets<RenderPipelineDescriptor>>,
+    mouse: Res<SceneMouse>,
     mut meshes: ResMut<Assets<Mesh>>,
     mut materials: ResMut<Assets<BevyMaterial>>,
     builders: ResMut<SceneBuilders>,
@@ -1102,13 +1124,13 @@ fn update_testbed(
     #[cfg(feature = "other-backends")] mut other_backends: NonSendMut<OtherBackends>,
     mut plugins: NonSendMut<Plugins>,
     mut ui_context: EguiContexts,
-    mut gfx_components: Query<(&mut Transform,)>,
+    mut gfx_components: Query<&mut Transform>,
     mut cameras: Query<(&Camera, &GlobalTransform, &mut OrbitCamera)>,
-    keys: Res<Input<KeyCode>>,
+    mut material_handles: Query<&mut Handle<BevyMaterial>>,
+    keys: Res<ButtonInput<KeyCode>>,
 ) {
     let meshes = &mut *meshes;
     let materials = &mut *materials;
-    let prev_example = state.selected_example;
 
     // Handle inputs
     {
@@ -1121,6 +1143,7 @@ fn update_testbed(
             camera_transform: *cameras.single().1,
             camera: &mut cameras.single_mut().2,
             keys: &keys,
+            mouse: &mouse,
         };
 
         let mut testbed = Testbed {
@@ -1198,18 +1221,6 @@ fn update_testbed(
             }
             plugins.0.clear();
 
-            if state.selected_example != prev_example {
-                harness.physics.integration_parameters = IntegrationParameters::default();
-
-                match state.solver_type {
-                    RapierSolverType::SmallStepsPgs => {} // It’s already the default.
-                    RapierSolverType::StandardPgs => harness
-                        .physics
-                        .integration_parameters
-                        .switch_to_standard_pgs_solver(),
-                }
-            }
-
             let selected_example = state.selected_example;
             let graphics = &mut *graphics;
             let meshes = &mut *meshes;
@@ -1223,6 +1234,7 @@ fn update_testbed(
                 camera_transform: *cameras.single().1,
                 camera: &mut cameras.single_mut().2,
                 keys: &keys,
+                mouse: &mouse,
             };
 
             let mut testbed = Testbed {
@@ -1250,9 +1262,11 @@ fn update_testbed(
                 harness.state.timestep_id,
                 &harness.physics.broad_phase,
                 &harness.physics.narrow_phase,
+                &harness.physics.islands,
                 &harness.physics.bodies,
                 &harness.physics.colliders,
                 &harness.physics.impulse_joints,
+                &harness.physics.multibody_joints,
             )
             .ok();
 
@@ -1269,17 +1283,36 @@ fn update_testbed(
                 .action_flags
                 .set(TestbedActionFlags::RESTORE_SNAPSHOT, false);
             if let Some(snapshot) = &state.snapshot {
-                if let Ok(w) = snapshot.restore() {
+                if let Ok(DeserializedPhysicsSnapshot {
+                    timestep_id,
+                    broad_phase,
+                    narrow_phase,
+                    island_manager,
+                    bodies,
+                    colliders,
+                    impulse_joints,
+                    multibody_joints,
+                }) = snapshot.restore()
+                {
                     clear(&mut commands, &mut state, &mut graphics, &mut plugins);
 
                     for plugin in &mut plugins.0 {
                         plugin.clear_graphics(&mut graphics, &mut commands);
                     }
 
-                    // set_world(w.3, w.4, w.5);
-                    harness.physics.broad_phase = w.1;
-                    harness.physics.narrow_phase = w.2;
-                    harness.state.timestep_id = w.0;
+                    harness.state.timestep_id = timestep_id;
+                    harness.physics.broad_phase = broad_phase;
+                    harness.physics.narrow_phase = narrow_phase;
+                    harness.physics.islands = island_manager;
+                    harness.physics.bodies = bodies;
+                    harness.physics.colliders = colliders;
+                    harness.physics.impulse_joints = impulse_joints;
+                    harness.physics.multibody_joints = multibody_joints;
+                    harness.physics.query_pipeline = QueryPipeline::new();
+
+                    state
+                        .action_flags
+                        .set(TestbedActionFlags::RESET_WORLD_GRAPHICS, true);
                 }
             }
         }
@@ -1340,15 +1373,15 @@ fn update_testbed(
         {
             if state.flags.contains(TestbedStateFlags::SLEEP) {
                 for (_, body) in harness.physics.bodies.iter_mut() {
-                    body.activation_mut().linear_threshold =
-                        RigidBodyActivation::default_linear_threshold();
+                    body.activation_mut().normalized_linear_threshold =
+                        RigidBodyActivation::default_normalized_linear_threshold();
                     body.activation_mut().angular_threshold =
                         RigidBodyActivation::default_angular_threshold();
                 }
             } else {
                 for (_, body) in harness.physics.bodies.iter_mut() {
                     body.wake_up(true);
-                    body.activation_mut().linear_threshold = -1.0;
+                    body.activation_mut().normalized_linear_threshold = -1.0;
                 }
             }
         }
@@ -1375,6 +1408,7 @@ fn update_testbed(
                     camera_transform: *cameras.single().1,
                     camera: &mut cameras.single_mut().2,
                     keys: &keys,
+                    mouse: &mouse,
                 };
                 harness.step_with_graphics(Some(&mut testbed_graphics));
 
@@ -1427,7 +1461,7 @@ fn update_testbed(
     if let Ok(window) = windows.get_single() {
         for (camera, camera_pos, _) in cameras.iter_mut() {
             highlight_hovered_body(
-                &mut *materials,
+                &mut material_handles,
                 &mut graphics,
                 &mut state,
                 &harness.physics,
@@ -1481,7 +1515,7 @@ fn clear(
 
 #[cfg(feature = "dim2")]
 fn highlight_hovered_body(
-    _materials: &mut Assets<BevyMaterial>,
+    _material_handles: &mut Query<&mut Handle<BevyMaterial>>,
     _graphics_manager: &mut GraphicsManager,
     _testbed_state: &mut TestbedState,
     _physics: &PhysicsState,
@@ -1494,7 +1528,7 @@ fn highlight_hovered_body(
 
 #[cfg(feature = "dim3")]
 fn highlight_hovered_body(
-    materials: &mut Assets<BevyMaterial>,
+    material_handles: &mut Query<&mut Handle<BevyMaterial>>,
     graphics_manager: &mut GraphicsManager,
     testbed_state: &mut TestbedState,
     physics: &PhysicsState,
@@ -1505,7 +1539,9 @@ fn highlight_hovered_body(
     if let Some(highlighted_body) = testbed_state.highlighted_body {
         if let Some(nodes) = graphics_manager.body_nodes_mut(highlighted_body) {
             for node in nodes {
-                node.unselect(materials)
+                if let Ok(mut handle) = material_handles.get_mut(node.entity) {
+                    *handle = node.material.clone_weak()
+                };
             }
         }
     }
@@ -1537,8 +1573,12 @@ fn highlight_hovered_body(
 
             if let Some(parent_handle) = collider.parent() {
                 testbed_state.highlighted_body = Some(parent_handle);
+                let selection_material = graphics_manager.selection_material();
+
                 for node in graphics_manager.body_nodes_mut(parent_handle).unwrap() {
-                    node.select(materials)
+                    if let Ok(mut handle) = material_handles.get_mut(node.entity) {
+                        *handle = selection_material.clone_weak();
+                    }
                 }
             }
         }
diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs
index 7d50f62..aa9dc7a 100644
--- a/src_testbed/ui.rs
+++ b/src_testbed/ui.rs
@@ -10,8 +10,9 @@ use crate::testbed::{
 };
 
 use crate::PhysicsState;
-use bevy_egui::egui::Slider;
+use bevy_egui::egui::{Slider, Ui};
 use bevy_egui::{egui, EguiContexts};
+use rapier::dynamics::IntegrationParameters;
 
 pub fn update_ui(
     ui_context: &mut EguiContexts,
@@ -79,9 +80,12 @@ pub fn update_ui(
 
         ui.separator();
 
+        ui.collapsing("Scene infos", |ui| {
+            scene_infos_ui(ui, &harness.physics);
+        });
         ui.collapsing("Profile infos", |ui| {
             ui.horizontal_wrapped(|ui| {
-                ui.label(profiling_string(&harness.physics.pipeline.counters))
+                profiling_ui(ui, &harness.physics.pipeline.counters);
             });
         });
         ui.collapsing("Serialization infos", |ui| {
@@ -109,8 +113,9 @@ pub fn update_ui(
                 .selected_text(format!("{:?}", state.solver_type))
                 .show_ui(ui, |ui| {
                     let solver_types = [
-                        RapierSolverType::SmallStepsPgs,
-                        RapierSolverType::StandardPgs,
+                        RapierSolverType::TgsSoft,
+                        RapierSolverType::TgsSoftNoWarmstart,
+                        RapierSolverType::PgsLegacy,
                     ];
                     for sty in solver_types {
                         changed = ui
@@ -122,11 +127,15 @@ pub fn update_ui(
 
             if changed {
                 match state.solver_type {
-                    RapierSolverType::SmallStepsPgs => {
-                        integration_parameters.switch_to_small_steps_pgs_solver()
+                    RapierSolverType::TgsSoft => {
+                        *integration_parameters = IntegrationParameters::tgs_soft();
                     }
-                    RapierSolverType::StandardPgs => {
-                        integration_parameters.switch_to_standard_pgs_solver()
+                    RapierSolverType::TgsSoftNoWarmstart => {
+                        *integration_parameters =
+                            IntegrationParameters::tgs_soft_without_warmstart();
+                    }
+                    RapierSolverType::PgsLegacy => {
+                        *integration_parameters = IntegrationParameters::pgs_legacy();
                     }
                 }
             }
@@ -146,10 +155,54 @@ pub fn update_ui(
             ui.add(
                 Slider::new(
                     &mut integration_parameters.num_additional_friction_iterations,
-                    1..=40,
+                    0..=40,
                 )
                 .text("num additional frict. iters."),
             );
+            ui.add(
+                Slider::new(
+                    &mut integration_parameters.num_internal_stabilization_iterations,
+                    0..=100,
+                )
+                .text("max internal stabilization iters."),
+            );
+            ui.add(
+                Slider::new(&mut integration_parameters.warmstart_coefficient, 0.0..=1.0)
+                    .text("warmstart coefficient"),
+            );
+
+            let mut substep_params = *integration_parameters;
+            substep_params.dt /= substep_params.num_solver_iterations.get() as Real;
+            let curr_erp = substep_params.contact_erp();
+            let curr_cfm_factor = substep_params.contact_cfm_factor();
+            ui.add(
+                Slider::new(
+                    &mut integration_parameters.contact_natural_frequency,
+                    0.01..=120.0,
+                )
+                .text(format!("contacts Hz (erp = {:.3})", curr_erp)),
+            );
+            ui.add(
+                Slider::new(
+                    &mut integration_parameters.contact_damping_ratio,
+                    0.01..=20.0,
+                )
+                .text(format!(
+                    "damping ratio (cfm-factor = {:.3})",
+                    curr_cfm_factor
+                )),
+            );
+            ui.add(
+                Slider::new(
+                    &mut integration_parameters.joint_natural_frequency,
+                    0.0..=1200000.0,
+                )
+                .text("joint erp"),
+            );
+            ui.add(
+                Slider::new(&mut integration_parameters.joint_damping_ratio, 0.0..=20.0)
+                    .text("joint damping ratio"),
+            );
         }
 
         #[cfg(feature = "parallel")]
@@ -167,6 +220,8 @@ pub fn update_ui(
             Slider::new(&mut integration_parameters.min_island_size, 1..=10_000)
                 .text("min island size"),
         );
+        ui.add(Slider::new(&mut state.nsteps, 1..=100).text("sims. per frame"));
+
         let mut frequency = integration_parameters.inv_dt().round() as u32;
         ui.add(Slider::new(&mut frequency, 0..=240).text("frequency (Hz)"));
         integration_parameters.set_inv_dt(frequency as Real);
@@ -222,43 +277,87 @@ pub fn update_ui(
     });
 }
 
-fn profiling_string(counters: &Counters) -> String {
-    format!(
-        r#"Total: {:.2}ms
-Collision detection: {:.2}ms
-|_ Broad-phase: {:.2}ms
-   Narrow-phase: {:.2}ms
-Island computation: {:.2}ms
-Solver: {:.2}ms
-|_ Velocity assembly: {:.2}ms
-   Velocity resolution: {:.2}ms
-   Velocity integration: {:.2}ms
-   Position assembly: {:.2}ms
-   Position resolution: {:.2}ms
-CCD: {:.2}ms
-|_ # of substeps: {}
-   TOI computation: {:.2}ms
-   Broad-phase: {:.2}ms
-   Narrow-phase: {:.2}ms
-   Solver: {:.2}ms"#,
+fn scene_infos_ui(ui: &mut Ui, physics: &PhysicsState) {
+    ui.label(format!("# rigid-bodies: {}", physics.bodies.len()));
+    ui.label(format!("# colliders: {}", physics.colliders.len()));
+    ui.label(format!("# impulse joint: {}", physics.impulse_joints.len()));
+    // ui.label(format!(
+    //     "# multibody joint: {}",
+    //     physics.multibody_joints.len()
+    // ));
+}
+
+fn profiling_ui(ui: &mut Ui, counters: &Counters) {
+    egui::CollapsingHeader::new(format!(
+        "Total: {:.2}ms - {} fps",
         counters.step_time(),
-        counters.collision_detection_time(),
-        counters.broad_phase_time(),
-        counters.narrow_phase_time(),
-        counters.island_construction_time(),
-        counters.solver_time(),
-        counters.solver.velocity_assembly_time.time(),
-        counters.velocity_resolution_time(),
-        counters.solver.velocity_update_time.time(),
-        counters.solver.position_assembly_time.time(),
-        counters.position_resolution_time(),
-        counters.ccd_time(),
-        counters.ccd.num_substeps,
-        counters.ccd.toi_computation_time.time(),
-        counters.ccd.broad_phase_time.time(),
-        counters.ccd.narrow_phase_time.time(),
-        counters.ccd.solver_time.time(),
-    )
+        (1000.0 / counters.step_time()).round()
+    ))
+    .id_source("total")
+    .show(ui, |ui| {
+        egui::CollapsingHeader::new(format!(
+            "Collision detection: {:.2}ms",
+            counters.collision_detection_time()
+        ))
+        .id_source("collision detection")
+        .show(ui, |ui| {
+            ui.label(format!("Broad-phase: {:.2}ms", counters.broad_phase_time()));
+            ui.label(format!(
+                "Narrow-phase: {:.2}ms",
+                counters.narrow_phase_time()
+            ));
+        });
+        egui::CollapsingHeader::new(format!("Solver: {:.2}ms", counters.solver_time()))
+            .id_source("solver")
+            .show(ui, |ui| {
+                ui.label(format!(
+                    "Velocity assembly: {:.2}ms",
+                    counters.solver.velocity_assembly_time.time()
+                ));
+                ui.label(format!(
+                    "Velocity resolution: {:.2}ms",
+                    counters.velocity_resolution_time()
+                ));
+                ui.label(format!(
+                    "Velocity integration: {:.2}ms",
+                    counters.solver.velocity_update_time.time()
+                ));
+                ui.label(format!(
+                    "Writeback: {:.2}ms",
+                    counters.solver.velocity_writeback_time.time()
+                ));
+            });
+        egui::CollapsingHeader::new(format!("CCD: {:.2}ms", counters.ccd_time()))
+            .id_source("ccd")
+            .show(ui, |ui| {
+                ui.label(format!("# of substeps: {}", counters.ccd.num_substeps));
+                ui.label(format!(
+                    "TOI computation: {:.2}ms",
+                    counters.ccd.toi_computation_time.time(),
+                ));
+                ui.label(format!(
+                    "Broad-phase: {:.2}ms",
+                    counters.ccd.broad_phase_time.time()
+                ));
+                ui.label(format!(
+                    "Narrow-phase: {:.2}ms",
+                    counters.ccd.narrow_phase_time.time(),
+                ));
+                ui.label(format!("Solver: {:.2}ms", counters.ccd.solver_time.time()));
+            });
+        ui.label(format!(
+            "Island computation: {:.2}ms",
+            counters.island_construction_time()
+        ));
+        ui.label(format!(
+            "Query pipeline: {:.2}ms",
+            counters.query_pipeline_update_time()
+        ));
+        ui.label(format!(
+            "User changes: {:.2}ms",
+            counters.stages.user_changes.time()
+        ));
+    });
 }
 
 fn serialization_string(timestep_id: usize, physics: &PhysicsState) -> String {