Add compound demo.

4 files changed, 16 insertions, 8 deletions

diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs
index 04b9b36..2b89efa 100644
--- a/examples3d/all_examples3.rs
+++ b/examples3d/all_examples3.rs
@@ -14,6 +14,7 @@ mod add_remove3;
 mod balls3;
 mod boxes3;
 mod capsules3;
+mod compound3;
 mod debug_boxes3;
 mod debug_triangle3;
 mod domino3;
@@ -71,6 +72,7 @@ pub fn main() {
         ("Balls", balls3::init_world),
         ("Boxes", boxes3::init_world),
         ("Capsules", capsules3::init_world),
+        ("Compound", compound3::init_world),
         ("Domino", domino3::init_world),
         ("Heightfield", heightfield3::init_world),
         ("Joints", joints3::init_world),
diff --git a/examples3d/compound3.rs b/examples3d/compound3.rs
index eb8a472..1c594c6 100644
--- a/examples3d/compound3.rs
+++ b/examples3d/compound3.rs
@@ -1,4 +1,4 @@
-use na::Point3;
+use na::{Point3, Vector3};
 use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
 use rapier3d::geometry::{ColliderBuilder, ColliderSet};
 use rapier_testbed3d::Testbed;
@@ -41,14 +41,20 @@ pub fn init_world(testbed: &mut Testbed) {
         for i in 0..num {
             for k in 0usize..num {
                 let x = i as f32 * shift - centerx + offset;
-                let y = j as f32 * shift + centery + 3.0;
+                let y = j as f32 * (shift * 2.0) + centery + 3.0;
                 let z = k as f32 * shift - centerz + offset;
 
                 // 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(rad, rad, rad).density(1.0).build();
-                colliders.insert(collider, handle, &mut bodies);
+                let collider1 = ColliderBuilder::cuboid(rad, rad, rad).density(1.0).build();
+                let collider2 = ColliderBuilder::cuboid(rad, rad, rad)
+                    .translation(0.0, -rad * 3.0, 0.0)
+                    .rotation(Vector3::x() * 0.5)
+                    .density(1.0)
+                    .build();
+                colliders.insert(collider1, handle, &mut bodies);
+                colliders.insert(collider2, handle, &mut bodies);
             }
         }
 
diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs
index 43f3bf9..a2a9bfc 100644
--- a/src_testbed/nphysics_backend.rs
+++ b/src_testbed/nphysics_backend.rs
@@ -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..045e697 100644
--- a/src_testbed/physx_backend.rs
+++ b/src_testbed/physx_backend.rs
@@ -399,7 +399,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 +416,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())
         }