Testbed physx backend: re-add joints.

4 files changed, 90 insertions, 217 deletions

diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs
index 2c8e5c6..d9e6aff 100644
--- a/examples3d/all_examples3.rs
+++ b/examples3d/all_examples3.rs
@@ -31,7 +31,6 @@ mod platform3;
 mod primitives3;
 mod restitution3;
 mod sensor3;
-mod stacks3;
 mod trimesh3;
 
 fn demo_name_from_command_line() -> Option<String> {
@@ -82,7 +81,6 @@ pub fn main() {
         ("Locked rotations", locked_rotations3::init_world),
         ("Platform", platform3::init_world),
         ("Restitution", restitution3::init_world),
-        ("Stacks", stacks3::init_world),
         ("Sensor", sensor3::init_world),
         ("TriMesh", trimesh3::init_world),
         ("Keva tower", keva3::init_world),
diff --git a/examples3d/stacks3.rs b/examples3d/stacks3.rs
deleted file mode 100644
index 55d58bb..0000000
--- a/examples3d/stacks3.rs
+++ /dev/null
@@ -1,171 +0,0 @@
-use na::{Point3, Translation3, UnitQuaternion, Vector3};
-use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
-use rapier3d::geometry::{ColliderBuilder, ColliderSet};
-use rapier_testbed3d::Testbed;
-
-fn create_tower_circle(
-    bodies: &mut RigidBodySet,
-    colliders: &mut ColliderSet,
-    offset: Vector3<f32>,
-    stack_height: usize,
-    nsubdivs: usize,
-    half_extents: Vector3<f32>,
-) {
-    let ang_step = std::f32::consts::PI * 2.0 / nsubdivs as f32;
-    let radius = 1.3 * nsubdivs as f32 * half_extents.x / std::f32::consts::PI;
-
-    let shift = half_extents * 2.0;
-    for i in 0usize..stack_height {
-        for j in 0..nsubdivs {
-            let fj = j as f32;
-            let fi = i as f32;
-            let y = fi * shift.y;
-            let pos = Translation3::from(offset)
-                * UnitQuaternion::new(Vector3::y() * (fi / 2.0 + fj) * ang_step)
-                * Translation3::new(0.0, y, radius);
-
-            // Build the rigid body.
-            let rigid_body = RigidBodyBuilder::new_dynamic().position(pos).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);
-        }
-    }
-}
-
-fn create_wall(
-    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);
-        }
-    }
-}
-
-fn create_pyramid(
-    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 {
-            for k in i..stack_height {
-                let fi = i as f32;
-                let fj = j as f32;
-                let fk = k as f32;
-                let x = (fi * shift.x / 2.0) + (fk - fi) * shift.x + offset.x
-                    - stack_height as f32 * half_extents.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);
-            }
-        }
-    }
-}
-
-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 = 200.0;
-    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 cube_size = 1.0;
-    let hext = Vector3::repeat(cube_size);
-    let bottomy = cube_size * 50.0;
-
-    create_pyramid(
-        &mut bodies,
-        &mut colliders,
-        Vector3::new(-20.0, bottomy, 0.0),
-        12,
-        hext,
-    );
-    create_wall(
-        &mut bodies,
-        &mut colliders,
-        Vector3::new(-2.0, bottomy, 0.0),
-        12,
-        hext,
-    );
-    create_wall(
-        &mut bodies,
-        &mut colliders,
-        Vector3::new(4.0, bottomy, 0.0),
-        12,
-        hext,
-    );
-    create_wall(
-        &mut bodies,
-        &mut colliders,
-        Vector3::new(10.0, bottomy, 0.0),
-        12,
-        hext,
-    );
-    create_tower_circle(
-        &mut bodies,
-        &mut colliders,
-        Vector3::new(25.0, bottomy, 0.0),
-        8,
-        24,
-        hext,
-    );
-
-    /*
-     * 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()
-}
diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs
index eac7323..53f6fe7 100644
--- a/src_testbed/physx_backend.rs
+++ b/src_testbed/physx_backend.rs
@@ -8,7 +8,9 @@ use physx::cooking::PxCooking;
 use physx::foundation::DefaultAllocator;
 use physx::prelude::*;
 use physx::scene::FrictionType;
+use physx::traits::Class;
 use physx::triangle_mesh::TriangleMesh;
+use physx_sys::{PxActor, PxRigidActor};
 use rapier::counters::Counters;
 use rapier::dynamics::{
     IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet,
@@ -166,6 +168,11 @@ impl PhysxWorld {
             let mut rapier2dynamic = HashMap::new();
             let mut rapier2static = HashMap::new();
 
+            /*
+             *
+             * Rigid bodies
+             *
+             */
             for (rapier_handle, rb) in bodies.iter() {
                 use physx::rigid_dynamic::RigidDynamic;
                 use physx::rigid_static::RigidStatic;
@@ -185,6 +192,11 @@ impl PhysxWorld {
                 }
             }
 
