Merge branch 'master' into collider-builder-debug

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);

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

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,

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
         )
     }
 }

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)
     }
 }

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,
         };

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
-    }
-}

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,

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()
+    }
+}

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;

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(

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;

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 {

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);
+    }
+}

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
             }

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,

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;
         }
 

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()
         }

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.  

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>],

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>,

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,

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>,

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,

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();
         }
     }
 

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);
+                }
             }
         }
 

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);
             }
         }
     }

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();
         }
     }
 

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;

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);
             }
         }
     }

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();
     }
 }

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,

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.

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(

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!(

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;
 

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

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>,
+    );
+}

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);
     }
 }

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;

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>>,
     ) {

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();

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)
     }
 }

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

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);

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,
         }
     }

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 {

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))
-    }
 }

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;

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;

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;

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,

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,

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));
+    }
 }

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(

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.

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,