Merge pull request #6 from dimforge/collider_removal

Add collider removal + fix rigid-bodies with multiple colliders

3 files changed, 56 insertions, 11 deletions

diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs
index 43f3bf9..c9ff2d1 100644
--- a/src_testbed/nphysics_backend.rs
+++ b/src_testbed/nphysics_backend.rs
@@ -13,7 +13,7 @@ use rapier::dynamics::{
     IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet,
 };
 use rapier::geometry::{Collider, ColliderSet, Shape};
-use rapier::math::{Isometry, Vector};
+use rapier::math::Vector;
 use std::collections::HashMap;
 #[cfg(feature = "dim3")]
 use {ncollide::shape::TriMesh, nphysics::joint::BallConstraint};
@@ -165,7 +165,7 @@ impl NPhysicsWorld {
 
             for coll_handle in rb.colliders() {
                 let collider = &mut colliders[*coll_handle];
-                collider.set_position_debug(pos * collider.delta());
+                collider.set_position_debug(pos * collider.position_wrt_parent());
             }
         }
     }
@@ -176,7 +176,7 @@ fn nphysics_collider_from_rapier_collider(
     is_dynamic: bool,
 ) -> Option<ColliderDesc<f32>> {
     let margin = ColliderDesc::<f32>::default_margin();
-    let mut pos = Isometry::identity();
+    let mut pos = *collider.position_wrt_parent();
 
     let shape = match collider.shape() {
         Shape::Cuboid(cuboid) => {
@@ -184,7 +184,7 @@ fn nphysics_collider_from_rapier_collider(
         }
         Shape::Ball(ball) => ShapeHandle::new(Ball::new(ball.radius - margin)),
         Shape::Capsule(capsule) => {
-            pos = capsule.transform_wrt_y();
+            pos *= capsule.transform_wrt_y();
             ShapeHandle::new(Capsule::new(capsule.half_height(), capsule.radius))
         }
         Shape::HeightField(heightfield) => ShapeHandle::new(heightfield.clone()),
diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs
index 9ae7bb8..b06fd7e 100644
--- a/src_testbed/physx_backend.rs
+++ b/src_testbed/physx_backend.rs
@@ -225,6 +225,28 @@ impl PhysxWorld {
             }
         }
 
+        // Update mass properties.
+        for (rapier_handle, physx_handle) in rapier2physx.iter() {
+            let rb = &bodies[*rapier_handle];
+            if let Some(rp) = scene.get_dynamic_mut(*physx_handle) {
+                let densities: Vec<_> = rb
+                    .colliders()
+                    .iter()
+                    .map(|h| colliders[*h].density())
+                    .collect();
+
+                unsafe {
+                    physx_sys::PxRigidBodyExt_updateMassAndInertia_mut(
+                        rp.as_ptr_mut().ptr as *mut physx_sys::PxRigidBody,
+                        densities.as_ptr(),
+                        densities.len() as u32,
+                        std::ptr::null(),
+                        false,
+                    );
+                }
+            }
+        }
+
         let mut res = Self {
             physics,
             cooking,
@@ -390,7 +412,7 @@ impl PhysxWorld {
 
             for coll_handle in rb.colliders() {
                 let collider = &mut colliders[*coll_handle];
-                collider.set_position_debug(iso * collider.delta());
+                collider.set_position_debug(iso * collider.position_wrt_parent());
             }
         }
     }
@@ -399,7 +421,7 @@ impl PhysxWorld {
 fn physx_collider_from_rapier_collider(
     collider: &Collider,
 ) -> Option<(ColliderDesc, Isometry3<f32>)> {
-    let mut local_pose = Isometry3::identity();
+    let mut local_pose = *collider.position_wrt_parent();
     let desc = match collider.shape() {
         Shape::Cuboid(cuboid) => ColliderDesc::Box(
             cuboid.half_extents.x,
@@ -416,7 +438,7 @@ fn physx_collider_from_rapier_collider(
             }
 
             let rot = UnitQuaternion::rotation_between(&Vector3::x(), &dir);
-            local_pose =
+            local_pose *=
                 Translation3::from(center.coords) * rot.unwrap_or(UnitQuaternion::identity());
             ColliderDesc::Capsule(capsule.radius, capsule.height())
         }
diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs
index 868bd91..450170e 100644
--- a/src_testbed/testbed.rs
+++ b/src_testbed/testbed.rs
@@ -791,6 +791,29 @@ impl Testbed {
                 .state
                 .action_flags
                 .set(TestbedActionFlags::EXAMPLE_CHANGED, true),
+            WindowEvent::Key(Key::C, Action::Release, _) => {
+                // Delete 1 collider of 10% of the remaining dynamic bodies.
+                let mut colliders: Vec<_> = self
+                    .physics
+                    .bodies
+                    .iter()
+                    .filter(|e| e.1.is_dynamic())
+                    .filter(|e| !e.1.colliders().is_empty())
+                    .map(|e| e.1.colliders().to_vec())
+                    .collect();
+                colliders.sort_by_key(|co| -(co.len() as isize));
+
+                let num_to_delete = (colliders.len() / 10).max(1);
+                for to_delete in &colliders[..num_to_delete] {
+                    self.physics.pipeline.remove_collider(
+                        to_delete[0],
+                        &mut self.physics.broad_phase,
+                        &mut self.physics.narrow_phase,
+                        &mut self.physics.bodies,
+                        &mut self.physics.colliders,
+                    );
+                }
+            }
             WindowEvent::Key(Key::D, Action::Release, _) => {
                 // Delete 10% of the remaining dynamic bodies.
                 let dynamic_bodies: Vec<_> = self
@@ -1539,7 +1562,7 @@ impl State for Testbed {
         }
 
         if self.state.flags.contains(TestbedStateFlags::CONTACT_POINTS) {
-            draw_contacts(window, &self.physics.narrow_phase, &self.physics.bodies);
+            draw_contacts(window, &self.physics.narrow_phase, &self.physics.colliders);
         }
 
         if self.state.running == RunMode::Step {
@@ -1634,7 +1657,7 @@ Hashes at frame: {}
     }
 }
 
-fn draw_contacts(window: &mut Window, nf: &NarrowPhase, bodies: &RigidBodySet) {
+fn draw_contacts(window: &mut Window, nf: &NarrowPhase, colliders: &ColliderSet) {
     for (_, _, pair) in nf.contact_graph().interaction_pairs() {
         for manifold in &pair.manifolds {
             for pt in manifold.all_contacts() {
@@ -1643,8 +1666,8 @@ fn draw_contacts(window: &mut Window, nf: &NarrowPhase, bodies: &RigidBodySet) {
                 } else {
                     Point3::new(1.0, 0.0, 0.0)
                 };
-                let pos1 = bodies[manifold.body_pair.body1].position;
-                let pos2 = bodies[manifold.body_pair.body2].position;
+                let pos1 = colliders[manifold.pair.collider1].position();
+                let pos2 = colliders[manifold.pair.collider2].position();
                 let start = pos1 * pt.local_p1;
                 let end = pos2 * pt.local_p2;
                 let n = pos1 * manifold.local_n1;