+            /*
+             *
+             * Colliders
+             *
+             */
             for (_, collider) in colliders.iter() {
                 if let Some((mut px_shape, px_material, collider_pos)) =
                     physx_collider_from_rapier_collider(&mut *physics, &collider)
@@ -198,7 +210,11 @@ impl PhysxWorld {
                         let actor = rapier2dynamic.get_mut(&collider.parent()).unwrap();
                         actor.attach_shape(&mut px_shape);
                     }
-                    // physx_sys::PxShape_setLocalPose_mut(shape, &pose);
+
+                    unsafe {
+                        let pose = collider_pos.into_physx();
+                        physx_sys::PxShape_setLocalPose_mut(px_shape.as_mut_ptr(), &pose.into());
+                    }
 
                     shapes.push(px_shape);
                     materials.push(px_material);
@@ -226,9 +242,16 @@ impl PhysxWorld {
             }
 
             /*
-               res.setup_joints(joints);
-               res
-            */
+             *
+             * Joints
+             *
+             */
+            Self::setup_joints(
+                &mut physics,
+                joints,
+                &mut rapier2static,
+                &mut rapier2dynamic,
+            );
 
             for (_, actor) in rapier2static {
                 scene.add_static_actor(actor);
@@ -246,27 +269,43 @@ impl PhysxWorld {
         })
     }
 
