Merge pull request #157 from dimforge/ccd

Implement Continuous Collision Detection

7 files changed, 114 insertions, 67 deletions

diff --git a/src_testbed/box2d_backend.rs b/src_testbed/box2d_backend.rs
index 29fd4fa..df31c95 100644
--- a/src_testbed/box2d_backend.rs
+++ b/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,
diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs
index bcdbbca..876cb7e 100644
--- a/src_testbed/engine.rs
+++ b/src_testbed/engine.rs
@@ -60,7 +60,6 @@ pub struct GraphicsManager {
     b2wireframe: HashMap<RigidBodyHandle, bool>,
     ground_color: Point3<f32>,
     camera: Camera,
-    ground_handle: Option<RigidBodyHandle>,
 }
 
 impl GraphicsManager {
@@ -87,14 +86,9 @@ impl GraphicsManager {
             c2color: HashMap::new(),
             ground_color: Point3::new(0.5, 0.5, 0.5),
             b2wireframe: HashMap::new(),
-            ground_handle: None,
         }
     }
 
-    pub fn set_ground_handle(&mut self, handle: Option<RigidBodyHandle>) {
-        self.ground_handle = handle
-    }
-
     pub fn clear(&mut self, window: &mut Window) {
         for sns in self.b2sn.values_mut() {
             for sn in sns.iter_mut() {
@@ -630,19 +624,17 @@ impl GraphicsManager {
         // );
         for (_, ns) in self.b2sn.iter_mut() {
             for n in ns.iter_mut() {
-                /*
-                if let Some(co) = colliders.get(n.collider()) {
-                    let bo = &bodies[co.parent()];
-
-                    if bo.is_dynamic() {
-                        if bo.is_sleeping() {
-                            n.set_color(Point3::new(1.0, 0.0, 0.0));
-                        } else {
-                            n.set_color(Point3::new(0.0, 1.0, 0.0));
-                        }
-                    }
-                }
-                 */
+                // if let Some(co) = colliders.get(n.collider()) {
+                //     let bo = &_bodies[co.parent()];
+                //
+                //     if bo.is_dynamic() {
+                //         if bo.is_ccd_active() {
+                //             n.set_color(Point3::new(1.0, 0.0, 0.0));
+                //         } else {
+                //             n.set_color(Point3::new(0.0, 1.0, 0.0));
+                //         }
+                //     }
+                // }
 
                 n.update(colliders);
                 n.draw(window);
diff --git a/src_testbed/harness/mod.rs b/src_testbed/harness/mod.rs
index 5e75d85..5fcc45c 100644
--- a/src_testbed/harness/mod.rs
+++ b/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();
@@ -179,6 +180,7 @@ impl Harness {
                     &mut physics.bodies,
                     &mut physics.colliders,
                     &mut physics.joints,
+                    &mut physics.ccd_solver,
                     &*physics.hooks,
                     event_handler,
                 );
@@ -194,6 +196,7 @@ impl Harness {
             &mut self.physics.bodies,
             &mut self.physics.colliders,
             &mut self.physics.joints,
+            &mut self.physics.ccd_solver,
             &*self.physics.hooks,
             &self.event_handler,
         );
diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs
index 698e255..2657b19 100644
--- a/src_testbed/nphysics_backend.rs
+++ b/src_testbed/nphysics_backend.rs
@@ -228,7 +228,11 @@ fn nphysics_collider_from_rapier_collider(
         }
     };
 
-    let density = if is_dynamic { collider.density() } else { 0.0 };
+    let density = if is_dynamic {
+        collider.density().unwrap_or(0.0)
+    } else {
+        0.0
+    };
 
     Some(
         ColliderDesc::new(shape)
diff --git a/src_testbed/physics/mod.rs b/src_testbed/physics/mod.rs
index 0987e32..b89f9c8 100644
--- a/src_testbed/physics/mod.rs
+++ b/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(),
diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs
index 02b57c3..8a3b155 100644
--- a/src_testbed/physx_backend.rs
+++ b/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,
-    PxMeshGeometryFlags, PxMeshScale_new, PxRigidActor,
+    FilterShaderCallbackInfo, PxBitAndByte, PxConvexFlags, PxConvexMeshGeometryFlags,
+    PxHeightFieldSample, PxMeshGeometryFlags, PxMeshScale_new, PxRigidActor,
 };
 use rapier::counters::Counters;
 use rapier::dynamics::{
@@ -160,15 +160,24 @@ 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,
                 solver_type: SolverType::PGS,
                 friction_type,
+                ccd_max_passes: integration_parameters.max_ccd_substeps as u32,
                 ..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,13 +240,13 @@ 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
                     .colliders()
                     .iter()
-                    .map(|h| colliders[*h].density())
+                    .map(|h| colliders[*h].density().unwrap_or(0.0))
                     .collect();
 
                 unsafe {
@@ -248,6 +257,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 +725,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
+}
diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs
index bc5cd6c..a2d20d0 100644
--- a/src_testbed/testbed.rs
+++ b/src_testbed/testbed.rs
@@ -24,7 +24,7 @@ use rapier::dynamics::{
 use rapier::geometry::{ColliderHandle, ColliderSet, NarrowPhase};
 #[cfg(feature = "dim3")]
 use rapier::geometry::{InteractionGroups, Ray};
-use rapier::math::{Isometry, Vector};
+use rapier::math::Vector;
 use rapier::pipeline::PhysicsHooks;
 
 #[cfg(all(feature = "dim2", feature = "other-backends"))]
@@ -125,7 +125,6 @@ pub struct Testbed {
     nsteps: usize,
     camera_locked: bool, // Used so that the camera can remain the same before and after we change backend or press the restart button.
     plugins: Vec<Box<dyn TestbedPlugin>>,
-    hide_counters: bool,
     //    persistant_contacts: HashMap<ContactId, bool>,
     font: Rc<Font>,
     cursor_pos: Point2<f32>,
@@ -185,7 +184,6 @@ impl Testbed {
             graphics,
             nsteps: 1,
             camera_locked: false,
-            hide_counters: true,
             //            persistant_contacts: HashMap::new(),
             font: Font::default(),
             cursor_pos: Point2::new(0.0f32, 0.0),
@@ -225,14 +223,6 @@ impl Testbed {
         self.state.can_grab_behind_ground = allow;
     }
 
-    pub fn hide_performance_counters(&mut self) {
-        self.hide_counters = true;
-    }
-
-    pub fn show_performance_counters(&mut self) {
-        self.hide_counters = false;
-    }
-
     pub fn integration_parameters_mut(&mut self) -> &mut IntegrationParameters {
         &mut self.harness.physics.integration_parameters
     }
@@ -769,11 +759,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) => {
@@ -1198,18 +1215,18 @@ impl State for Testbed {
                 }
             }
 
-            if self
-                .state
-                .prev_flags
-                .contains(TestbedStateFlags::SUB_STEPPING)
-                != self.state.flags.contains(TestbedStateFlags::SUB_STEPPING)
-            {
-                self.harness
-                    .physics
-                    .integration_parameters
-                    .return_after_ccd_substep =
-                    self.state.flags.contains(TestbedStateFlags::SUB_STEPPING);
-            }
+            // if self
+            //     .state
+            //     .prev_flags
+            //     .contains(TestbedStateFlags::SUB_STEPPING)
+            //     != self.state.flags.contains(TestbedStateFlags::SUB_STEPPING)
+            // {
+            //     self.harness
+            //         .physics
+            //         .integration_parameters
+            //         .return_after_ccd_substep =
+            //         self.state.flags.contains(TestbedStateFlags::SUB_STEPPING);
+            // }
 
             if self.state.prev_flags.contains(TestbedStateFlags::SHAPES)
                 != self.state.flags.contains(TestbedStateFlags::SHAPES)
@@ -1315,14 +1332,6 @@ impl State for Testbed {
                 for plugin in &mut self.plugins {
                     plugin.run_callbacks(window, &mut self.harness.physics, &self.harness.state);
                 }
-
-                //                if true {
-                //                    // !self.hide_counters {
-                //                    #[cfg(not(feature = "log"))]
-                //                    println!("{}", self.world.counters);
-                //                    #[cfg(feature = "log")]
-                //                    debug!("{}", self.world.counters);
-                //                }
             }
         }
 
@@ -1452,8 +1461,19 @@ Hashes at frame: {}
 }
 
 fn draw_contacts(window: &mut Window, nf: &NarrowPhase, colliders: &ColliderSet) {
+    use rapier::math::Isometry;
+
     for pair in nf.contact_pairs() {
         for manifold in &pair.manifolds {
+            /*
+            for contact in &manifold.data.solver_contacts {
+                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)