From fc0b3bf39484029d956055943b38bb55ab2c5791 Mon Sep 17 00:00:00 2001
From: Sébastien Crozet <developer@crozet.re>
Date: Tue, 1 Sep 2020 17:35:32 +0200
Subject: Mass properties: add a max number of iterations for the local-frame
 rotation computation.

---
 examples3d/compound3.rs         | 26 ++++++++++++++------------
 src/dynamics/mass_properties.rs |  6 ++++--
 src_testbed/physx_backend.rs    |  4 ++--
 3 files changed, 20 insertions(+), 16 deletions(-)

diff --git a/examples3d/compound3.rs b/examples3d/compound3.rs
index 1c594c6..a8a9939 100644
--- a/examples3d/compound3.rs
+++ b/examples3d/compound3.rs
@@ -1,4 +1,4 @@
-use na::{Point3, Vector3};
+use na::Point3;
 use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
 use rapier3d::geometry::{ColliderBuilder, ColliderSet};
 use rapier_testbed3d::Testbed;
@@ -28,33 +28,35 @@ pub fn init_world(testbed: &mut Testbed) {
      * Create the cubes
      */
     let num = 8;
-    let rad = 1.0;
+    let rad = 0.2;
 
-    let shift = rad * 2.0 + rad;
+    let shift = rad * 4.0 + rad;
     let centerx = shift * (num / 2) as f32;
     let centery = shift / 2.0;
     let centerz = shift * (num / 2) as f32;
 
     let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5;
 
-    for j in 0usize..47 {
+    for j in 0usize..25 {
         for i in 0..num {
             for k in 0usize..num {
-                let x = i as f32 * shift - centerx + offset;
-                let y = j as f32 * (shift * 2.0) + centery + 3.0;
-                let z = k as f32 * shift - centerz + offset;
+                let x = i as f32 * shift * 5.0 - centerx + offset;
+                let y = j as f32 * (shift * 5.0) + centery + 3.0;
+                let z = k as f32 * shift * 2.0 - centerz + offset;
 
                 // Build the rigid body.
                 let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
                 let handle = bodies.insert(rigid_body);
-                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)
+                let collider1 = ColliderBuilder::cuboid(rad * 10.0, rad, rad).build();
+                let collider2 = ColliderBuilder::cuboid(rad, rad * 10.0, rad)
+                    .translation(rad * 10.0, rad * 10.0, 0.0)
+                    .build();
+                let collider3 = ColliderBuilder::cuboid(rad, rad * 10.0, rad)
+                    .translation(-rad * 10.0, rad * 10.0, 0.0)
                     .build();
                 colliders.insert(collider1, handle, &mut bodies);
                 colliders.insert(collider2, handle, &mut bodies);
+                colliders.insert(collider3, handle, &mut bodies);
             }
         }
 
diff --git a/src/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs
index f2fce4b..586eaaf 100644
--- a/src/dynamics/mass_properties.rs
+++ b/src/dynamics/mass_properties.rs
@@ -209,7 +209,8 @@ impl Sub<MassProperties> for MassProperties {
         let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
         let inertia = i1 - i2;
         let eigen = inertia.symmetric_eigen();
-        let principal_inertia_local_frame = Rotation::from_matrix(&eigen.eigenvectors);
+        let principal_inertia_local_frame =
+            Rotation::from_matrix_eps(&eigen.eigenvectors, 1.0e-6, 10, na::one());
         let principal_inertia = eigen.eigenvalues;
         // NOTE: we drop the negative eigenvalues that may result from subtraction rounding errors.
         let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.max(0.0).sqrt()));
@@ -272,7 +273,8 @@ impl Add<MassProperties> for MassProperties {
         let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
         let inertia = i1 + i2;
         let eigen = inertia.symmetric_eigen();
-        let principal_inertia_local_frame = Rotation::from_matrix(&eigen.eigenvectors);
+        let principal_inertia_local_frame =
+            Rotation::from_matrix_eps(&eigen.eigenvectors, 1.0e-6, 10, na::one());
         let principal_inertia = eigen.eigenvalues;
         let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.sqrt()));
 
diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs
index 66036c8..b06fd7e 100644
--- a/src_testbed/physx_backend.rs
+++ b/src_testbed/physx_backend.rs
@@ -228,7 +228,7 @@ impl PhysxWorld {
         // Update mass properties.
         for (rapier_handle, physx_handle) in rapier2physx.iter() {
             let rb = &bodies[*rapier_handle];
-            if let Some(mut ra) = scene.get_dynamic_mut(*physx_handle) {
+            if let Some(rp) = scene.get_dynamic_mut(*physx_handle) {
                 let densities: Vec<_> = rb
                     .colliders()
                     .iter()
@@ -237,7 +237,7 @@ impl PhysxWorld {
 
                 unsafe {
                     physx_sys::PxRigidBodyExt_updateMassAndInertia_mut(
-                        ra.as_ptr_mut().ptr as *mut physx_sys::PxRigidBody,
+                        rp.as_ptr_mut().ptr as *mut physx_sys::PxRigidBody,
                         densities.as_ptr(),
                         densities.len() as u32,
                         std::ptr::null(),
-- 
cgit