-    fn setup_joints(&mut self, joints: &JointSet) {
-        /*
+    fn setup_joints(
+        physics: &mut PxPhysicsFoundation,
+        joints: &JointSet,
+        rapier2static: &mut HashMap<RigidBodyHandle, Owner<PxRigidStatic>>,
+        rapier2dynamic: &mut HashMap<RigidBodyHandle, Owner<PxRigidDynamic>>,
+    ) {
         unsafe {
             for joint in joints.iter() {
-                let actor1 = self.rapier2physx[&joint.1.body1];
-                let actor2 = self.rapier2physx[&joint.1.body2];
+                let actor1 = rapier2static
+                    .get_mut(&joint.1.body1)
+                    .map(|act| &mut **act as *mut PxRigidStatic as *mut PxRigidActor)
+                    .or(rapier2dynamic
+                        .get_mut(&joint.1.body1)
+                        .map(|act| &mut **act as *mut PxRigidDynamic as *mut PxRigidActor))
+                    .unwrap();
+                let actor2 = rapier2static
+                    .get_mut(&joint.1.body2)
+                    .map(|act| &mut **act as *mut PxRigidStatic as *mut PxRigidActor)
+                    .or(rapier2dynamic
+                        .get_mut(&joint.1.body2)
+                        .map(|act| &mut **act as *mut PxRigidDynamic as *mut PxRigidActor))
+                    .unwrap();
 
                 match &joint.1.params {
                     JointParams::BallJoint(params) => {
-                        let frame1 = physx::transform::gl_to_px_tf(
-                            Isometry3::new(params.local_anchor1.coords, na::zero()).into_glam(),
-                        );
-                        let frame2 = physx::transform::gl_to_px_tf(
-                            Isometry3::new(params.local_anchor2.coords, na::zero()).into_glam(),
-                        );
+                        let frame1 = Isometry3::new(params.local_anchor1.coords, na::zero())
+                            .into_physx()
+                            .into();
+                        let frame2 = Isometry3::new(params.local_anchor2.coords, na::zero())
+                            .into_physx()
+                            .into();
 
                         physx_sys::phys_PxSphericalJointCreate(
-                            self.physics.get_raw_mut(),
-                            actor1.0 as *mut _,
+                            physics.as_mut_ptr(),
+                            actor1,
                             &frame1 as *const _,
-                            actor2.0 as *mut _,
+                            actor2,
                             &frame2 as *const _,
                         );
                     }
@@ -292,18 +331,18 @@ impl PhysxWorld {
                         let axisangle1 = rotmat1.scaled_axis();
                         let axisangle2 = rotmat2.scaled_axis();
 
-                        let frame1 = physx::transform::gl_to_px_tf(
-                            Isometry3::new(params.local_anchor1.coords, axisangle1).into_glam(),
-                        );
-                        let frame2 = physx::transform::gl_to_px_tf(
-                            Isometry3::new(params.local_anchor2.coords, axisangle2).into_glam(),
-                        );
+                        let frame1 = Isometry3::new(params.local_anchor1.coords, axisangle1)
+                            .into_physx()
+                            .into();
+                        let frame2 = Isometry3::new(params.local_anchor2.coords, axisangle2)
+                            .into_physx()
+                            .into();
 
                         physx_sys::phys_PxRevoluteJointCreate(
-                            self.physics.get_raw_mut(),
-                            actor1.0 as *mut _,
+                            physics.as_mut_ptr(),
+                            actor1,
                             &frame1 as *const _,
-                            actor2.0 as *mut _,
+                            actor2,
                             &frame2 as *const _,
                         );
                     }
@@ -331,18 +370,18 @@ impl PhysxWorld {
                         let axisangle1 = rotmat1.scaled_axis();
                         let axisangle2 = rotmat2.scaled_axis();
 
-                        let frame1 = physx::transform::gl_to_px_tf(
-                            Isometry3::new(params.local_anchor1.coords, axisangle1).into_glam(),
-                        );
-                        let frame2 = physx::transform::gl_to_px_tf(
-                            Isometry3::new(params.local_anchor2.coords, axisangle2).into_glam(),
-                        );
+                        let frame1 = Isometry3::new(params.local_anchor1.coords, axisangle1)
+                            .into_physx()
+                            .into();
+                        let frame2 = Isometry3::new(params.local_anchor2.coords, axisangle2)
+                            .into_physx()
+                            .into();
 
                         let joint = physx_sys::phys_PxPrismaticJointCreate(
-                            self.physics.get_raw_mut(),
-                            actor1.0 as *mut _,
+                            physics.as_mut_ptr(),
+                            actor1,
                             &frame1 as *const _,
-                            actor2.0 as *mut _,
+                            actor2,
                             &frame2 as *const _,
                         );
 
@@ -365,23 +404,20 @@ impl PhysxWorld {
                         }
                     }
                     JointParams::FixedJoint(params) => {
-                        let frame1 =
-                            physx::transform::gl_to_px_tf(params.local_anchor1.into_glam());
-                        let frame2 =
-                            physx::transform::gl_to_px_tf(params.local_anchor2.into_glam());
+                        let frame1 = params.local_anchor1.into_physx().into();
+                        let frame2 = params.local_anchor2.into_physx().into();
 
                         physx_sys::phys_PxFixedJointCreate(
-                            self.physics.get_raw_mut(),
-                            actor1.0 as *mut _,
+                            physics.as_mut_ptr(),
+                            actor1,
                             &frame1 as *const _,
-                            actor2.0 as *mut _,
+                            actor2,
                             &frame2 as *const _,
                         );
                     }
                 }
             }
         }
-         */
     }
 
     pub fn step(&mut self, counters: &mut Counters, params: &IntegrationParameters) {
@@ -457,7 +493,9 @@ fn physx_collider_from_rapier_collider(
         }
 
         let rot = UnitQuaternion::rotation_between(&Vector3::x(), &dir);
-        local_pose *= Translation3::from(center.coords) * rot.unwrap_or(UnitQuaternion::identity());
+        local_pose = local_pose
+            * Translation3::from(center.coords)
+            * rot.unwrap_or(UnitQuaternion::identity());
         let geometry = PxCapsuleGeometry::new(capsule.radius, capsule.half_height());
         physics.create_shape(&geometry, materials, true, shape_flags, ())
     } else if let Some(trimesh) = shape.as_trimesh() {
diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs
index 10b3a0a..bef9b8f 100644
--- a/src_testbed/testbed.rs
+++ b/src_testbed/testbed.rs
@@ -1126,6 +1126,14 @@ impl State for Testbed {
 
                 if self.state.selected_example != prev_example {
                     self.physics.integration_parameters = IntegrationParameters::default();
+                    if self.state.selected_backend == PHYSX_BACKEND_PATCH_FRICTION
+                        || self.state.selected_backend == PHYSX_BACKEND_TWO_FRICTION_DIR
+                    {
+                        std::mem::swap(
+                            &mut self.physics.integration_parameters.max_velocity_iterations,
+                            &mut self.physics.integration_parameters.max_position_iterations,
+                        )
+                    }
                 }
 
                 self.builders[self.state.selected_example].1(self);