First working version of non-linear CCD based on single-substep motion-clamping.

Cargo.toml

@@ -19,10 +19,10 @@ members = [ "build/rapier2d", "build/rapier2d-f64", "build/rapier_testbed2d", "e
 
 #kiss3d = { git = "https://github.com/sebcrozet/kiss3d" }
 #nalgebra = { git = "https://github.com/dimforge/nalgebra", branch = "dev" }
-#parry2d = { git = "https://github.com/dimforge/parry" }
+parry2d = { git = "https://github.com/dimforge/parry", branch = "special_cases" }
-#parry3d = { git = "https://github.com/dimforge/parry" }
+parry3d = { git = "https://github.com/dimforge/parry", branch = "special_cases" }
-#parry2d-f64 = { git = "https://github.com/dimforge/parry" }
+parry2d-f64 = { git = "https://github.com/dimforge/parry", branch = "special_cases" }
-#parry3d-f64 = { git = "https://github.com/dimforge/parry" }
+parry3d-f64 = { git = "https://github.com/dimforge/parry", branch = "special_cases" }
 #ncollide2d = { git = "https://github.com/dimforge/ncollide" }
 #ncollide3d = { git = "https://github.com/dimforge/ncollide" }
 #nphysics2d = { git = "https://github.com/dimforge/nphysics" }

benchmarks3d/all_benchmarks3.rs

@@ -13,6 +13,7 @@ use std::cmp::Ordering;
 mod balls3;
 mod boxes3;
 mod capsules3;
+mod ccd3;
 mod compound3;
 mod convex_polyhedron3;
 mod heightfield3;
@@ -52,6 +53,7 @@ pub fn main() {
         ("Balls", balls3::init_world),
         ("Boxes", boxes3::init_world),
         ("Capsules", capsules3::init_world),
+        ("CCD", ccd3::init_world),
         ("Compound", compound3::init_world),
         ("Convex polyhedron", convex_polyhedron3::init_world),
         ("Heightfield", heightfield3::init_world),

benchmarks3d/ccd3.rs

@@ -0,0 +1,85 @@
+use na::Point3;
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 100.1;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let num = 4;
+    let numj = 20;
+    let rad = 1.0;
+
+    let shiftx = rad * 2.0 + rad;
+    let shifty = rad * 2.0 + rad;
+    let shiftz = rad * 2.0 + rad;
+    let centerx = shiftx * (num / 2) as f32;
+    let centery = shifty / 2.0;
+    let centerz = shiftz * (num / 2) as f32;
+
+    let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5;
+
+    for j in 0usize..numj {
+        for i in 0..num {
+            for k in 0usize..num {
+                let x = i as f32 * shiftx - centerx + offset;
+                let y = j as f32 * shifty + centery + 3.0;
+                let z = k as f32 * shiftz - centerz + offset;
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::new_dynamic()
+                    .translation(x, y, z)
+                    .linvel(0.0, -1000.0, 0.0)
+                    .ccd_enabled(true)
+                    .build();
+                let handle = bodies.insert(rigid_body);
+
+                let collider = match j % 5 {
+                    0 => ColliderBuilder::cuboid(rad, rad, rad),
+                    1 => ColliderBuilder::ball(rad),
+                    // Rounded cylinders are much more efficient that cylinder, even if the
+                    // rounding margin is small.
+                    // 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0),
+                    // 3 => ColliderBuilder::cone(rad, rad),
+                    _ => ColliderBuilder::capsule_y(rad, rad),
+                };
+
+                colliders.insert(collider.build(), handle, &mut bodies);
+            }
+        }
+
+        offset -= 0.05 * rad * (num as f32 - 1.0);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}

examples2d/all_examples2.rs

@@ -11,6 +11,7 @@ use rapier_testbed2d::Testbed;
 use std::cmp::Ordering;
 
 mod add_remove2;
+mod ccd2;
 mod collision_groups2;
 mod convex_polygons2;
 mod damping2;
@@ -60,6 +61,7 @@ pub fn main() {
 
     let mut builders: Vec<(_, fn(&mut Testbed))> = vec![
         ("Add remove", add_remove2::init_world),
+        ("CCD", ccd2::init_world),
         ("Collision groups", collision_groups2::init_world),
         ("Convex polygons", convex_polygons2::init_world),
         ("Damping", damping2::init_world),

examples3d/all_examples3.rs

@@ -10,6 +10,7 @@ use inflector::Inflector;
 use rapier_testbed3d::Testbed;
 use std::cmp::Ordering;
 
+mod ccd3;
 mod collision_groups3;
 mod compound3;
 mod convex_decomposition3;
@@ -77,6 +78,7 @@ pub fn main() {
     let mut builders: Vec<(_, fn(&mut Testbed))> = vec![
         ("Fountain", fountain3::init_world),
         ("Primitives", primitives3::init_world),
+        ("CCD", ccd3::init_world),
         ("Collision groups", collision_groups3::init_world),
         ("Compound", compound3::init_world),
         ("Convex decomposition", convex_decomposition3::init_world),

examples3d/ccd3.rs

@@ -0,0 +1,145 @@
+use na::{Isometry3, Point3, Vector3};
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+fn create_wall(
+    testbed: &mut Testbed,
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    offset: Vector3<f32>,
+    stack_height: usize,
+    half_extents: Vector3<f32>,
+) {
+    let shift = half_extents * 2.0;
+    for i in 0usize..stack_height {
+        for j in i..stack_height {
+            let fj = j as f32;
+            let fi = i as f32;
+            let x = offset.x;
+            let y = fi * shift.y + offset.y;
+            let z = (fi * shift.z / 2.0) + (fj - fi) * shift.z + offset.z
+                - stack_height as f32 * half_extents.z;
+
+            // Build the rigid body.
+            let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+            let handle = bodies.insert(rigid_body);
+            let collider =
+                ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z).build();
+            colliders.insert(collider, handle, bodies);
+            testbed.set_body_color(handle, Point3::new(218. / 255., 201. / 255., 1.0));
+        }
+    }
+}
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 50.0;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+    colliders.insert(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create the pyramids.
+     */
+    let num_z = 8;
+    let num_x = 5;
+    let shift_y = ground_height + 0.5;
+    let shift_z = (num_z as f32 + 2.0) * 2.0;
+
+    for i in 0..num_x {
+        let x = i as f32 * 6.0;
+        create_wall(
+            testbed,
+            &mut bodies,
+            &mut colliders,
+            Vector3::new(x, shift_y, 0.0),
+            num_z,
+            Vector3::new(0.5, 0.5, 1.0),
+        );
+
+        create_wall(
+            testbed,
+            &mut bodies,
+            &mut colliders,
+            Vector3::new(x, shift_y, shift_z),
+            num_z,
+            Vector3::new(0.5, 0.5, 1.0),
+        );
+    }
+
+    /*
+     * Create two very fast rigid-bodies.
+     * The first one has CCD enabled and a sensor collider attached to it.
+     * The second one has CCD enabled and a collider attached to it.
+     */
+    let collider = ColliderBuilder::ball(1.0)
+        .density(10.0)
+        .sensor(true)
+        .build();
+    let mut rigid_body = RigidBodyBuilder::new_dynamic()
+        .linvel(1000.0, 0.0, 0.0)
+        .translation(-20.0, shift_y + 2.0, 0.0)
+        .ccd_enabled(true)
+        .build();
+    let sensor_handle = bodies.insert(rigid_body);
+    colliders.insert(collider, sensor_handle, &mut bodies);
+
+    // Second rigid-body with CCD enabled.
+    let collider = ColliderBuilder::ball(1.0).density(10.0).build();
+    let mut rigid_body = RigidBodyBuilder::new_dynamic()
+        .linvel(1000.0, 0.0, 0.0)
+        .translation(-20.0, shift_y + 2.0, shift_z)
+        .ccd_enabled(true)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    colliders.insert(collider.clone(), handle, &mut bodies);
+
+    // Callback that will be executed on the main loop to handle proximities.
+    testbed.add_callback(move |_, mut graphics, physics, events, _| {
+        while let Ok(prox) = events.intersection_events.try_recv() {
+            let color = if prox.intersecting {
+                Point3::new(1.0, 1.0, 0.0)
+            } else {
+                Point3::new(0.5, 0.5, 1.0)
+            };
+
+            let parent_handle1 = physics.colliders.get(prox.collider1).unwrap().parent();
+            let parent_handle2 = physics.colliders.get(prox.collider2).unwrap().parent();
+
+            if let Some(graphics) = &mut graphics {
+                if parent_handle1 != ground_handle && parent_handle1 != sensor_handle {
+                    graphics.set_body_color(parent_handle1, color);
+                }
+                if parent_handle2 != ground_handle && parent_handle2 != sensor_handle {
+                    graphics.set_body_color(parent_handle2, color);
+                }
+            }
+        }
+    });
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}

src/data/coarena.rs

@@ -29,6 +29,20 @@ impl<T> Coarena<T> {
             .and_then(|(gg, t)| if g == *gg { Some(t) } else { None })
     }
 
+    /// Inserts an element into this coarena.
+    pub fn insert(&mut self, a: Index, value: T)
+    where
+        T: Clone + Default,
+    {
+        let (i1, g1) = a.into_raw_parts();
+
+        if self.data.len() <= i1 {
+            self.data.resize(i1 + 1, (u32::MAX as u64, T::default()));
+        }
+
+        self.data[i1] = (g1, value);
+    }
+
     /// Ensure that elements at the two given indices exist in this coarena, and return their reference.
     ///
     /// Missing elements are created automatically and initialized with the `default` value.

src/dynamics/ccd/toi_entry.rs

@@ -0,0 +1,147 @@
+use crate::data::Coarena;
+use crate::dynamics::ccd::ccd_solver::CCDContact;
+use crate::dynamics::ccd::CCDData;
+use crate::dynamics::{IntegrationParameters, RigidBody, RigidBodyHandle};
+use crate::geometry::{Collider, ColliderHandle};
+use crate::math::{Isometry, Real};
+use crate::parry::query::PersistentQueryDispatcher;
+use crate::utils::WCross;
+use na::{RealField, Unit};
+use parry::query::{NonlinearRigidMotion, QueryDispatcher, TOI};
+
+#[derive(Copy, Clone, Debug)]
+pub struct TOIEntry {
+    pub toi: Real,
+    pub c1: ColliderHandle,
+    pub b1: RigidBodyHandle,
+    pub c2: ColliderHandle,
+    pub b2: RigidBodyHandle,
+    pub is_intersection_test: bool,
+    pub timestamp: usize,
+}
+
+impl TOIEntry {
+    fn new(
+        toi: Real,
+        c1: ColliderHandle,
+        b1: RigidBodyHandle,
+        c2: ColliderHandle,
+        b2: RigidBodyHandle,
+        is_intersection_test: bool,
+        timestamp: usize,
+    ) -> Self {
+        Self {
+            toi,
+            c1,
+            b1,
+            c2,
+            b2,
+            is_intersection_test,
+            timestamp,
+        }
+    }
+
+    pub fn try_from_colliders<QD: ?Sized + PersistentQueryDispatcher<(), ()>>(
+        params: &IntegrationParameters,
+        query_dispatcher: &QD,
+        ch1: ColliderHandle,
+        ch2: ColliderHandle,
+        c1: &Collider,
+        c2: &Collider,
+        b1: &RigidBody,
+        b2: &RigidBody,
+        frozen1: Option<Real>,
+        frozen2: Option<Real>,
+        start_time: Real,
+        end_time: Real,
+        body_params: &Coarena<CCDData>,
+    ) -> Option<Self> {
+        assert!(start_time <= end_time);
+
+        let linvel1 = frozen1.is_none() as u32 as Real * b1.linvel;
+        let linvel2 = frozen2.is_none() as u32 as Real * b2.linvel;
+
+        let vel12 = linvel2 - linvel1;
+        let thickness = (c1.shape().ccd_thickness() + c2.shape().ccd_thickness());
+
+        if params.dt * vel12.norm() < thickness {
+            return None;
+        }
+
+        let is_intersection_test = c1.is_sensor() || c2.is_sensor();
+
+        let body_params1 = body_params.get(c1.parent.0)?;
+        let body_params2 = body_params.get(c2.parent.0)?;
+
+        // Compute the TOI.
+        let mut motion1 = body_params1.motion(params.dt, b1, 0.0);
+        let mut motion2 = body_params2.motion(params.dt, b2, 0.0);
+
+        if let Some(t) = frozen1 {
+            motion1.freeze(t);
+        }
+
+        if let Some(t) = frozen2 {
+            motion2.freeze(t);
+        }
+
+        let mut toi;
+        let motion_c1 = motion1.prepend(*c1.position_wrt_parent());
+        let motion_c2 = motion2.prepend(*c2.position_wrt_parent());
+
+        // println!("start_time: {}", start_time);
+
+        // If this is just an intersection test (i.e. with sensors)
+        // then we can stop the TOI search immediately if it starts with
+        // a penetration because we don't care about the whether the velocity
+        // at the impact is a separating velocity or not.
+        // If the TOI search involves two non-sensor colliders then
+        // we don't want to stop the TOI search at the first penetration
+        // because the colliders may be in a separating trajectory.
+        let stop_at_penetration = is_intersection_test;
+
+        let res_toi = query_dispatcher
+            .nonlinear_time_of_impact(
+                &motion_c1,
+                c1.shape(),
+                &motion_c2,
+                c2.shape(),
+                start_time,
+                end_time,
+                stop_at_penetration,
+            )
+            .ok();
+
+        toi = res_toi??;
+
+        Some(Self::new(
+            toi.toi,
+            ch1,
+            c1.parent(),
+            ch2,
+            c2.parent(),
+            is_intersection_test,
+            0,
+        ))
+    }
+}
+
+impl PartialOrd for TOIEntry {
+    fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
+        (-self.toi).partial_cmp(&(-other.toi))
+    }
+}
+
+impl Ord for TOIEntry {
+    fn cmp(&self, other: &Self) -> std::cmp::Ordering {
+        self.partial_cmp(other).unwrap()
+    }
+}
+
+impl PartialEq for TOIEntry {
+    fn eq(&self, other: &Self) -> bool {
+        self.toi == other.toi
+    }
+}
+
+impl Eq for TOIEntry {}

src/dynamics/mod.rs

@@ -18,6 +18,7 @@ pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBui
 pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet};
 pub use parry::mass_properties::MassProperties;
 // #[cfg(not(feature = "parallel"))]
+pub use self::ccd::CCDSolver;
 pub use self::coefficient_combine_rule::CoefficientCombineRule;
 pub(crate) use self::joint::JointGraphEdge;
 pub(crate) use self::rigid_body::RigidBodyChanges;
@@ -26,6 +27,7 @@ pub(crate) use self::solver::IslandSolver;
 #[cfg(feature = "parallel")]
 pub(crate) use self::solver::ParallelIslandSolver;
 
+mod ccd;
 mod coefficient_combine_rule;
 mod integration_parameters;
 mod joint;

src/dynamics/rigid_body.rs

@@ -36,6 +36,7 @@ bitflags::bitflags! {
         const ROTATION_LOCKED_X = 1 << 1;
         const ROTATION_LOCKED_Y = 1 << 2;
         const ROTATION_LOCKED_Z = 1 << 3;
+        const CCD_ENABLED = 1 << 4;
     }
 }
 
@@ -58,7 +59,16 @@ bitflags::bitflags! {
 pub struct RigidBody {
     /// The world-space position of the rigid-body.
     pub(crate) position: Isometry<Real>,
-    pub(crate) predicted_position: Isometry<Real>,
+    /// The next position of the rigid-body.
+    ///
+    /// At the beginning of the timestep, and when the
+    /// timestep is complete we must have position == next_position
+    /// except for kinematic bodies.
+    ///
+    /// The next_position is updated after the velocity and position
+    /// resolution. Then it is either validated (ie. we set position := set_position)
+    /// or clamped by CCD.
+    pub(crate) next_position: Isometry<Real>,
     /// The local mass properties of the rigid-body.
     pub(crate) mass_properties: MassProperties,
     /// The world-space center of mass of the rigid-body.
@@ -76,6 +86,10 @@ pub struct RigidBody {
     pub linear_damping: Real,
     /// Damping factor for gradually slowing down the angular motion of the rigid-body.
     pub angular_damping: Real,
+    /// The maximum linear velocity this rigid-body can reach.
+    pub max_linear_velocity: Real,
+    /// The maximum angular velocity this rigid-body can reach.
+    pub max_angular_velocity: Real,
     /// Accumulation of external forces (only for dynamic bodies).
     pub(crate) force: Vector<Real>,
     /// Accumulation of external torques (only for dynamic bodies).
@@ -97,13 +111,14 @@ pub struct RigidBody {
     dominance_group: i8,
     /// User-defined data associated to this rigid-body.
     pub user_data: u128,
+    pub(crate) ccd_thickness: Real,
 }
 
 impl RigidBody {
     fn new() -> Self {
         Self {
             position: Isometry::identity(),
-            predicted_position: Isometry::identity(),
+            next_position: Isometry::identity(),
             mass_properties: MassProperties::zero(),
             world_com: Point::origin(),
             effective_inv_mass: 0.0,
@@ -115,6 +130,8 @@ impl RigidBody {
             gravity_scale: 1.0,
             linear_damping: 0.0,
             angular_damping: 0.0,
+            max_linear_velocity: Real::MAX,
+            max_angular_velocity: 100.0,
             colliders: Vec::new(),
             activation: ActivationStatus::new_active(),
             joint_graph_index: InteractionGraph::<(), ()>::invalid_graph_index(),
@@ -127,6 +144,7 @@ impl RigidBody {
             body_status: BodyStatus::Dynamic,
             dominance_group: 0,
             user_data: 0,
+            ccd_thickness: Real::MAX,
         }
     }
 
@@ -176,6 +194,20 @@ impl RigidBody {
         }
     }
 
+    /// Enables of disable CCD (continuous collision-detection) for this rigid-body.
+    pub fn enable_ccd(&mut self, enabled: bool) {
+        self.flags.set(RigidBodyFlags::CCD_ENABLED, enabled)
+    }
+
+    /// Is CCD (continous collision-detection) enabled for this rigid-body?
+    pub fn is_ccd_enabled(&self) -> bool {
+        self.flags.contains(RigidBodyFlags::CCD_ENABLED)
+    }
+
+    pub(crate) fn should_resolve_ccd(&self, dt: Real) -> bool {
+        self.is_ccd_enabled() && self.is_dynamic() && self.linvel.norm() * dt > self.ccd_thickness
+    }
+
     /// Sets the rigid-body's mass properties.
     ///
     /// If `wake_up` is `true` then the rigid-body will be woken up if it was
@@ -228,8 +260,8 @@ impl RigidBody {
     /// If this rigid-body is kinematic this value is set by the `set_next_kinematic_position`
     /// method and is used for estimating the kinematic body velocity at the next timestep.
     /// For non-kinematic bodies, this value is currently unspecified.
-    pub fn predicted_position(&self) -> &Isometry<Real> {
+    pub fn next_position(&self) -> &Isometry<Real> {
-        &self.predicted_position
+        &self.next_position
     }
 
     /// The scale factor applied to the gravity affecting this rigid-body.
@@ -254,6 +286,8 @@ impl RigidBody {
             true,
         );
 
+        self.ccd_thickness = self.ccd_thickness.min(coll.shape().ccd_thickness());
+
         let mass_properties = coll
             .mass_properties()
             .transform_by(coll.position_wrt_parent());
@@ -265,8 +299,8 @@ impl RigidBody {
     pub(crate) fn update_colliders_positions(&mut self, colliders: &mut ColliderSet) {
         for handle in &self.colliders {
             let collider = &mut colliders[*handle];
+            collider.prev_position = self.position;
             collider.position = self.position * collider.delta;
-            collider.predicted_position = self.predicted_position * collider.delta;
         }
     }
 
@@ -331,18 +365,39 @@ impl RigidBody {
         !self.linvel.is_zero() || !self.angvel.is_zero()
     }
 
-    fn integrate_velocity(&self, dt: Real) -> Isometry<Real> {
+    pub(crate) fn integrate_velocity(&self, dt: Real) -> Isometry<Real> {
         let com = self.position * self.mass_properties.local_com;
         let shift = Translation::from(com.coords);
         shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse()
     }
 
-    pub(crate) fn integrate(&mut self, dt: Real) {
+    pub(crate) fn position_at_time(&self, dt: Real) -> Isometry<Real> {
-        // TODO: do we want to apply damping before or after the velocity integration?
+        self.integrate_velocity(dt) * self.position
-        self.linvel *= 1.0 / (1.0 + dt * self.linear_damping);
+    }
-        self.angvel *= 1.0 / (1.0 + dt * self.angular_damping);
 
-        self.position = self.integrate_velocity(dt) * self.position;
+    pub(crate) fn integrate_next_position(&mut self, dt: Real, apply_damping: bool) {
+        // TODO: do we want to apply damping before or after the velocity integration?
+        if apply_damping {
+            self.linvel *= 1.0 / (1.0 + dt * self.linear_damping);
+            self.angvel *= 1.0 / (1.0 + dt * self.angular_damping);
+
+            // self.linvel = self.linvel.cap_magnitude(self.max_linear_velocity);
+            // #[cfg(feature = "dim2")]
+            // {
+            //     self.angvel = na::clamp(
+            //         self.angvel,
+            //         -self.max_angular_velocity,
+            //         self.max_angular_velocity,
+            //     );
+            // }
+            // #[cfg(feature = "dim3")]
+            // {
+            //     self.angvel = self.angvel.cap_magnitude(self.max_angular_velocity);
+            // }
+        }
+
+        self.next_position = self.integrate_velocity(dt) * self.position;
+        let _ = self.next_position.rotation.renormalize();
     }
 
     /// The linear velocity of this rigid-body.
@@ -416,7 +471,8 @@ impl RigidBody {
     /// put to sleep because it did not move for a while.
     pub fn set_position(&mut self, pos: Isometry<Real>, wake_up: bool) {
         self.changes.insert(RigidBodyChanges::POSITION);
-        self.set_position_internal(pos);
+        self.position = pos;
+        self.next_position = pos;
 
         // TODO: Do we really need to check that the body isn't dynamic?
         if wake_up && self.is_dynamic() {
@@ -424,24 +480,19 @@ impl RigidBody {
         }
     }
 
-    pub(crate) fn set_position_internal(&mut self, pos: Isometry<Real>) {
+    pub(crate) fn set_next_position(&mut self, pos: Isometry<Real>) {
-        self.position = pos;
+        self.next_position = pos;
-
-        // TODO: update the predicted position for dynamic bodies too?
-        if self.is_static() || self.is_kinematic() {
-            self.predicted_position = pos;
-        }
     }
 
     /// If this rigid body is kinematic, sets its future position after the next timestep integration.
     pub fn set_next_kinematic_position(&mut self, pos: Isometry<Real>) {
         if self.is_kinematic() {
-            self.predicted_position = pos;
+            self.next_position = pos;
         }
     }
 
-    pub(crate) fn compute_velocity_from_predicted_position(&mut self, inv_dt: Real) {
+    pub(crate) fn compute_velocity_from_next_position(&mut self, inv_dt: Real) {
-        let dpos = self.predicted_position * self.position.inverse();
+        let dpos = self.next_position * self.position.inverse();
         #[cfg(feature = "dim2")]
         {
             self.angvel = dpos.rotation.angle() * inv_dt;
@@ -453,8 +504,8 @@ impl RigidBody {
         self.linvel = dpos.translation.vector * inv_dt;
     }
 
-    pub(crate) fn update_predicted_position(&mut self, dt: Real) {
+    pub(crate) fn update_next_position(&mut self, dt: Real) {
-        self.predicted_position = self.integrate_velocity(dt) * self.position;
+        self.next_position = self.integrate_velocity(dt) * self.position;
     }
 
     pub(crate) fn update_world_mass_properties(&mut self) {
@@ -666,6 +717,7 @@ pub struct RigidBodyBuilder {
     mass_properties: MassProperties,
     can_sleep: bool,
     sleeping: bool,
+    ccd_enabled: bool,
     dominance_group: i8,
     user_data: u128,
 }
@@ -685,6 +737,7 @@ impl RigidBodyBuilder {
             mass_properties: MassProperties::zero(),
             can_sleep: true,
             sleeping: false,
+            ccd_enabled: false,
             dominance_group: 0,
             user_data: 0,
         }
@@ -888,6 +941,12 @@ impl RigidBodyBuilder {
         self
     }
 
+    /// Enabled continuous collision-detection for this rigid-body.
+    pub fn ccd_enabled(mut self, enabled: bool) -> Self {
+        self.ccd_enabled = enabled;
+        self
+    }
+
     /// Sets whether or not the rigid-body is to be created asleep.
     pub fn sleeping(mut self, sleeping: bool) -> Self {
         self.sleeping = sleeping;
@@ -897,8 +956,8 @@ impl RigidBodyBuilder {
     /// Build a new rigid-body with the parameters configured with this builder.
     pub fn build(&self) -> RigidBody {
         let mut rb = RigidBody::new();
-        rb.predicted_position = self.position; // FIXME: compute the correct value?
+        rb.next_position = self.position; // FIXME: compute the correct value?
-        rb.set_position_internal(self.position);
+        rb.position = self.position;
         rb.linvel = self.linvel;
         rb.angvel = self.angvel;
         rb.body_status = self.body_status;
@@ -909,6 +968,7 @@ impl RigidBodyBuilder {
         rb.gravity_scale = self.gravity_scale;
         rb.flags = self.flags;
         rb.dominance_group = self.dominance_group;
+        rb.enable_ccd(self.ccd_enabled);
 
         if self.can_sleep && self.sleeping {
             rb.sleep();

src/dynamics/solver/island_solver.rs

@@ -59,7 +59,7 @@ impl IslandSolver {
 
             counters.solver.velocity_update_time.resume();
             bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
-                rb.integrate(params.dt)
+                rb.integrate_next_position(params.dt, true)
             });
             counters.solver.velocity_update_time.pause();
 
@@ -77,7 +77,7 @@ impl IslandSolver {
             bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
                 // Since we didn't run the velocity solver we need to integrate the accelerations here
                 rb.integrate_accelerations(params.dt);
-                rb.integrate(params.dt);
+                rb.integrate_next_position(params.dt, true);
             });
             counters.solver.velocity_update_time.pause();
         }

src/dynamics/solver/joint_constraint/ball_position_constraint.rs

@@ -114,7 +114,7 @@ impl BallPositionGroundConstraint {
             // are the local_anchors. The rb1 and rb2 have
             // already been flipped by the caller.
             Self {
-                anchor1: rb1.predicted_position * cparams.local_anchor2,
+                anchor1: rb1.next_position * cparams.local_anchor2,
                 im2: rb2.effective_inv_mass,
                 ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
                 local_anchor2: cparams.local_anchor1,
@@ -123,7 +123,7 @@ impl BallPositionGroundConstraint {
             }
         } else {
             Self {
-                anchor1: rb1.predicted_position * cparams.local_anchor1,
+                anchor1: rb1.next_position * cparams.local_anchor1,
                 im2: rb2.effective_inv_mass,
                 ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
                 local_anchor2: cparams.local_anchor2,

src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs

@@ -134,7 +134,7 @@ impl WBallPositionGroundConstraint {
         cparams: [&BallJoint; SIMD_WIDTH],
         flipped: [bool; SIMD_WIDTH],
     ) -> Self {
-        let position1 = Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]);
+        let position1 = Isometry::from(array![|ii| rbs1[ii].next_position; SIMD_WIDTH]);
         let anchor1 = position1
             * Point::from(array![|ii| if flipped[ii] {
                 cparams[ii].local_anchor2

src/dynamics/solver/joint_constraint/fixed_position_constraint.rs

@@ -100,10 +100,10 @@ impl FixedPositionGroundConstraint {
         let local_anchor2;
 
         if flipped {
-            anchor1 = rb1.predicted_position * cparams.local_anchor2;
+            anchor1 = rb1.next_position * cparams.local_anchor2;
             local_anchor2 = cparams.local_anchor1;
         } else {
-            anchor1 = rb1.predicted_position * cparams.local_anchor1;
+            anchor1 = rb1.next_position * cparams.local_anchor1;
             local_anchor2 = cparams.local_anchor2;
         };
 

src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs

@@ -119,14 +119,14 @@ impl PrismaticPositionGroundConstraint {
         let local_axis2;
 
         if flipped {
-            frame1 = rb1.predicted_position * cparams.local_frame2();
+            frame1 = rb1.next_position * cparams.local_frame2();
             local_frame2 = cparams.local_frame1();
-            axis1 = rb1.predicted_position * cparams.local_axis2;
+            axis1 = rb1.next_position * cparams.local_axis2;
             local_axis2 = cparams.local_axis1;
         } else {
-            frame1 = rb1.predicted_position * cparams.local_frame1();
+            frame1 = rb1.next_position * cparams.local_frame1();
             local_frame2 = cparams.local_frame2();
-            axis1 = rb1.predicted_position * cparams.local_axis1;
+            axis1 = rb1.next_position * cparams.local_axis1;
             local_axis2 = cparams.local_axis2;
         };
 

src/dynamics/solver/joint_constraint/revolute_position_constraint.rs

@@ -145,23 +145,23 @@ impl RevolutePositionGroundConstraint {
         let local_basis2;
 
         if flipped {
-            anchor1 = rb1.predicted_position * cparams.local_anchor2;
+            anchor1 = rb1.next_position * cparams.local_anchor2;
             local_anchor2 = cparams.local_anchor1;
-            axis1 = rb1.predicted_position * cparams.local_axis2;
+            axis1 = rb1.next_position * cparams.local_axis2;
             local_axis2 = cparams.local_axis1;
             basis1 = [
-                rb1.predicted_position * cparams.basis2[0],
+                rb1.next_position * cparams.basis2[0],
-                rb1.predicted_position * cparams.basis2[1],
+                rb1.next_position * cparams.basis2[1],
             ];
             local_basis2 = cparams.basis1;
         } else {
-            anchor1 = rb1.predicted_position * cparams.local_anchor1;
+            anchor1 = rb1.next_position * cparams.local_anchor1;
             local_anchor2 = cparams.local_anchor2;
-            axis1 = rb1.predicted_position * cparams.local_axis1;
+            axis1 = rb1.next_position * cparams.local_axis1;
             local_axis2 = cparams.local_axis2;
             basis1 = [
-                rb1.predicted_position * cparams.basis1[0],
+                rb1.next_position * cparams.basis1[0],
-                rb1.predicted_position * cparams.basis1[1],
+                rb1.next_position * cparams.basis1[1],
             ];
             local_basis2 = cparams.basis2;
         };

src/dynamics/solver/parallel_island_solver.rs

@@ -277,7 +277,7 @@ impl ParallelIslandSolver {
                         rb.linvel += dvel.linear;
                         rb.angvel += rb.effective_world_inv_inertia_sqrt.transform_vector(dvel.angular);
                         rb.integrate(params.dt);
-                        positions[rb.active_set_offset] = rb.position;
+                        positions[rb.active_set_offset] = rb.next_position;
                     }
                 }
 
@@ -298,7 +298,7 @@ impl ParallelIslandSolver {
                     let batch_size = thread.batch_size;
                     for handle in active_bodies[thread.position_writeback_index] {
                         let rb = &mut bodies[handle.0];
-                        rb.set_position_internal(positions[rb.active_set_offset]);
+                        rb.set_next_position(positions[rb.active_set_offset]);
                     }
                 }
             })

src/dynamics/solver/position_solver.rs

@@ -25,7 +25,7 @@ impl PositionSolver {
         self.positions.extend(
             bodies
                 .iter_active_island(island_id)
-                .map(|(_, b)| b.position),
+                .map(|(_, b)| b.next_position),
         );
 
         for _ in 0..params.max_position_iterations {
@@ -39,7 +39,7 @@ impl PositionSolver {
         }
 
         bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
-            rb.set_position_internal(self.positions[rb.active_set_offset])
+            rb.set_next_position(self.positions[rb.active_set_offset])
         });
     }
 }

src/geometry/collider.rs

@@ -2,7 +2,7 @@ use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle};
 use crate::geometry::{InteractionGroups, SAPProxyIndex, SharedShape, SolverFlags};
 use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector, DIM};
 use crate::parry::transformation::vhacd::VHACDParameters;
-use parry::bounding_volume::AABB;
+use parry::bounding_volume::{BoundingVolume, AABB};
 use parry::shape::Shape;
 
 bitflags::bitflags! {
@@ -62,7 +62,7 @@ pub struct Collider {
     pub(crate) parent: RigidBodyHandle,
     pub(crate) delta: Isometry<Real>,
     pub(crate) position: Isometry<Real>,
-    pub(crate) predicted_position: Isometry<Real>,
+    pub(crate) prev_position: Isometry<Real>,
     /// The friction coefficient of this collider.
     pub friction: Real,
     /// The restitution coefficient of this collider.
@@ -139,11 +139,12 @@ impl Collider {
         self.shape.compute_aabb(&self.position)
     }
 
-    // pub(crate) fn compute_aabb_with_prediction(&self) -> AABB {
+    /// Compute the axis-aligned bounding box of this collider.
-    //     let aabb1 = self.shape.compute_aabb(&self.position);
+    pub fn compute_swept_aabb(&self, next_position: &Isometry<Real>) -> AABB {
-    //     let aabb2 = self.shape.compute_aabb(&self.predicted_position);
+        let aabb1 = self.shape.compute_aabb(&self.position);
-    //     aabb1.merged(&aabb2)
+        let aabb2 = self.shape.compute_aabb(next_position);
-    // }
+        aabb1.merged(&aabb2)
+    }
 
     /// Compute the local-space mass properties of this collider.
     pub fn mass_properties(&self) -> MassProperties {
@@ -595,8 +596,8 @@ impl ColliderBuilder {
             flags,
             solver_flags,
             parent: RigidBodyHandle::invalid(),
+            prev_position: Isometry::identity(),
             position: Isometry::identity(),
-            predicted_position: Isometry::identity(),
             proxy_index: crate::INVALID_U32,
             collision_groups: self.collision_groups,
             solver_groups: self.solver_groups,

src/geometry/collider_set.rs

@@ -108,8 +108,8 @@ impl ColliderSet {
         let parent = bodies
             .get_mut(parent_handle)
             .expect("Parent rigid body not found.");
+        coll.prev_position = parent.position * coll.delta;
         coll.position = parent.position * coll.delta;
-        coll.predicted_position = parent.predicted_position * coll.delta;
         let handle = ColliderHandle(self.colliders.insert(coll));
         let coll = self.colliders.get(handle.0).unwrap();
         parent.add_collider(handle, &coll);

src/geometry/narrow_phase.rs

@@ -71,6 +71,14 @@ impl NarrowPhase {
         }
     }
 
+    /// The query dispatcher used by this narrow-phase to select the right collision-detection
+    /// algorithms depending of the shape types.
+    pub fn query_dispatcher(
+        &self,
+    ) -> &dyn PersistentQueryDispatcher<ContactManifoldData, ContactData> {
+        &*self.query_dispatcher
+    }
+
     /// The contact graph containing all contact pairs and their contact information.
     pub fn contact_graph(&self) -> &InteractionGraph<ColliderHandle, ContactPair> {
         &self.contact_graph

src/pipeline/collision_pipeline.rs

@@ -69,21 +69,18 @@ impl CollisionPipeline {
 
         // // Update kinematic bodies velocities.
         // bodies.foreach_active_kinematic_body_mut_internal(|_, body| {
-        //     body.compute_velocity_from_predicted_position(integration_parameters.inv_dt());
+        //     body.compute_velocity_from_next_position(integration_parameters.inv_dt());
         // });
 
         // Update colliders positions and kinematic bodies positions.
         bodies.foreach_active_body_mut_internal(|_, rb| {
-            if rb.is_kinematic() {
+            rb.position = rb.next_position;
-                rb.position = rb.predicted_position;
+            rb.update_colliders_positions(colliders);
-            } else {
-                rb.update_predicted_position(0.0);
-            }
 
             for handle in &rb.colliders {
                 let collider = &mut colliders[*handle];
+                collider.prev_position = collider.position;
                 collider.position = rb.position * collider.delta;
-                collider.predicted_position = rb.predicted_position * collider.delta;
             }
         });
 

src/pipeline/physics_pipeline.rs

@@ -3,7 +3,7 @@
 use crate::counters::Counters;
 #[cfg(not(feature = "parallel"))]
 use crate::dynamics::IslandSolver;
-use crate::dynamics::{IntegrationParameters, JointSet, RigidBodySet};
+use crate::dynamics::{CCDSolver, IntegrationParameters, JointSet, RigidBodySet};
 #[cfg(feature = "parallel")]
 use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
 use crate::geometry::{
@@ -68,6 +68,7 @@ impl PhysicsPipeline {
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
         joints: &mut JointSet,
+        ccd_solver: Option<&mut CCDSolver>,
         hooks: &dyn PhysicsHooks,
         events: &dyn EventHandler,
     ) {
@@ -81,7 +82,7 @@ impl PhysicsPipeline {
         // there to determine if this kinematic body should wake-up dynamic
         // bodies it is touching.
         bodies.foreach_active_kinematic_body_mut_internal(|_, body| {
-            body.compute_velocity_from_predicted_position(integration_parameters.inv_dt());
+            body.compute_velocity_from_next_position(integration_parameters.inv_dt());
         });
 
         self.counters.stages.collision_detection_time.start();
@@ -218,23 +219,33 @@ impl PhysicsPipeline {
             });
         }
 
-        // Update colliders positions and kinematic bodies positions.
+        // Handle CCD
-        // FIXME: do this in the solver?
+        if let Some(ccd_solver) = ccd_solver {
+            let impacts = ccd_solver.predict_next_impacts(
+                integration_parameters,
+                bodies,
+                colliders,
+                integration_parameters.dt,
+                events,
+            );
+            ccd_solver.clamp_motions(integration_parameters.dt, bodies, &impacts);
+        }
+
+        // Set the rigid-bodies and kinematic bodies to their final position.
         bodies.foreach_active_body_mut_internal(|_, rb| {
             if rb.is_kinematic() {
-                rb.position = rb.predicted_position;
                 rb.linvel = na::zero();
                 rb.angvel = na::zero();
-            } else {
-                rb.update_predicted_position(integration_parameters.dt);
             }
 
+            rb.position = rb.next_position;
             rb.update_colliders_positions(colliders);
         });
 
         self.counters.stages.solver_time.pause();
 
         bodies.modified_inactive_set.clear();
+
         self.counters.step_completed();
     }
 }

src/pipeline/query_pipeline.rs

@@ -1,10 +1,9 @@
 use crate::dynamics::RigidBodySet;
 use crate::geometry::{
     Collider, ColliderHandle, ColliderSet, InteractionGroups, PointProjection, Ray,
-    RayIntersection, SimdQuadTree,
+    RayIntersection, SimdQuadTree, AABB,
 };
 use crate::math::{Isometry, Point, Real, Vector};
-use crate::parry::motion::RigidMotion;
 use parry::query::details::{
     IntersectionCompositeShapeShapeBestFirstVisitor,
     NonlinearTOICompositeShapeShapeBestFirstVisitor, PointCompositeShapeProjBestFirstVisitor,
@@ -15,7 +14,7 @@ use parry::query::details::{
 use parry::query::visitors::{
     BoundingVolumeIntersectionsVisitor, PointIntersectionsVisitor, RayIntersectionsVisitor,
 };
-use parry::query::{DefaultQueryDispatcher, QueryDispatcher, TOI};
+use parry::query::{DefaultQueryDispatcher, NonlinearRigidMotion, QueryDispatcher, TOI};
 use parry::shape::{FeatureId, Shape, TypedSimdCompositeShape};
 use std::sync::Arc;
 
@@ -95,7 +94,7 @@ impl QueryPipeline {
     /// Initializes an empty query pipeline with a custom `QueryDispatcher`.
     ///
     /// Use this constructor in order to use a custom `QueryDispatcher` that is
-    /// awary of your own user-defined shapes.
+    /// aware of your own user-defined shapes.
     pub fn with_query_dispatcher<D>(d: D) -> Self
     where
         D: 'static + QueryDispatcher,
@@ -108,11 +107,26 @@ impl QueryPipeline {
         }
     }
 
+    /// The query dispatcher used by this query pipeline for running scene queries.
+    pub fn query_dispatcher(&self) -> &dyn QueryDispatcher {
+        &*self.query_dispatcher
+    }
+
     /// Update the acceleration structure on the query pipeline.
-    pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) {
+    pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet, use_swept_aabb: bool) {
         if !self.tree_built {
-            let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb()));
+            if !use_swept_aabb {
-            self.quadtree.clear_and_rebuild(data, self.dilation_factor);
+                let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb()));
+                self.quadtree.clear_and_rebuild(data, self.dilation_factor);
+            } else {
+                let data = colliders.iter().map(|(h, co)| {
+                    let next_position =
+                        bodies[co.parent()].next_position * co.position_wrt_parent();
+                    (h, co.compute_swept_aabb(&next_position))
+                });
+                self.quadtree.clear_and_rebuild(data, self.dilation_factor);
+            }
+
             // FIXME: uncomment this once we handle insertion/removals properly.
             // self.tree_built = true;
             return;
@@ -127,10 +141,22 @@ impl QueryPipeline {
             }
         }
 
-        self.quadtree.update(
+        if !use_swept_aabb {
-            |handle| colliders[*handle].compute_aabb(),
+            self.quadtree.update(
-            self.dilation_factor,
+                |handle| colliders[*handle].compute_aabb(),
-        );
+                self.dilation_factor,
+            );
+        } else {
+            self.quadtree.update(
+                |handle| {
+                    let co = &colliders[*handle];
+                    let next_position =
+                        bodies[co.parent()].next_position * co.position_wrt_parent();
+                    co.compute_swept_aabb(&next_position)
+                },
+                self.dilation_factor,
+            );
+        }
     }
 
     /// Find the closest intersection between a ray and a set of collider.
@@ -336,6 +362,16 @@ impl QueryPipeline {
             .map(|h| (h.1 .1 .0, h.1 .0, h.1 .1 .1))
     }
 
+    /// Finds all handles of all the colliders with an AABB intersecting the given AABB.
+    pub fn colliders_with_aabb_intersecting_aabb(
+        &self,
+        aabb: &AABB,
+        mut callback: impl FnMut(&ColliderHandle) -> bool,
+    ) {
+        let mut visitor = BoundingVolumeIntersectionsVisitor::new(aabb, &mut callback);
+        self.quadtree.traverse_depth_first(&mut visitor);
+    }
+
     /// Casts a shape at a constant linear velocity and retrieve the first collider it hits.
     ///
     /// This is similar to ray-casting except that we are casting a whole shape instead of
@@ -386,20 +422,24 @@ impl QueryPipeline {
     pub fn nonlinear_cast_shape(
         &self,
         colliders: &ColliderSet,
-        shape_motion: &dyn RigidMotion,
+        shape_motion: &NonlinearRigidMotion,
         shape: &dyn Shape,
-        max_toi: Real,
+        start_time: Real,
-        target_distance: Real,
+        end_time: Real,
+        stop_at_penetration: bool,
         groups: InteractionGroups,
     ) -> Option<(ColliderHandle, TOI)> {
         let pipeline_shape = self.as_composite_shape(colliders, groups);
+        let pipeline_motion = NonlinearRigidMotion::identity();
         let mut visitor = NonlinearTOICompositeShapeShapeBestFirstVisitor::new(
             &*self.query_dispatcher,
-            shape_motion,
+            &pipeline_motion,
             &pipeline_shape,
+            shape_motion,
             shape,
-            max_toi,
+            start_time,
-            target_distance,
+            end_time,
+            stop_at_penetration,
         );
         self.quadtree.traverse_best_first(&mut visitor).map(|h| h.1)
     }

src_testbed/box2d_backend.rs

@@ -37,7 +37,7 @@ impl Box2dWorld {
         joints: &JointSet,
     ) -> Self {
         let mut world = b2::World::new(&na_vec_to_b2_vec(gravity));
-        world.set_continuous_physics(false);
+        world.set_continuous_physics(bodies.iter().any(|b| b.1.is_ccd_enabled()));
 
         let mut res = Box2dWorld {
             world,
@@ -77,14 +77,11 @@ impl Box2dWorld {
                 angular_velocity: body.angvel(),
                 linear_damping,
                 angular_damping,
+                bullet: body.is_ccd_enabled(),
                 ..b2::BodyDef::new()
             };
             let b2_handle = self.world.create_body(&def);
             self.rapier2box2d.insert(handle, b2_handle);
-
-            // Collider.
-            let mut b2_body = self.world.body_mut(b2_handle);
-            b2_body.set_bullet(false /* collider.is_ccd_enabled() */);
         }
     }
 
@@ -163,7 +160,7 @@ impl Box2dWorld {
 
         fixture_def.restitution = collider.restitution;
         fixture_def.friction = collider.friction;
-        fixture_def.density = collider.density();
+        fixture_def.density = collider.density().unwrap_or(1.0);
         fixture_def.is_sensor = collider.is_sensor();
         fixture_def.filter = b2::Filter::new();
 
@@ -215,8 +212,6 @@ impl Box2dWorld {
     }
 
     pub fn step(&mut self, counters: &mut Counters, params: &IntegrationParameters) {
-        //        self.world.set_continuous_physics(world.integration_parameters.max_ccd_substeps != 0);
-
         counters.step_started();
         self.world.step(
             params.dt,

src_testbed/harness/mod.rs

@@ -4,7 +4,7 @@ use crate::{
 };
 use kiss3d::window::Window;
 use plugin::HarnessPlugin;
-use rapier::dynamics::{IntegrationParameters, JointSet, RigidBodySet};
+use rapier::dynamics::{CCDSolver, IntegrationParameters, JointSet, RigidBodySet};
 use rapier::geometry::{BroadPhase, ColliderSet, NarrowPhase};
 use rapier::math::Vector;
 use rapier::pipeline::{ChannelEventCollector, PhysicsHooks, PhysicsPipeline, QueryPipeline};
@@ -133,6 +133,7 @@ impl Harness {
         self.physics.broad_phase = BroadPhase::new();
         self.physics.narrow_phase = NarrowPhase::new();
         self.state.timestep_id = 0;
+        self.physics.ccd_solver = CCDSolver::new();
         self.physics.query_pipeline = QueryPipeline::new();
         self.physics.pipeline = PhysicsPipeline::new();
         self.physics.pipeline.counters.enable();
@@ -194,13 +195,14 @@ impl Harness {
             &mut self.physics.bodies,
             &mut self.physics.colliders,
             &mut self.physics.joints,
+            Some(&mut self.physics.ccd_solver),
             &*self.physics.hooks,
             &self.event_handler,
         );
 
         self.physics
             .query_pipeline
-            .update(&self.physics.bodies, &self.physics.colliders);
+            .update(&self.physics.bodies, &self.physics.colliders, false);
 
         for plugin in &mut self.plugins {
             plugin.step(&mut self.physics, &self.state)

src_testbed/physics/mod.rs

@@ -1,5 +1,5 @@
 use crossbeam::channel::Receiver;
-use rapier::dynamics::{IntegrationParameters, JointSet, RigidBodySet};
+use rapier::dynamics::{CCDSolver, IntegrationParameters, JointSet, RigidBodySet};
 use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, IntersectionEvent, NarrowPhase};
 use rapier::math::Vector;
 use rapier::pipeline::{PhysicsHooks, PhysicsPipeline, QueryPipeline};
@@ -73,6 +73,7 @@ pub struct PhysicsState {
     pub bodies: RigidBodySet,
     pub colliders: ColliderSet,
     pub joints: JointSet,
+    pub ccd_solver: CCDSolver,
     pub pipeline: PhysicsPipeline,
     pub query_pipeline: QueryPipeline,
     pub integration_parameters: IntegrationParameters,
@@ -88,6 +89,7 @@ impl PhysicsState {
             bodies: RigidBodySet::new(),
             colliders: ColliderSet::new(),
             joints: JointSet::new(),
+            ccd_solver: CCDSolver::new(),
             pipeline: PhysicsPipeline::new(),
             query_pipeline: QueryPipeline::new(),
             integration_parameters: IntegrationParameters::default(),

src_testbed/physx_backend.rs

@@ -13,8 +13,8 @@ use physx::prelude::*;
 use physx::scene::FrictionType;
 use physx::traits::Class;
 use physx_sys::{
-    PxBitAndByte, PxConvexFlags, PxConvexMeshGeometryFlags, PxHeightFieldSample,
+    FilterShaderCallbackInfo, PxBitAndByte, PxConvexFlags, PxConvexMeshGeometryFlags,
-    PxMeshGeometryFlags, PxMeshScale_new, PxRigidActor,
+    PxHeightFieldSample, PxMeshGeometryFlags, PxMeshScale_new, PxRigidActor,
 };
 use rapier::counters::Counters;
 use rapier::dynamics::{
@@ -160,7 +160,7 @@ impl PhysxWorld {
                 FrictionType::Patch
             };
 
-            let scene_desc = SceneDescriptor {
+            let mut scene_desc = SceneDescriptor {
                 gravity: gravity.into_physx(),
                 thread_count: num_threads as u32,
                 broad_phase_type: BroadPhaseType::AutomaticBoxPruning,
@@ -169,6 +169,14 @@ impl PhysxWorld {
                 ..SceneDescriptor::new(())
             };
 
+            let ccd_enabled = bodies.iter().any(|(_, rb)| rb.is_ccd_enabled());
+
+            if ccd_enabled {
+                scene_desc.simulation_filter_shader =
+                    FilterShaderDescriptor::CallDefaultFirst(ccd_filter_shader);
+                scene_desc.flags.insert(SceneFlag::EnableCcd);
+            }
+
             let mut scene: Owner<PxScene> = physics.create(scene_desc).unwrap();
             let mut rapier2dynamic = HashMap::new();
             let mut rapier2static = HashMap::new();
@@ -231,7 +239,7 @@ impl PhysxWorld {
                 }
             }
 
-            // Update mass properties.
+            // Update mass properties and CCD flags.
             for (rapier_handle, actor) in rapier2dynamic.iter_mut() {
                 let rb = &bodies[*rapier_handle];
                 let densities: Vec<_> = rb
@@ -248,6 +256,23 @@ impl PhysxWorld {
                         std::ptr::null(),
                         false,
                     );
+
+                    if rb.is_ccd_enabled() {
+                        physx_sys::PxRigidBody_setRigidBodyFlag_mut(
+                            std::mem::transmute(actor.as_mut()),
+                            RigidBodyFlag::EnableCCD as u32,
+                            true,
+                        );
+                        // physx_sys::PxRigidBody_setMinCCDAdvanceCoefficient_mut(
+                        //     std::mem::transmute(actor.as_mut()),
+                        //     0.0,
+                        // );
+                        // physx_sys::PxRigidBody_setRigidBodyFlag_mut(
+                        //     std::mem::transmute(actor.as_mut()),
+                        //     RigidBodyFlag::EnableCCDFriction as u32,
+                        //     true,
+                        // );
+                    }
                 }
             }
 
@@ -699,3 +724,8 @@ impl AdvanceCallback<PxArticulationLink, PxRigidDynamic> for OnAdvance {
     ) {
     }
 }
+
+unsafe extern "C" fn ccd_filter_shader(data: *mut FilterShaderCallbackInfo) -> u16 {
+    (*(*data).pairFlags).mBits |= physx_sys::PxPairFlag::eDETECT_CCD_CONTACT as u16;
+    0
+}

src_testbed/testbed.rs

@@ -769,11 +769,38 @@ impl Testbed {
     }
 
     #[cfg(feature = "dim3")]
-    fn handle_special_event(&mut self, window: &mut Window, _event: Event) {
+    fn handle_special_event(&mut self, window: &mut Window, event: Event) {
+        use rapier::dynamics::RigidBodyBuilder;
+        use rapier::geometry::ColliderBuilder;
+
         if window.is_conrod_ui_capturing_mouse() {
             return;
         }
 
+        match event.value {
+            WindowEvent::Key(Key::Space, Action::Release, _) => {
+                let cam_pos = self.graphics.camera().view_transform().inverse();
+                let forward = cam_pos * -Vector::z();
+                let vel = forward * 1000.0;
+
+                let bodies = &mut self.harness.physics.bodies;
+                let colliders = &mut self.harness.physics.colliders;
+
+                let collider = ColliderBuilder::cuboid(4.0, 2.0, 0.4).density(20.0).build();
+                // let collider = ColliderBuilder::ball(2.0).density(1.0).build();
+                let body = RigidBodyBuilder::new_dynamic()
+                    .position(cam_pos)
+                    .linvel(vel.x, vel.y, vel.z)
+                    .ccd_enabled(true)
+                    .build();
+
+                let body_handle = bodies.insert(body);
+                colliders.insert(collider, body_handle, bodies);
+                self.graphics.add(window, body_handle, bodies, colliders);
+            }
+            _ => {}
+        }
+
         /*
         match event.value {
             WindowEvent::MouseButton(MouseButton::Button1, Action::Press, modifier) => {
@@ -1454,6 +1481,20 @@ Hashes at frame: {}
 fn draw_contacts(window: &mut Window, nf: &NarrowPhase, colliders: &ColliderSet) {
     for pair in nf.contact_pairs() {
         for manifold in &pair.manifolds {
+            for contact in &manifold.data.solver_contacts {
+                let color = if contact.dist > 0.0 {
+                    Point3::new(0.0, 0.0, 1.0)
+                } else {
+                    Point3::new(1.0, 0.0, 0.0)
+                };
+
+                let p = contact.point;
+                let n = manifold.data.normal;
+
+                use crate::engine::GraphicsWindow;
+                window.draw_graphics_line(&p, &(p + n * 0.4), &Point3::new(0.5, 1.0, 0.5));
+            }
+            /*
             for pt in manifold.contacts() {
                 let color = if pt.dist > 0.0 {
                     Point3::new(0.0, 0.0, 1.0)
@@ -1474,6 +1515,7 @@ fn draw_contacts(window: &mut Window, nf: &NarrowPhase, colliders: &ColliderSet)
                 window.draw_graphics_line(&start, &(start + n * 0.4), &Point3::new(0.5, 1.0, 0.5));
                 window.draw_graphics_line(&start, &end, &color);
             }
+             */
         }
     }
 }