nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Mass properties: add a max number of iterations for the local-frame rotation computation.

Browse Source
This commit is contained in:
Sébastien Crozet
2020-09-01 17:35:32 +02:00
parent 2f2a073ce4
commit fc0b3bf394
3 changed files with 20 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
examples3d/compound3.rs
View File

@@ -1,4 +1,4 @@
use na::{Point3, Vector3}; use na::Point3;
use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
use rapier3d::geometry::{ColliderBuilder, ColliderSet}; use rapier3d::geometry::{ColliderBuilder, ColliderSet};
use rapier_testbed3d::Testbed; use rapier_testbed3d::Testbed;
@@ -28,33 +28,35 @@ pub fn init_world(testbed: &mut Testbed) {
* Create the cubes * Create the cubes
*/ */
let num = 8; 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 centerx = shift * (num / 2) as f32;
let centery = shift / 2.0; let centery = shift / 2.0;
let centerz = shift * (num / 2) as f32; let centerz = shift * (num / 2) as f32;
let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5; 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 i in 0..num {
for k in 0usize..num { for k in 0usize..num {
let x = i as f32 * shift - centerx + offset; let x = i as f32 * shift * 5.0 - centerx + offset;
let y = j as f32 * (shift * 2.0) + centery + 3.0; let y = j as f32 * (shift * 5.0) + centery + 3.0;
let z = k as f32 * shift - centerz + offset; let z = k as f32 * shift * 2.0 - centerz + offset;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
let collider1 = ColliderBuilder::cuboid(rad, rad, rad).density(1.0).build(); let collider1 = ColliderBuilder::cuboid(rad * 10.0, rad, rad).build();
let collider2 = ColliderBuilder::cuboid(rad, rad, rad) let collider2 = ColliderBuilder::cuboid(rad, rad * 10.0, rad)
.translation(0.0, -rad * 3.0, 0.0) .translation(rad * 10.0, rad * 10.0, 0.0)
.rotation(Vector3::x() * 0.5) .build();
.density(1.0) let collider3 = ColliderBuilder::cuboid(rad, rad * 10.0, rad)
.translation(-rad * 10.0, rad * 10.0, 0.0)
.build(); .build();
colliders.insert(collider1, handle, &mut bodies); colliders.insert(collider1, handle, &mut bodies);
colliders.insert(collider2, handle, &mut bodies); colliders.insert(collider2, handle, &mut bodies);
colliders.insert(collider3, handle, &mut bodies);
} }
} }

6
src/dynamics/mass_properties.rs
View File

@@ -209,7 +209,8 @@ impl Sub<MassProperties> for MassProperties {
let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
let inertia = i1 - i2; let inertia = i1 - i2;
let eigen = inertia.symmetric_eigen(); 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 principal_inertia = eigen.eigenvalues;
// NOTE: we drop the negative eigenvalues that may result from subtraction rounding errors. // 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())); 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 i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
let inertia = i1 + i2; let inertia = i1 + i2;
let eigen = inertia.symmetric_eigen(); 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 principal_inertia = eigen.eigenvalues;
let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.sqrt())); let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.sqrt()));

4
src_testbed/physx_backend.rs
View File

@@ -228,7 +228,7 @@ impl PhysxWorld {
// Update mass properties. // Update mass properties.
for (rapier_handle, physx_handle) in rapier2physx.iter() { for (rapier_handle, physx_handle) in rapier2physx.iter() {
let rb = &bodies[*rapier_handle]; 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 let densities: Vec<_> = rb
.colliders() .colliders()
.iter() .iter()
@@ -237,7 +237,7 @@ impl PhysxWorld {
unsafe { unsafe {
physx_sys::PxRigidBodyExt_updateMassAndInertia_mut( 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.as_ptr(),
densities.len() as u32, densities.len() as u32,
std::ptr::null(), std::ptr::null(),