From df2156ffd02ea1b8c86e86f1d68c5e4e915e6d98 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Mon, 31 Aug 2020 17:58:14 +0200 Subject: [PATCH 01/15] Allow the removal of a collider. --- examples3d/compound3.rs | 68 +++++++++++ src/dynamics/mass_properties.rs | 165 +++++++++++++++++++++++++- src/dynamics/rigid_body.rs | 20 +++- src/geometry/broad_phase_multi_sap.rs | 4 +- src/geometry/collider.rs | 41 ++++++- src/geometry/collider_set.rs | 6 +- src/pipeline/physics_pipeline.rs | 24 +++- 7 files changed, 318 insertions(+), 10 deletions(-) create mode 100644 examples3d/compound3.rs diff --git a/examples3d/compound3.rs b/examples3d/compound3.rs new file mode 100644 index 0000000..eb8a472 --- /dev/null +++ b/examples3d/compound3.rs @@ -0,0 +1,68 @@ +use na::Point3; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +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.1; + 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 num = 8; + let rad = 1.0; + + let shift = rad * 2.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 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 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); + } + } + + offset -= 0.05 * rad * (num as f32 - 1.0); + } + + /* + * 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/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs index cc2979c..8affe0a 100644 --- a/src/dynamics/mass_properties.rs +++ b/src/dynamics/mass_properties.rs @@ -1,7 +1,7 @@ use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Vector}; use crate::utils; use num::Zero; -use std::ops::{Add, AddAssign}; +use std::ops::{Add, AddAssign, Sub, SubAssign}; #[cfg(feature = "dim3")] use {na::Matrix3, std::ops::MulAssign}; @@ -24,6 +24,59 @@ pub struct MassProperties { pub principal_inertia_local_frame: Rotation, } +impl approx::AbsDiffEq for MassProperties { + type Epsilon = f32; + fn default_epsilon() -> Self::Epsilon { + f32::default_epsilon() + } + + fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool { + self.local_com.abs_diff_eq(&other.local_com, epsilon) + && self.inv_mass.abs_diff_eq(&other.inv_mass, epsilon) + && self + .inv_principal_inertia_sqrt + .abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon) + // && self + // .principal_inertia_local_frame + // .abs_diff_eq(&other.principal_inertia_local_frame, epsilon) + } +} + +impl approx::RelativeEq for MassProperties { + fn default_max_relative() -> Self::Epsilon { + f32::default_max_relative() + } + + fn relative_eq( + &self, + other: &Self, + epsilon: Self::Epsilon, + max_relative: Self::Epsilon, + ) -> bool { + #[cfg(feature = "dim2")] + let inertia_is_ok = self.inv_principal_inertia_sqrt.relative_eq( + &other.inv_principal_inertia_sqrt, + epsilon, + max_relative, + ); + + #[cfg(feature = "dim3")] + let inertia_is_ok = self.reconstruct_inverse_inertia_matrix().relative_eq( + &other.reconstruct_inverse_inertia_matrix(), + epsilon, + max_relative, + ); + + inertia_is_ok + && self + .local_com + .relative_eq(&other.local_com, epsilon, max_relative) + && self + .inv_mass + .relative_eq(&other.inv_mass, epsilon, max_relative) + } +} + impl MassProperties { #[cfg(feature = "dim2")] pub(crate) fn new(local_com: Point, mass: f32, principal_inertia: f32) -> Self { @@ -90,6 +143,18 @@ impl MassProperties { } } + #[cfg(feature = "dim3")] + /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axii. + pub fn reconstruct_inverse_inertia_matrix(&self) -> Matrix3 { + let inv_principal_inertia = self.inv_principal_inertia_sqrt.map(|e| e * e); + self.principal_inertia_local_frame.to_rotation_matrix() + * Matrix3::from_diagonal(&inv_principal_inertia) + * self + .principal_inertia_local_frame + .inverse() + .to_rotation_matrix() + } + #[cfg(feature = "dim3")] /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axii. pub fn reconstruct_inertia_matrix(&self) -> Matrix3 { @@ -143,6 +208,67 @@ impl Zero for MassProperties { } } +impl Sub for MassProperties { + type Output = Self; + + #[cfg(feature = "dim2")] + fn sub(self, other: MassProperties) -> Self { + if self.is_zero() || other.is_zero() { + return self; + } + + let m1 = utils::inv(self.inv_mass); + let m2 = utils::inv(other.inv_mass); + let inv_mass = utils::inv(m1 - m2); + + let local_com = (self.local_com * m1 - other.local_com.coords * m2) * inv_mass; + let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com); + let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); + let inertia = i1 - i2; + // NOTE: we drop the negative eigenvalues that may result from subtraction rounding errors. + let inv_principal_inertia_sqrt = utils::inv(inertia.max(0.0).sqrt()); + + Self { + local_com, + inv_mass, + inv_principal_inertia_sqrt, + } + } + + #[cfg(feature = "dim3")] + fn sub(self, other: MassProperties) -> Self { + if self.is_zero() || other.is_zero() { + return self; + } + + let m1 = utils::inv(self.inv_mass); + let m2 = utils::inv(other.inv_mass); + let inv_mass = utils::inv(m1 - m2); + let local_com = (self.local_com * m1 - other.local_com.coords * m2) * inv_mass; + let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com); + 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 = 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())); + + Self { + local_com, + inv_mass, + inv_principal_inertia_sqrt, + principal_inertia_local_frame, + } + } +} + +impl SubAssign for MassProperties { + fn sub_assign(&mut self, rhs: MassProperties) { + *self = *self - rhs + } +} + impl Add for MassProperties { type Output = Self; @@ -204,3 +330,40 @@ impl AddAssign for MassProperties { *self = *self + rhs } } + +#[cfg(test)] +mod test { + use super::MassProperties; + use crate::geometry::ColliderBuilder; + use approx::assert_relative_eq; + use num::Zero; + + #[test] + fn mass_properties_add_sub() { + // Check that addition and subtraction of mass properties behave as expected. + let c1 = ColliderBuilder::capsule_x(1.0, 2.0).build(); + let c2 = ColliderBuilder::capsule_y(3.0, 4.0).build(); + let c3 = ColliderBuilder::ball(5.0).build(); + + let m1 = c1.mass_properties(); + let m2 = c2.mass_properties(); + let m3 = c3.mass_properties(); + let m1m2m3 = m1 + m2 + m3; + + assert_relative_eq!(m1 + m2, m2 + m1, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - m1, m2 + m3, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - m2, m1 + m3, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - m3, m1 + m2, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - (m1 + m2), m3, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - (m1 + m3), m2, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - (m2 + m3), m1, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - m1 - m2, m3, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - m1 - m3, m2, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - m2 - m3, m1, epsilon = 1.0e-6); + assert_relative_eq!( + m1m2m3 - m1 - m2 - m3, + MassProperties::zero(), + epsilon = 1.0e-6 + ); + } +} diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 584dc96..d9cddd1 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -1,5 +1,5 @@ use crate::dynamics::MassProperties; -use crate::geometry::{ColliderHandle, InteractionGraph, RigidBodyGraphIndex}; +use crate::geometry::{Collider, ColliderHandle, InteractionGraph, RigidBodyGraphIndex}; use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Translation, Vector}; use crate::utils::{WCross, WDot}; use num::Zero; @@ -137,6 +137,24 @@ impl RigidBody { crate::utils::inv(self.mass_properties.inv_mass) } + /// Adds a collider to this rigid-body. + pub(crate) fn add_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) { + let mass_properties = coll.mass_properties(); + self.colliders.push(handle); + self.mass_properties += mass_properties; + self.update_world_mass_properties(); + } + + /// Removes a collider from this rigid-body. + pub(crate) fn remove_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) { + if let Some(i) = self.colliders.iter().position(|e| *e == handle) { + self.colliders.swap_remove(i); + let mass_properties = coll.mass_properties(); + self.mass_properties -= mass_properties; + self.update_world_mass_properties(); + } + } + /// Put this rigid body to sleep. /// /// A sleeping body no longer moves and is no longer simulated by the physics engine unless diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs index 0505e21..054fccf 100644 --- a/src/geometry/broad_phase_multi_sap.rs +++ b/src/geometry/broad_phase_multi_sap.rs @@ -662,7 +662,7 @@ mod test { let rb = RigidBodyBuilder::new_dynamic().build(); let co = ColliderBuilder::ball(0.5).build(); let hrb = bodies.insert(rb); - let hco = colliders.insert(co, hrb, &mut bodies); + colliders.insert(co, hrb, &mut bodies); broad_phase.update_aabbs(0.0, &bodies, &mut colliders); @@ -681,7 +681,7 @@ mod test { let rb = RigidBodyBuilder::new_dynamic().build(); let co = ColliderBuilder::ball(0.5).build(); let hrb = bodies.insert(rb); - let hco = colliders.insert(co, hrb, &mut bodies); + colliders.insert(co, hrb, &mut bodies); // Make sure the proxy handles is recycled properly. broad_phase.update_aabbs(0.0, &bodies, &mut colliders); diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 2457212..01c78ac 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -3,7 +3,7 @@ use crate::geometry::{ Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon, Proximity, Triangle, Trimesh, }; -use crate::math::{Isometry, Point, Vector}; +use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; use na::Point3; use ncollide::bounding_volume::{HasBoundingVolume, AABB}; use num::Zero; @@ -159,6 +159,11 @@ impl Collider { &self.position } + /// The position of this collider wrt the body it is attached to. + pub fn position_wrt_parent(&self) -> &Isometry { + &self.delta + } + /// The density of this collider. pub fn density(&self) -> f32 { self.density @@ -347,7 +352,41 @@ impl ColliderBuilder { self } + /// Sets the initial translation of the collider to be created, + /// relative to the rigid-body it is attached to. + #[cfg(feature = "dim2")] + pub fn translation(mut self, x: f32, y: f32) -> Self { + self.delta.translation.x = x; + self.delta.translation.y = y; + self + } + + /// Sets the initial translation of the collider to be created, + /// relative to the rigid-body it is attached to. + #[cfg(feature = "dim3")] + pub fn translation(mut self, x: f32, y: f32, z: f32) -> Self { + self.delta.translation.x = x; + self.delta.translation.y = y; + self.delta.translation.z = z; + self + } + + /// Sets the initial orientation of the collider to be created, + /// relative to the rigid-body it is attached to. + pub fn rotation(mut self, angle: AngVector) -> Self { + self.delta.rotation = Rotation::new(angle); + self + } + + /// Sets the initial position (translation and orientation) of the collider to be created, + /// relative to the rigid-body it is attached to. + pub fn position(mut self, pos: Isometry) -> Self { + self.delta = pos; + self + } + /// Set the position of this collider in the local-space of the rigid-body it is attached to. + #[deprecated(note = "Use `.position` instead.")] pub fn delta(mut self, delta: Isometry) -> Self { self.delta = delta; self diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs index 73d4a06..22bba1b 100644 --- a/src/geometry/collider_set.rs +++ b/src/geometry/collider_set.rs @@ -47,7 +47,6 @@ impl ColliderSet { parent_handle: RigidBodyHandle, bodies: &mut RigidBodySet, ) -> ColliderHandle { - let mass_properties = coll.mass_properties(); coll.parent = parent_handle; let parent = bodies .get_mut_internal(parent_handle) @@ -55,9 +54,8 @@ impl ColliderSet { coll.position = parent.position * coll.delta; coll.predicted_position = parent.predicted_position * coll.delta; let handle = self.colliders.insert(coll); - parent.colliders.push(handle); - parent.mass_properties += mass_properties; - parent.update_world_mass_properties(); + let coll = self.colliders.get(handle).unwrap(); + parent.add_collider_internal(handle, &coll); bodies.activate(parent_handle); handle } diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 6e18a03..8449477 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -7,7 +7,8 @@ use crate::dynamics::{IntegrationParameters, JointSet, RigidBody, RigidBodyHandl #[cfg(feature = "parallel")] use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver}; use crate::geometry::{ - BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, NarrowPhase, + BroadPhase, BroadPhasePairEvent, Collider, ColliderHandle, ColliderPair, ColliderSet, + ContactManifoldIndex, NarrowPhase, }; use crate::math::Vector; use crate::pipeline::EventHandler; @@ -245,6 +246,27 @@ impl PhysicsPipeline { self.counters.step_completed(); } + /// Remove a collider and all its associated data. + pub fn remove_collider( + &mut self, + handle: ColliderHandle, + broad_phase: &mut BroadPhase, + narrow_phase: &mut NarrowPhase, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + ) -> Option { + broad_phase.remove_colliders(&[handle], colliders); + narrow_phase.remove_colliders(&[handle], colliders, bodies); + let collider = colliders.remove_internal(handle)?; + + if let Some(parent) = bodies.get_mut_internal(collider.parent) { + parent.remove_collider_internal(handle, &collider); + bodies.wake_up(collider.parent); + } + + Some(collider) + } + /// Remove a rigid-body and all its associated data. pub fn remove_rigid_body( &mut self, From c286f44c4e2aa1692781e9c4c7c1ce2bacdd2415 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Mon, 31 Aug 2020 19:02:37 +0200 Subject: [PATCH 02/15] Constraint solver: properly take non-zero center of masses into account. --- src/dynamics/solver/velocity_constraint.rs | 8 +++----- src/dynamics/solver/velocity_constraint_wide.rs | 6 ++++-- src/dynamics/solver/velocity_ground_constraint.rs | 4 ++-- src/dynamics/solver/velocity_ground_constraint_wide.rs | 7 +++++-- 4 files changed, 14 insertions(+), 11 deletions(-) diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 9212e89..eb80018 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -215,10 +215,8 @@ impl VelocityConstraint { for k in 0..manifold_points.len() { let manifold_point = &manifold_points[k]; - let dp1 = (rb1.position * manifold_point.local_p1).coords - - rb1.position.translation.vector; - let dp2 = (rb2.position * manifold_point.local_p2).coords - - rb2.position.translation.vector; + let dp1 = (rb1.position * manifold_point.local_p1) - rb1.world_com; + let dp2 = (rb2.position * manifold_point.local_p2) - rb2.world_com; let vel1 = rb1.linvel + rb1.angvel.gcross(dp1); let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); @@ -355,7 +353,7 @@ impl VelocityConstraint { } } - // Solve penetration. + // Solve non-penetration. for i in 0..self.num_contacts as usize { let elt = &mut self.elements[i].normal_part; let dimpulse = self.dir1.dot(&mj_lambda1.linear) + elt.gcross1.gdot(mj_lambda1.angular) diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 8f387a5..4204e68 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -80,6 +80,7 @@ impl WVelocityConstraint { let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); let ii2: AngularInertia = @@ -89,6 +90,7 @@ impl WVelocityConstraint { let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); let force_dir1 = position1 * -Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]); @@ -130,8 +132,8 @@ impl WVelocityConstraint { let impulse = SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]); - let dp1 = p1.coords - position1.translation.vector; - let dp2 = p2.coords - position2.translation.vector; + let dp1 = p1 - world_com1; + let dp2 = p2 - world_com2; let vel1 = linvel1 + angvel1.gcross(dp1); let vel2 = linvel2 + angvel2.gcross(dp2); diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 457c3b3..65a61bd 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -155,8 +155,8 @@ impl VelocityGroundConstraint { rb2.position * manifold_point.local_p2, ) }; - let dp2 = p2.coords - rb2.position.translation.vector; - let dp1 = p1.coords - rb1.position.translation.vector; + let dp2 = p2 - rb2.world_com; + let dp1 = p1 - rb1.world_com; let vel1 = rb1.linvel + rb1.angvel.gcross(dp1); let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index ec9e186..af2e1e9 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -89,6 +89,9 @@ impl WVelocityGroundConstraint { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + let force_dir1 = position1 * -Vector::from( array![|ii| if flipped[ii] { manifolds[ii].local_n2 } else { manifolds[ii].local_n1 }; SIMD_WIDTH], @@ -130,8 +133,8 @@ impl WVelocityGroundConstraint { let impulse = SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]); - let dp1 = p1.coords - position1.translation.vector; - let dp2 = p2.coords - position2.translation.vector; + let dp1 = p1 - world_com1; + let dp2 = p2 - world_com2; let vel1 = linvel1 + angvel1.gcross(dp1); let vel2 = linvel2 + angvel2.gcross(dp2); From 5731b994634661f403cf589fdf1337d0184c7d8e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Mon, 31 Aug 2020 19:03:15 +0200 Subject: [PATCH 03/15] =?UTF-8?q?Fix=20box-box=20CD=C2=A0when=20colliders?= =?UTF-8?q?=20have=20non-identity=20delta=20pos.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../cuboid_cuboid_contact_generator.rs | 38 ++++++++++++------- 1 file changed, 24 insertions(+), 14 deletions(-) diff --git a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs index d879a22..0d382ac 100644 --- a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs @@ -10,8 +10,10 @@ pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationCont generate_contacts( ctxt.prediction_distance, cube1, + ctxt.collider1.position_wrt_parent(), ctxt.position1, cube2, + ctxt.collider2.position_wrt_parent(), ctxt.position2, ctxt.manifold, ); @@ -26,15 +28,19 @@ pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationCont pub fn generate_contacts<'a>( prediction_distance: f32, mut cube1: &'a Cuboid, + mut origin1: &'a Isometry, mut pos1: &'a Isometry, mut cube2: &'a Cuboid, + mut origin2: &'a Isometry, mut pos2: &'a Isometry, manifold: &mut ContactManifold, ) { let mut pos12 = pos1.inverse() * pos2; let mut pos21 = pos12.inverse(); + let mut orig_pos12 = origin1 * pos12 * origin2.inverse(); + let mut orig_pos21 = orig_pos12.inverse(); - if manifold.try_update_contacts(&pos12) { + if manifold.try_update_contacts(&orig_pos12) { return; } @@ -81,8 +87,9 @@ pub fn generate_contacts<'a>( if sep2.0 > sep1.0 && sep2.0 > sep3.0 { // The reference shape will be the second shape. std::mem::swap(&mut cube1, &mut cube2); - std::mem::swap(&mut pos1, &mut pos2); std::mem::swap(&mut pos12, &mut pos21); + std::mem::swap(&mut orig_pos12, &mut orig_pos21); + std::mem::swap(&mut origin1, &mut origin2); manifold.swap_identifiers(); best_sep = sep2; swapped = true; @@ -97,46 +104,49 @@ pub fn generate_contacts<'a>( // Now the reference feature is from `cube1` and the best separation is `best_sep`. // Everything must be expressed in the local-space of `cube1` for contact clipping. - let feature1 = cuboid::support_feature(cube1, best_sep.1); + let mut feature1 = cuboid::support_feature(cube1, best_sep.1); + feature1.transform_by(origin1); let mut feature2 = cuboid::support_feature(cube2, pos21 * -best_sep.1); feature2.transform_by(&pos12); + feature2.transform_by(origin1); + let n1 = origin1 * best_sep.1; match (&feature1, &feature2) { (CuboidFeature::Face(f1), CuboidFeature::Vertex(v2)) => { - CuboidFeature::face_vertex_contacts(f1, &best_sep.1, v2, &pos21, manifold) + CuboidFeature::face_vertex_contacts(f1, &n1, v2, &orig_pos21, manifold) } #[cfg(feature = "dim3")] (CuboidFeature::Face(f1), CuboidFeature::Edge(e2)) => CuboidFeature::face_edge_contacts( prediction_distance, f1, - &best_sep.1, + &n1, e2, - &pos21, + &orig_pos21, manifold, false, ), (CuboidFeature::Face(f1), CuboidFeature::Face(f2)) => CuboidFeature::face_face_contacts( prediction_distance, f1, - &best_sep.1, + &n1, f2, - &pos21, + &orig_pos21, manifold, ), #[cfg(feature = "dim3")] (CuboidFeature::Edge(e1), CuboidFeature::Edge(e2)) => { - CuboidFeature::edge_edge_contacts(e1, &best_sep.1, e2, &pos21, manifold) + CuboidFeature::edge_edge_contacts(e1, &n1, e2, &orig_pos21, manifold) } #[cfg(feature = "dim3")] (CuboidFeature::Edge(e1), CuboidFeature::Face(f2)) => { // Since f2 is also expressed in the local-space of the first - // feature, the position we provide here is pos21. + // feature, the position we provide here is orig_pos21. CuboidFeature::face_edge_contacts( prediction_distance, f2, - &-best_sep.1, + &-n1, e1, - &pos21, + &orig_pos21, manifold, true, ) @@ -144,8 +154,8 @@ pub fn generate_contacts<'a>( _ => unreachable!(), // The other cases are not possible. } - manifold.local_n1 = best_sep.1; - manifold.local_n2 = pos21 * -best_sep.1; + manifold.local_n1 = n1; + manifold.local_n2 = orig_pos21 * -n1; manifold.kinematics.category = KinematicsCategory::PlanePoint; manifold.kinematics.radius1 = 0.0; manifold.kinematics.radius2 = 0.0; From ce26fe107727ee4ef25ee728b3bb3a40914fdc99 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Mon, 31 Aug 2020 19:03:56 +0200 Subject: [PATCH 04/15] Add compound demo. --- examples3d/all_examples3.rs | 2 ++ examples3d/compound3.rs | 14 ++++++++++---- src_testbed/nphysics_backend.rs | 4 ++-- src_testbed/physx_backend.rs | 4 ++-- 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> { let margin = ColliderDesc::::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)> { - 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()) } From 03b437f278bbcbd391acd23a4d8fa074915eb00c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Mon, 31 Aug 2020 19:04:32 +0200 Subject: [PATCH 05/15] Disallow contacts between two colliders attached to the same parent. --- Cargo.toml | 7 +++++++ examples3d/Cargo.toml | 2 +- src/geometry/narrow_phase.rs | 7 ++++++- 3 files changed, 14 insertions(+), 2 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 32e6107..a766033 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -10,3 +10,10 @@ debug = false codegen-units = 1 #opt-level = 1 #lto = true + + +#[profile.dev.package.rapier3d] +#opt-level = 3 +# +#[profile.dev.package.kiss3d] +#opt-level = 3 \ No newline at end of file diff --git a/examples3d/Cargo.toml b/examples3d/Cargo.toml index 3176696..8091db8 100644 --- a/examples3d/Cargo.toml +++ b/examples3d/Cargo.toml @@ -24,4 +24,4 @@ path = "../build/rapier3d" [[bin]] name = "all_examples3" -path = "./all_examples3.rs" \ No newline at end of file +path = "./all_examples3.rs" diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 3eb2c30..1a36511 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -96,7 +96,7 @@ impl NarrowPhase { } // We have to manage the fact that one other collider will - // hive its graph index changed because of the node's swap-remove. + // have its graph index changed because of the node's swap-remove. if let Some(replacement) = self .proximity_graph .remove_node(proximity_graph_id) @@ -129,6 +129,11 @@ impl NarrowPhase { if let (Some(co1), Some(co2)) = colliders.get2_mut_internal(pair.collider1, pair.collider2) { + if co1.parent == co2.parent { + // Same parents. Ignore collisions. + continue; + } + if co1.is_sensor() || co2.is_sensor() { let gid1 = co1.proximity_graph_index; let gid2 = co2.proximity_graph_index; From 9622827dc6aadb391512b95381edb1efc26b1b90 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Tue, 1 Sep 2020 14:02:59 +0200 Subject: [PATCH 06/15] Fix constraints resolution with non-identity relative collider position. --- src/dynamics/solver/position_constraint.rs | 4 +- .../solver/position_constraint_wide.rs | 7 +++- .../solver/position_ground_constraint.rs | 28 +++++++++----- .../solver/position_ground_constraint_wide.rs | 16 ++++++-- src/dynamics/solver/velocity_constraint.rs | 8 ++-- .../solver/velocity_constraint_wide.rs | 18 ++++++--- .../solver/velocity_ground_constraint.rs | 28 +++++++------- .../solver/velocity_ground_constraint_wide.rs | 20 +++++++--- src/geometry/contact.rs | 24 +++++++++--- .../cuboid_cuboid_contact_generator.rs | 38 +++++++------------ src_testbed/nphysics_backend.rs | 2 +- 11 files changed, 118 insertions(+), 75 deletions(-) diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs index 608a342..69fcf57 100644 --- a/src/dynamics/solver/position_constraint.rs +++ b/src/dynamics/solver/position_constraint.rs @@ -104,8 +104,8 @@ impl PositionConstraint { let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS]; for l in 0..manifold_points.len() { - local_p1[l] = manifold_points[l].local_p1 + shift1; - local_p2[l] = manifold_points[l].local_p2 + shift2; + local_p1[l] = manifold.delta1 * (manifold_points[l].local_p1 + shift1); + local_p2[l] = manifold.delta2 * (manifold_points[l].local_p2 + shift2); } let constraint = PositionConstraint { diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs index 8828c3d..0633926 100644 --- a/src/dynamics/solver/position_constraint_wide.rs +++ b/src/dynamics/solver/position_constraint_wide.rs @@ -51,6 +51,9 @@ impl WPositionConstraint { let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]); + let delta1 = Isometry::from(array![|ii| manifolds[ii].delta1; SIMD_WIDTH]); + let delta2 = Isometry::from(array![|ii| manifolds[ii].delta2; SIMD_WIDTH]); + let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -85,8 +88,8 @@ impl WPositionConstraint { let local_p2 = Point::from(array![|ii| manifold_points[ii][i].local_p2; SIMD_WIDTH]); - constraint.local_p1[i] = local_p1 + shift1; - constraint.local_p2[i] = local_p2 + shift2; + constraint.local_p1[i] = delta1 * (local_p1 + shift1); + constraint.local_p2[i] = delta2 * (local_p2 + shift2); } if push { diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs index e6e83c6..dcd2d64 100644 --- a/src/dynamics/solver/position_ground_constraint.rs +++ b/src/dynamics/solver/position_ground_constraint.rs @@ -34,22 +34,30 @@ impl PositionGroundConstraint { let local_n1; let local_n2; + let delta1; + let delta2; if flip { std::mem::swap(&mut rb1, &mut rb2); local_n1 = manifold.local_n2; local_n2 = manifold.local_n1; + delta1 = &manifold.delta2; + delta2 = &manifold.delta1; } else { local_n1 = manifold.local_n1; local_n2 = manifold.local_n2; + delta1 = &manifold.delta1; + delta2 = &manifold.delta2; }; + let coll_pos1 = rb1.position * delta1; let shift1 = local_n1 * -manifold.kinematics.radius1; let shift2 = local_n2 * -manifold.kinematics.radius2; + let n1 = coll_pos1 * local_n1; let radius = manifold.kinematics.radius1 + manifold.kinematics.radius2 /* - params.allowed_linear_error */; - for (l, manifold_points) in manifold + for (l, manifold_contacts) in manifold .active_contacts() .chunks(MAX_MANIFOLD_POINTS) .enumerate() @@ -59,16 +67,16 @@ impl PositionGroundConstraint { if flip { // Don't forget that we already swapped rb1 and rb2 above. - // So if we flip, only manifold_points[k].{local_p1,local_p2} have to + // So if we flip, only manifold_contacts[k].{local_p1,local_p2} have to // be swapped. - for k in 0..manifold_points.len() { - p1[k] = rb1.predicted_position * (manifold_points[k].local_p2 + shift1); - local_p2[k] = manifold_points[k].local_p1 + shift2; + for k in 0..manifold_contacts.len() { + p1[k] = coll_pos1 * (manifold_contacts[k].local_p2 + shift1); + local_p2[k] = delta2 * (manifold_contacts[k].local_p1 + shift2); } } else { - for k in 0..manifold_points.len() { - p1[k] = rb1.predicted_position * (manifold_points[k].local_p1 + shift1); - local_p2[k] = manifold_points[k].local_p2 + shift2; + for k in 0..manifold_contacts.len() { + p1[k] = coll_pos1 * (manifold_contacts[k].local_p1 + shift1); + local_p2[k] = delta2 * (manifold_contacts[k].local_p2 + shift2); } } @@ -76,11 +84,11 @@ impl PositionGroundConstraint { rb2: rb2.active_set_offset, p1, local_p2, - n1: rb1.predicted_position * local_n1, + n1, radius, im2: rb2.mass_properties.inv_mass, ii2: rb2.world_inv_inertia_sqrt.squared(), - num_contacts: manifold_points.len() as u8, + num_contacts: manifold_contacts.len() as u8, erp: params.erp, max_linear_correction: params.max_linear_correction, }; diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs index faa928b..e423c0a 100644 --- a/src/dynamics/solver/position_ground_constraint_wide.rs +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -54,16 +54,24 @@ impl WPositionGroundConstraint { array![|ii| if flipped[ii] { manifolds[ii].local_n1 } else { manifolds[ii].local_n2 }; SIMD_WIDTH], ); + let delta1 = Isometry::from( + array![|ii| if flipped[ii] { manifolds[ii].delta2 } else { manifolds[ii].delta1 }; SIMD_WIDTH], + ); + let delta2 = Isometry::from( + array![|ii| if flipped[ii] { manifolds[ii].delta1 } else { manifolds[ii].delta2 }; SIMD_WIDTH], + ); + let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]); - let position1 = Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]); + let coll_pos1 = + delta1 * Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]); let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; let radius = radius1 + radius2 /*- SimdFloat::splat(params.allowed_linear_error)*/; - let n1 = position1 * local_n1; + let n1 = coll_pos1 * local_n1; for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH]; @@ -90,8 +98,8 @@ impl WPositionGroundConstraint { array![|ii| if flipped[ii] { manifold_points[ii][i].local_p1 } else { manifold_points[ii][i].local_p2 }; SIMD_WIDTH], ); - constraint.p1[i] = position1 * local_p1 - n1 * radius1; - constraint.local_p2[i] = local_p2 - local_n2 * radius2; + constraint.p1[i] = coll_pos1 * local_p1 - n1 * radius1; + constraint.local_p2[i] = delta2 * (local_p2 - local_n2 * radius2); } if push { diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index eb80018..190076d 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -148,7 +148,9 @@ impl VelocityConstraint { let rb2 = &bodies[manifold.body_pair.body2]; let mj_lambda1 = rb1.active_set_offset; let mj_lambda2 = rb2.active_set_offset; - let force_dir1 = rb1.position * (-manifold.local_n1); + let pos_coll1 = rb1.position * manifold.delta1; + let pos_coll2 = rb2.position * manifold.delta2; + let force_dir1 = pos_coll1 * (-manifold.local_n1); let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff; for (l, manifold_points) in manifold @@ -215,8 +217,8 @@ impl VelocityConstraint { for k in 0..manifold_points.len() { let manifold_point = &manifold_points[k]; - let dp1 = (rb1.position * manifold_point.local_p1) - rb1.world_com; - let dp2 = (rb2.position * manifold_point.local_p2) - rb2.world_com; + let dp1 = (pos_coll1 * manifold_point.local_p1) - rb1.world_com; + let dp2 = (pos_coll2 * manifold_point.local_p2) - rb2.world_com; let vel1 = rb1.linvel + rb1.angvel.gcross(dp1); let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 4204e68..f515d5e 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -72,6 +72,9 @@ impl WVelocityConstraint { let rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH]; let rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH]; + let delta1 = Isometry::from(array![|ii| manifolds[ii].delta1; SIMD_WIDTH]); + let delta2 = Isometry::from(array![|ii| manifolds[ii].delta2; SIMD_WIDTH]); + let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); let ii1: AngularInertia = AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); @@ -79,7 +82,7 @@ impl WVelocityConstraint { let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); - let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); + let pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); @@ -89,10 +92,13 @@ impl WVelocityConstraint { let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); - let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let force_dir1 = position1 * -Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]); + let coll_pos1 = pos1 * delta1; + let coll_pos2 = pos2 * delta2; + + let force_dir1 = coll_pos1 * -Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]); let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -120,11 +126,11 @@ impl WVelocityConstraint { }; for k in 0..num_points { - // FIXME: can we avoid the multiplications by position1/position2 here? + // FIXME: can we avoid the multiplications by coll_pos1/coll_pos2 here? // By working as much as possible in local-space. - let p1 = position1 + let p1 = coll_pos1 * Point::from(array![|ii| manifold_points[ii][k].local_p1; SIMD_WIDTH]); - let p2 = position2 + let p2 = coll_pos2 * Point::from(array![|ii| manifold_points[ii][k].local_p2; SIMD_WIDTH]); let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 65a61bd..d9229ff 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -66,20 +66,22 @@ impl VelocityGroundConstraint { let mut rb1 = &bodies[manifold.body_pair.body1]; let mut rb2 = &bodies[manifold.body_pair.body2]; let flipped = !rb2.is_dynamic(); + let force_dir1; + let coll_pos1; + let coll_pos2; if flipped { + coll_pos1 = rb2.position * manifold.delta2; + coll_pos2 = rb1.position * manifold.delta1; + force_dir1 = coll_pos1 * (-manifold.local_n2); std::mem::swap(&mut rb1, &mut rb2); + } else { + coll_pos1 = rb1.position * manifold.delta1; + coll_pos2 = rb2.position * manifold.delta2; + force_dir1 = coll_pos1 * (-manifold.local_n1); } let mj_lambda2 = rb2.active_set_offset; - let force_dir1 = if flipped { - // NOTE: we already swapped rb1 and rb2 - // so we multiply by rb1.position. - rb1.position * (-manifold.local_n2) - } else { - rb1.position * (-manifold.local_n1) - }; - let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff; for (l, manifold_points) in manifold @@ -144,15 +146,15 @@ impl VelocityGroundConstraint { let manifold_point = &manifold_points[k]; let (p1, p2) = if flipped { // NOTE: we already swapped rb1 and rb2 - // so we multiply by rb2.position. + // so we multiply by coll_pos1/coll_pos2. ( - rb1.position * manifold_point.local_p2, - rb2.position * manifold_point.local_p1, + coll_pos1 * manifold_point.local_p2, + coll_pos2 * manifold_point.local_p1, ) } else { ( - rb1.position * manifold_point.local_p1, - rb2.position * manifold_point.local_p2, + coll_pos1 * manifold_point.local_p1, + coll_pos2 * manifold_point.local_p2, ) }; let dp2 = p2 - rb2.world_com; diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index af2e1e9..18e9257 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -86,13 +86,23 @@ impl WVelocityGroundConstraint { let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); - let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); - let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); + let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + + let delta1 = Isometry::from( + array![|ii| if flipped[ii] { manifolds[ii].delta2 } else { manifolds[ii].delta1 }; SIMD_WIDTH], + ); + let delta2 = Isometry::from( + array![|ii| if flipped[ii] { manifolds[ii].delta1 } else { manifolds[ii].delta2 }; SIMD_WIDTH], + ); + + let coll_pos1 = pos1 * delta1; + let coll_pos2 = pos2 * delta2; let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let force_dir1 = position1 + let force_dir1 = coll_pos1 * -Vector::from( array![|ii| if flipped[ii] { manifolds[ii].local_n2 } else { manifolds[ii].local_n1 }; SIMD_WIDTH], ); @@ -120,11 +130,11 @@ impl WVelocityGroundConstraint { }; for k in 0..num_points { - let p1 = position1 + let p1 = coll_pos1 * Point::from( array![|ii| if flipped[ii] { manifold_points[ii][k].local_p2 } else { manifold_points[ii][k].local_p1 }; SIMD_WIDTH], ); - let p2 = position2 + let p2 = coll_pos2 * Point::from( array![|ii| if flipped[ii] { manifold_points[ii][k].local_p1 } else { manifold_points[ii][k].local_p2 }; SIMD_WIDTH], ); diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs index 0beec0a..2cda3e0 100644 --- a/src/geometry/contact.rs +++ b/src/geometry/contact.rs @@ -273,16 +273,21 @@ pub struct ContactManifold { /// The pair of subshapes involved in this contact manifold. pub subshape_index_pair: (usize, usize), pub(crate) warmstart_multiplier: f32, - // We put the friction and restitution here because - // this avoids reading the colliders inside of the + // The two following are set by the constraints solver. + pub(crate) constraint_index: usize, + pub(crate) position_constraint_index: usize, + // We put the following fields here to avoids reading the colliders inside of the // contact preparation method. /// The friction coefficient for of all the contacts on this contact manifold. pub friction: f32, /// The restitution coefficient for all the contacts on this contact manifold. pub restitution: f32, - // The following are set by the constraints solver. - pub(crate) constraint_index: usize, - pub(crate) position_constraint_index: usize, + /// The relative position between the first collider and its parent at the time the + /// contact points were generated. + pub delta1: Isometry, + /// The relative position between the second collider and its parent at the time the + /// contact points were generated. + pub delta2: Isometry, } impl ContactManifold { @@ -290,6 +295,8 @@ impl ContactManifold { pair: ColliderPair, subshapes: (usize, usize), body_pair: BodyPair, + delta1: Isometry, + delta2: Isometry, friction: f32, restitution: f32, ) -> ContactManifold { @@ -308,6 +315,8 @@ impl ContactManifold { warmstart_multiplier: Self::min_warmstart_multiplier(), friction, restitution, + delta1, + delta2, constraint_index: 0, position_constraint_index: 0, } @@ -329,6 +338,8 @@ impl ContactManifold { warmstart_multiplier: self.warmstart_multiplier, friction: self.friction, restitution: self.restitution, + delta1: self.delta1, + delta2: self.delta2, constraint_index: self.constraint_index, position_constraint_index: self.position_constraint_index, } @@ -349,6 +360,8 @@ impl ContactManifold { pair, (subshape1, subshape2), BodyPair::new(coll1.parent, coll2.parent), + *coll1.delta(), + *coll2.delta(), (coll1.friction + coll2.friction) * 0.5, (coll1.restitution + coll2.restitution) * 0.5, ) @@ -391,6 +404,7 @@ impl ContactManifold { self.pair = self.pair.swap(); self.body_pair = self.body_pair.swap(); self.subshape_index_pair = (self.subshape_index_pair.1, self.subshape_index_pair.0); + std::mem::swap(&mut self.delta1, &mut self.delta2); } pub(crate) fn update_warmstart_multiplier(&mut self) { diff --git a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs index 0d382ac..d879a22 100644 --- a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs @@ -10,10 +10,8 @@ pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationCont generate_contacts( ctxt.prediction_distance, cube1, - ctxt.collider1.position_wrt_parent(), ctxt.position1, cube2, - ctxt.collider2.position_wrt_parent(), ctxt.position2, ctxt.manifold, ); @@ -28,19 +26,15 @@ pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationCont pub fn generate_contacts<'a>( prediction_distance: f32, mut cube1: &'a Cuboid, - mut origin1: &'a Isometry, mut pos1: &'a Isometry, mut cube2: &'a Cuboid, - mut origin2: &'a Isometry, mut pos2: &'a Isometry, manifold: &mut ContactManifold, ) { let mut pos12 = pos1.inverse() * pos2; let mut pos21 = pos12.inverse(); - let mut orig_pos12 = origin1 * pos12 * origin2.inverse(); - let mut orig_pos21 = orig_pos12.inverse(); - if manifold.try_update_contacts(&orig_pos12) { + if manifold.try_update_contacts(&pos12) { return; } @@ -87,9 +81,8 @@ pub fn generate_contacts<'a>( if sep2.0 > sep1.0 && sep2.0 > sep3.0 { // The reference shape will be the second shape. std::mem::swap(&mut cube1, &mut cube2); + std::mem::swap(&mut pos1, &mut pos2); std::mem::swap(&mut pos12, &mut pos21); - std::mem::swap(&mut orig_pos12, &mut orig_pos21); - std::mem::swap(&mut origin1, &mut origin2); manifold.swap_identifiers(); best_sep = sep2; swapped = true; @@ -104,49 +97,46 @@ pub fn generate_contacts<'a>( // Now the reference feature is from `cube1` and the best separation is `best_sep`. // Everything must be expressed in the local-space of `cube1` for contact clipping. - let mut feature1 = cuboid::support_feature(cube1, best_sep.1); - feature1.transform_by(origin1); + let feature1 = cuboid::support_feature(cube1, best_sep.1); let mut feature2 = cuboid::support_feature(cube2, pos21 * -best_sep.1); feature2.transform_by(&pos12); - feature2.transform_by(origin1); - let n1 = origin1 * best_sep.1; match (&feature1, &feature2) { (CuboidFeature::Face(f1), CuboidFeature::Vertex(v2)) => { - CuboidFeature::face_vertex_contacts(f1, &n1, v2, &orig_pos21, manifold) + CuboidFeature::face_vertex_contacts(f1, &best_sep.1, v2, &pos21, manifold) } #[cfg(feature = "dim3")] (CuboidFeature::Face(f1), CuboidFeature::Edge(e2)) => CuboidFeature::face_edge_contacts( prediction_distance, f1, - &n1, + &best_sep.1, e2, - &orig_pos21, + &pos21, manifold, false, ), (CuboidFeature::Face(f1), CuboidFeature::Face(f2)) => CuboidFeature::face_face_contacts( prediction_distance, f1, - &n1, + &best_sep.1, f2, - &orig_pos21, + &pos21, manifold, ), #[cfg(feature = "dim3")] (CuboidFeature::Edge(e1), CuboidFeature::Edge(e2)) => { - CuboidFeature::edge_edge_contacts(e1, &n1, e2, &orig_pos21, manifold) + CuboidFeature::edge_edge_contacts(e1, &best_sep.1, e2, &pos21, manifold) } #[cfg(feature = "dim3")] (CuboidFeature::Edge(e1), CuboidFeature::Face(f2)) => { // Since f2 is also expressed in the local-space of the first - // feature, the position we provide here is orig_pos21. + // feature, the position we provide here is pos21. CuboidFeature::face_edge_contacts( prediction_distance, f2, - &-n1, + &-best_sep.1, e1, - &orig_pos21, + &pos21, manifold, true, ) @@ -154,8 +144,8 @@ pub fn generate_contacts<'a>( _ => unreachable!(), // The other cases are not possible. } - manifold.local_n1 = n1; - manifold.local_n2 = orig_pos21 * -n1; + manifold.local_n1 = best_sep.1; + manifold.local_n2 = pos21 * -best_sep.1; manifold.kinematics.category = KinematicsCategory::PlanePoint; manifold.kinematics.radius1 = 0.0; manifold.kinematics.radius2 = 0.0; diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs index a2a9bfc..f66d987 100644 --- a/src_testbed/nphysics_backend.rs +++ b/src_testbed/nphysics_backend.rs @@ -13,7 +13,7 @@ use rapier::dynamics::{ IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet, }; use rapier::geometry::{Collider, ColliderSet, Shape}; -use rapier::math::{Isometry, Vector}; +use rapier::math::Vector; use std::collections::HashMap; #[cfg(feature = "dim3")] use {ncollide::shape::TriMesh, nphysics::joint::BallConstraint}; From 2f2a073ce47eaa17f44d88b9dc6cc56362c374e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Tue, 1 Sep 2020 17:05:24 +0200 Subject: [PATCH 07/15] Fix mass property update when adding a collider. --- src/dynamics/mass_properties.rs | 119 ++++++++++++++++++-------------- src/dynamics/rigid_body.rs | 10 ++- src/geometry/collider.rs | 1 + src/geometry/contact.rs | 4 +- src_testbed/nphysics_backend.rs | 2 +- src_testbed/physx_backend.rs | 24 ++++++- src_testbed/testbed.rs | 31 +++++++-- 7 files changed, 127 insertions(+), 64 deletions(-) diff --git a/src/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs index 8affe0a..f2fce4b 100644 --- a/src/dynamics/mass_properties.rs +++ b/src/dynamics/mass_properties.rs @@ -24,59 +24,6 @@ pub struct MassProperties { pub principal_inertia_local_frame: Rotation, } -impl approx::AbsDiffEq for MassProperties { - type Epsilon = f32; - fn default_epsilon() -> Self::Epsilon { - f32::default_epsilon() - } - - fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool { - self.local_com.abs_diff_eq(&other.local_com, epsilon) - && self.inv_mass.abs_diff_eq(&other.inv_mass, epsilon) - && self - .inv_principal_inertia_sqrt - .abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon) - // && self - // .principal_inertia_local_frame - // .abs_diff_eq(&other.principal_inertia_local_frame, epsilon) - } -} - -impl approx::RelativeEq for MassProperties { - fn default_max_relative() -> Self::Epsilon { - f32::default_max_relative() - } - - fn relative_eq( - &self, - other: &Self, - epsilon: Self::Epsilon, - max_relative: Self::Epsilon, - ) -> bool { - #[cfg(feature = "dim2")] - let inertia_is_ok = self.inv_principal_inertia_sqrt.relative_eq( - &other.inv_principal_inertia_sqrt, - epsilon, - max_relative, - ); - - #[cfg(feature = "dim3")] - let inertia_is_ok = self.reconstruct_inverse_inertia_matrix().relative_eq( - &other.reconstruct_inverse_inertia_matrix(), - epsilon, - max_relative, - ); - - inertia_is_ok - && self - .local_com - .relative_eq(&other.local_com, epsilon, max_relative) - && self - .inv_mass - .relative_eq(&other.inv_mass, epsilon, max_relative) - } -} - impl MassProperties { #[cfg(feature = "dim2")] pub(crate) fn new(local_com: Point, mass: f32, principal_inertia: f32) -> Self { @@ -190,6 +137,19 @@ impl MassProperties { Matrix3::zeros() } } + + /// Transform each element of the mass properties. + pub fn transform_by(&self, m: &Isometry) -> Self { + // NOTE: we don't apply the parallel axis theorem here + // because the center of mass is also transformed. + Self { + local_com: m * self.local_com, + inv_mass: self.inv_mass, + inv_principal_inertia_sqrt: self.inv_principal_inertia_sqrt, + #[cfg(feature = "dim3")] + principal_inertia_local_frame: m.rotation * self.principal_inertia_local_frame, + } + } } impl Zero for MassProperties { @@ -331,6 +291,59 @@ impl AddAssign for MassProperties { } } +impl approx::AbsDiffEq for MassProperties { + type Epsilon = f32; + fn default_epsilon() -> Self::Epsilon { + f32::default_epsilon() + } + + fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool { + self.local_com.abs_diff_eq(&other.local_com, epsilon) + && self.inv_mass.abs_diff_eq(&other.inv_mass, epsilon) + && self + .inv_principal_inertia_sqrt + .abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon) + // && self + // .principal_inertia_local_frame + // .abs_diff_eq(&other.principal_inertia_local_frame, epsilon) + } +} + +impl approx::RelativeEq for MassProperties { + fn default_max_relative() -> Self::Epsilon { + f32::default_max_relative() + } + + fn relative_eq( + &self, + other: &Self, + epsilon: Self::Epsilon, + max_relative: Self::Epsilon, + ) -> bool { + #[cfg(feature = "dim2")] + let inertia_is_ok = self.inv_principal_inertia_sqrt.relative_eq( + &other.inv_principal_inertia_sqrt, + epsilon, + max_relative, + ); + + #[cfg(feature = "dim3")] + let inertia_is_ok = self.reconstruct_inverse_inertia_matrix().relative_eq( + &other.reconstruct_inverse_inertia_matrix(), + epsilon, + max_relative, + ); + + inertia_is_ok + && self + .local_com + .relative_eq(&other.local_com, epsilon, max_relative) + && self + .inv_mass + .relative_eq(&other.inv_mass, epsilon, max_relative) + } +} + #[cfg(test)] mod test { use super::MassProperties; diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index d9cddd1..a2fcacc 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -139,7 +139,9 @@ impl RigidBody { /// Adds a collider to this rigid-body. pub(crate) fn add_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) { - let mass_properties = coll.mass_properties(); + let mass_properties = coll + .mass_properties() + .transform_by(coll.position_wrt_parent()); self.colliders.push(handle); self.mass_properties += mass_properties; self.update_world_mass_properties(); @@ -149,7 +151,9 @@ impl RigidBody { pub(crate) fn remove_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) { if let Some(i) = self.colliders.iter().position(|e| *e == handle) { self.colliders.swap_remove(i); - let mass_properties = coll.mass_properties(); + let mass_properties = coll + .mass_properties() + .transform_by(coll.position_wrt_parent()); self.mass_properties -= mass_properties; self.update_world_mass_properties(); } @@ -189,7 +193,7 @@ impl RigidBody { } fn integrate_velocity(&self, dt: f32) -> Isometry { - let com = &self.position * self.mass_properties.local_com; // FIXME: use non-origin center of masses. + let com = &self.position * self.mass_properties.local_com; let shift = Translation::from(com.coords); shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse() } diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 01c78ac..aed76c8 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -150,6 +150,7 @@ impl Collider { } /// The position of this collider expressed in the local-space of the rigid-body it is attached to. + #[deprecated(note = "use `.position_wrt_parent()` instead.")] pub fn delta(&self) -> &Isometry { &self.delta } diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs index 2cda3e0..7e235c2 100644 --- a/src/geometry/contact.rs +++ b/src/geometry/contact.rs @@ -360,8 +360,8 @@ impl ContactManifold { pair, (subshape1, subshape2), BodyPair::new(coll1.parent, coll2.parent), - *coll1.delta(), - *coll2.delta(), + *coll1.position_wrt_parent(), + *coll2.position_wrt_parent(), (coll1.friction + coll2.friction) * 0.5, (coll1.restitution + coll2.restitution) * 0.5, ) diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs index f66d987..c9ff2d1 100644 --- a/src_testbed/nphysics_backend.rs +++ b/src_testbed/nphysics_backend.rs @@ -165,7 +165,7 @@ impl NPhysicsWorld { for coll_handle in rb.colliders() { let collider = &mut colliders[*coll_handle]; - collider.set_position_debug(pos * collider.delta()); + collider.set_position_debug(pos * collider.position_wrt_parent()); } } } diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs index 045e697..66036c8 100644 --- a/src_testbed/physx_backend.rs +++ b/src_testbed/physx_backend.rs @@ -225,6 +225,28 @@ 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) { + let densities: Vec<_> = rb + .colliders() + .iter() + .map(|h| colliders[*h].density()) + .collect(); + + unsafe { + physx_sys::PxRigidBodyExt_updateMassAndInertia_mut( + ra.as_ptr_mut().ptr as *mut physx_sys::PxRigidBody, + densities.as_ptr(), + densities.len() as u32, + std::ptr::null(), + false, + ); + } + } + } + let mut res = Self { physics, cooking, @@ -390,7 +412,7 @@ impl PhysxWorld { for coll_handle in rb.colliders() { let collider = &mut colliders[*coll_handle]; - collider.set_position_debug(iso * collider.delta()); + collider.set_position_debug(iso * collider.position_wrt_parent()); } } } diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 868bd91..450170e 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -791,6 +791,29 @@ impl Testbed { .state .action_flags .set(TestbedActionFlags::EXAMPLE_CHANGED, true), + WindowEvent::Key(Key::C, Action::Release, _) => { + // Delete 1 collider of 10% of the remaining dynamic bodies. + let mut colliders: Vec<_> = self + .physics + .bodies + .iter() + .filter(|e| e.1.is_dynamic()) + .filter(|e| !e.1.colliders().is_empty()) + .map(|e| e.1.colliders().to_vec()) + .collect(); + colliders.sort_by_key(|co| -(co.len() as isize)); + + let num_to_delete = (colliders.len() / 10).max(1); + for to_delete in &colliders[..num_to_delete] { + self.physics.pipeline.remove_collider( + to_delete[0], + &mut self.physics.broad_phase, + &mut self.physics.narrow_phase, + &mut self.physics.bodies, + &mut self.physics.colliders, + ); + } + } WindowEvent::Key(Key::D, Action::Release, _) => { // Delete 10% of the remaining dynamic bodies. let dynamic_bodies: Vec<_> = self @@ -1539,7 +1562,7 @@ impl State for Testbed { } if self.state.flags.contains(TestbedStateFlags::CONTACT_POINTS) { - draw_contacts(window, &self.physics.narrow_phase, &self.physics.bodies); + draw_contacts(window, &self.physics.narrow_phase, &self.physics.colliders); } if self.state.running == RunMode::Step { @@ -1634,7 +1657,7 @@ Hashes at frame: {} } } -fn draw_contacts(window: &mut Window, nf: &NarrowPhase, bodies: &RigidBodySet) { +fn draw_contacts(window: &mut Window, nf: &NarrowPhase, colliders: &ColliderSet) { for (_, _, pair) in nf.contact_graph().interaction_pairs() { for manifold in &pair.manifolds { for pt in manifold.all_contacts() { @@ -1643,8 +1666,8 @@ fn draw_contacts(window: &mut Window, nf: &NarrowPhase, bodies: &RigidBodySet) { } else { Point3::new(1.0, 0.0, 0.0) }; - let pos1 = bodies[manifold.body_pair.body1].position; - let pos2 = bodies[manifold.body_pair.body2].position; + let pos1 = colliders[manifold.pair.collider1].position(); + let pos2 = colliders[manifold.pair.collider2].position(); let start = pos1 * pt.local_p1; let end = pos2 * pt.local_p2; let n = pos1 * manifold.local_n1; From fc0b3bf39484029d956055943b38bb55ab2c5791 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Tue, 1 Sep 2020 17:35:32 +0200 Subject: [PATCH 08/15] 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 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 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(), From 1b7e343266c6012672c57b12e27b3a31d6d27eb1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Tue, 1 Sep 2020 17:39:49 +0200 Subject: [PATCH 09/15] Add CI. --- .circleci/config.yml | 102 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 102 insertions(+) create mode 100644 .circleci/config.yml diff --git a/.circleci/config.yml b/.circleci/config.yml new file mode 100644 index 0000000..315c77f --- /dev/null +++ b/.circleci/config.yml @@ -0,0 +1,102 @@ +version: 2.1 + +executors: + rust-executor: + docker: + - image: rust:latest + +jobs: + check-fmt: + executor: rust-executor + steps: + - checkout + - run: + name: install rustfmt + command: rustup component add rustfmt + - run: + name: check formatting + command: cargo fmt -- --check + build-native: + executor: rust-executor + steps: + - checkout + - run: + name: build rapier2d + command: cargo build --verbose -p rapier2d; + - run: + name: build rapier3d + command: cargo build --verbose -p rapier3d; + - run: + name: build rapier2d SIMD + command: cd build/rapier2d; cargo build --verbose --features simd-stable; + - run: + name: build rapier3d SIMD + command: cd build/rapier3d; cargo build --verbose --features simd-stable; + - run: + name: build rapier2d SIMD Paallel + command: cd build/rapier2d; cargo build --verbose --features simd-stable --features parallel; + - run: + name: build rapier3d SIMD Paallel + command: cd build/rapier3d; cargo build --verbose --features simd-stable --features parallel; + - run: + name: test rapier2d + command: cargo test --verbose -p rapier2d; + - run: + name: test rapier3d + command: cargo test --verbose -p rapier3d; + - run: + name: check rapier_testbed2d + command: cargo check --verbose -p rapier_testbed2d; + - run: + name: check rapier_testbed3d + command: cargo check --verbose -p rapier_testbed3d; + - run: + name: check rapier-examples-2d + command: cargo check -j 1 --verbose -p rapier-examples-2d; + - run: + name: check rapier-examples-3d + command: cargo check -j 1 --verbose -p rapier-examples-3d; + - run: + name: check rapier_testbed2d with fluids + command: cd build/rapier_testbed2d && cargo check --verbose --features=fluids; + - run: + name: check rapier_testbed3d with fluids + command: cd build/rapier_testbed3d && cargo check --verbose --features=fluids; + build-wasm: + executor: rust-executor + steps: + - checkout + - run: + name: install cargo-web + command: cargo install -f cargo-web; + - run: + name: build rapier2d + command: cd build/rapier2d && cargo web build --verbose --target wasm32-unknown-unknown; + - run: + name: build rapier3d + command: cd build/rapier3d && cargo web build --verbose --target wasm32-unknown-unknown; + - run: + name: build rapier-examples-2d + command: cd examples2d && cargo web build --verbose --target wasm32-unknown-unknown; + - run: + name: build rapier-examples-3d + command: cd examples3d && cargo web build --verbose --target wasm32-unknown-unknown; + - run: + name: build rapier_testbed2d + command: cd build/rapier_testbed2d && cargo web build --verbose --target wasm32-unknown-unknown; + - run: + name: build rapier_testbed3d + command: cd build/rapier_testbed3d && cargo web build --verbose --target wasm32-unknown-unknown; + + +workflows: + version: 2 + build: + jobs: + - check-fmt + - build-native: + requires: + - check-fmt + - build-wasm: + requires: + - check-fmt From 939e569491ca81d8deaca6f13119490c439fc9bf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Tue, 1 Sep 2020 17:47:21 +0200 Subject: [PATCH 10/15] Take local inertial frame into accound for abs comparison of MassProperties. --- src/dynamics/mass_properties.rs | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs index 586eaaf..22e7da5 100644 --- a/src/dynamics/mass_properties.rs +++ b/src/dynamics/mass_properties.rs @@ -300,14 +300,22 @@ impl approx::AbsDiffEq for MassProperties { } fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool { - self.local_com.abs_diff_eq(&other.local_com, epsilon) + #[cfg(feature = "dim2")] + let inertia_is_ok = self + .inv_principal_inertia_sqrt + .abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon); + + #[cfg(feature = "dim3")] + let inertia_is_ok = self + .reconstruct_inverse_inertia_matrix() + .abs_diff_eq(&other.reconstruct_inverse_inertia_matrix(), epsilon); + + inertia_is_ok + && self.local_com.abs_diff_eq(&other.local_com, epsilon) && self.inv_mass.abs_diff_eq(&other.inv_mass, epsilon) && self .inv_principal_inertia_sqrt .abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon) - // && self - // .principal_inertia_local_frame - // .abs_diff_eq(&other.principal_inertia_local_frame, epsilon) } } From ff3ae6a7e004ff68a546832e43fff809f3352cf6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Tue, 1 Sep 2020 17:48:51 +0200 Subject: [PATCH 11/15] Run cargofmt + add rustfmt.toml --- rustfmt.toml | 3 +++ .../contact_generator/ball_polygon_contact_generator.rs | 1 + .../contact_generator/cuboid_polygon_contact_generator.rs | 1 + .../proximity_detector/ball_polygon_proximity_detector.rs | 1 + .../proximity_detector/cuboid_polygon_proximity_detector.rs | 1 + 5 files changed, 7 insertions(+) create mode 100644 rustfmt.toml diff --git a/rustfmt.toml b/rustfmt.toml new file mode 100644 index 0000000..a71ae5f --- /dev/null +++ b/rustfmt.toml @@ -0,0 +1,3 @@ +#indent_style = "Block" +#where_single_line = true +#brace_style = "PreferSameLine" \ No newline at end of file diff --git a/src/geometry/contact_generator/ball_polygon_contact_generator.rs b/src/geometry/contact_generator/ball_polygon_contact_generator.rs index e69de29..8b13789 100644 --- a/src/geometry/contact_generator/ball_polygon_contact_generator.rs +++ b/src/geometry/contact_generator/ball_polygon_contact_generator.rs @@ -0,0 +1 @@ + diff --git a/src/geometry/contact_generator/cuboid_polygon_contact_generator.rs b/src/geometry/contact_generator/cuboid_polygon_contact_generator.rs index e69de29..8b13789 100644 --- a/src/geometry/contact_generator/cuboid_polygon_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_polygon_contact_generator.rs @@ -0,0 +1 @@ + diff --git a/src/geometry/proximity_detector/ball_polygon_proximity_detector.rs b/src/geometry/proximity_detector/ball_polygon_proximity_detector.rs index e69de29..8b13789 100644 --- a/src/geometry/proximity_detector/ball_polygon_proximity_detector.rs +++ b/src/geometry/proximity_detector/ball_polygon_proximity_detector.rs @@ -0,0 +1 @@ + diff --git a/src/geometry/proximity_detector/cuboid_polygon_proximity_detector.rs b/src/geometry/proximity_detector/cuboid_polygon_proximity_detector.rs index e69de29..8b13789 100644 --- a/src/geometry/proximity_detector/cuboid_polygon_proximity_detector.rs +++ b/src/geometry/proximity_detector/cuboid_polygon_proximity_detector.rs @@ -0,0 +1 @@ + From d2bc2779c9462ec94c1b55960bccb12e4f80d1c2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Tue, 1 Sep 2020 17:55:14 +0200 Subject: [PATCH 12/15] CI: fix test execution. --- .circleci/config.yml | 11 ++++------- examples2d/Cargo.toml | 2 +- examples3d/Cargo.toml | 2 +- 3 files changed, 6 insertions(+), 9 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index 315c77f..74c63ca 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,17 +33,14 @@ jobs: name: build rapier3d SIMD command: cd build/rapier3d; cargo build --verbose --features simd-stable; - run: - name: build rapier2d SIMD Paallel + name: build rapier2d SIMD Parallel command: cd build/rapier2d; cargo build --verbose --features simd-stable --features parallel; - run: - name: build rapier3d SIMD Paallel + name: build rapier3d SIMD Parallel command: cd build/rapier3d; cargo build --verbose --features simd-stable --features parallel; - run: - name: test rapier2d - command: cargo test --verbose -p rapier2d; - - run: - name: test rapier3d - command: cargo test --verbose -p rapier3d; + name: test + command: cargo test - run: name: check rapier_testbed2d command: cargo check --verbose -p rapier_testbed2d; diff --git a/examples2d/Cargo.toml b/examples2d/Cargo.toml index 7e97948..ad63958 100644 --- a/examples2d/Cargo.toml +++ b/examples2d/Cargo.toml @@ -1,5 +1,5 @@ [package] -name = "nphysics-examples-2d" +name = "rapier-examples-2d" version = "0.1.0" authors = [ "Sébastien Crozet " ] edition = "2018" diff --git a/examples3d/Cargo.toml b/examples3d/Cargo.toml index 8091db8..efc3cce 100644 --- a/examples3d/Cargo.toml +++ b/examples3d/Cargo.toml @@ -1,5 +1,5 @@ [package] -name = "nphysics-examples-3d" +name = "rapier-examples-3d" version = "0.1.0" authors = [ "Sébastien Crozet " ] edition = "2018" From a1a34dada22343c93b4ce3eac404e52d592b3514 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Tue, 1 Sep 2020 17:56:28 +0200 Subject: [PATCH 13/15] CI: build for WASM with the wasm-bindgen feature enabled. --- .circleci/config.yml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index 74c63ca..98524a8 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -68,22 +68,22 @@ jobs: command: cargo install -f cargo-web; - run: name: build rapier2d - command: cd build/rapier2d && cargo web build --verbose --target wasm32-unknown-unknown; + command: cd build/rapier2d && cargo web build --verbose --features wasm-bindgen --target wasm32-unknown-unknown; - run: name: build rapier3d - command: cd build/rapier3d && cargo web build --verbose --target wasm32-unknown-unknown; + command: cd build/rapier3d && cargo web build --verbose --features wasm-bindgen --target wasm32-unknown-unknown; - run: name: build rapier-examples-2d - command: cd examples2d && cargo web build --verbose --target wasm32-unknown-unknown; + command: cd examples2d && cargo web build --verbose --features wasm-bindgen --target wasm32-unknown-unknown; - run: name: build rapier-examples-3d - command: cd examples3d && cargo web build --verbose --target wasm32-unknown-unknown; + command: cd examples3d && cargo web build --verbose --features wasm-bindgen --target wasm32-unknown-unknown; - run: name: build rapier_testbed2d - command: cd build/rapier_testbed2d && cargo web build --verbose --target wasm32-unknown-unknown; + command: cd build/rapier_testbed2d && cargo web build --features wasm-bindgen --verbose --target wasm32-unknown-unknown; - run: name: build rapier_testbed3d - command: cd build/rapier_testbed3d && cargo web build --verbose --target wasm32-unknown-unknown; + command: cd build/rapier_testbed3d && cargo web build --features wasm-bindgen --verbose --target wasm32-unknown-unknown; workflows: From 221787c978e9105a7e1505c9ea702ed670f1441a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Tue, 1 Sep 2020 18:08:53 +0200 Subject: [PATCH 14/15] CI: install XCB dependencies. --- .circleci/config.yml | 15 ++------------- 1 file changed, 2 insertions(+), 13 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index 98524a8..a2414f7 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -20,6 +20,8 @@ jobs: executor: rust-executor steps: - checkout + - run: apt-get update + - run: apt-get install -y cmake libxcb-composite0-dev - run: name: build rapier2d command: cargo build --verbose -p rapier2d; @@ -72,19 +74,6 @@ jobs: - run: name: build rapier3d command: cd build/rapier3d && cargo web build --verbose --features wasm-bindgen --target wasm32-unknown-unknown; - - run: - name: build rapier-examples-2d - command: cd examples2d && cargo web build --verbose --features wasm-bindgen --target wasm32-unknown-unknown; - - run: - name: build rapier-examples-3d - command: cd examples3d && cargo web build --verbose --features wasm-bindgen --target wasm32-unknown-unknown; - - run: - name: build rapier_testbed2d - command: cd build/rapier_testbed2d && cargo web build --features wasm-bindgen --verbose --target wasm32-unknown-unknown; - - run: - name: build rapier_testbed3d - command: cd build/rapier_testbed3d && cargo web build --features wasm-bindgen --verbose --target wasm32-unknown-unknown; - workflows: version: 2 From 763b9092422fd5677ffd47ec1b081951dc1c63e4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Tue, 1 Sep 2020 18:14:48 +0200 Subject: [PATCH 15/15] CI: remove use of the nonexistent "fluid" feature. --- .circleci/config.yml | 6 ------ 1 file changed, 6 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index a2414f7..c0722a9 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -55,12 +55,6 @@ jobs: - run: name: check rapier-examples-3d command: cargo check -j 1 --verbose -p rapier-examples-3d; - - run: - name: check rapier_testbed2d with fluids - command: cd build/rapier_testbed2d && cargo check --verbose --features=fluids; - - run: - name: check rapier_testbed3d with fluids - command: cd build/rapier_testbed3d && cargo check --verbose --features=fluids; build-wasm: executor: rust-executor steps: