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::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);
}
}

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 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()));

4
src_testbed/physx_backend.rs
View File

@@ -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(),