From df2156ffd02ea1b8c86e86f1d68c5e4e915e6d98 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Mon, 31 Aug 2020 17:58:14 +0200 Subject: 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, -- cgit From c286f44c4e2aa1692781e9c4c7c1ce2bacdd2415 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Mon, 31 Aug 2020 19:02:37 +0200 Subject: 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); -- cgit From 5731b994634661f403cf589fdf1337d0184c7d8e Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Mon, 31 Aug 2020 19:03:15 +0200 Subject: Fix box-box CD when colliders have non-identity delta pos. 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; -- cgit From ce26fe107727ee4ef25ee728b3bb3a40914fdc99 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Mon, 31 Aug 2020 19:03:56 +0200 Subject: 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()) } -- cgit From 03b437f278bbcbd391acd23a4d8fa074915eb00c Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Mon, 31 Aug 2020 19:04:32 +0200 Subject: 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; -- cgit From 9622827dc6aadb391512b95381edb1efc26b1b90 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Tue, 1 Sep 2020 14:02:59 +0200 Subject: Fix constraints resolution with non-identity relative collider position. --- src/dynamics/solver/position_constraint.rs | 4 +-- src/dynamics/solver/position_constraint_wide.rs | 7 ++-- src/dynamics/solver/position_ground_constraint.rs | 28 ++++++++++------ .../solver/position_ground_constraint_wide.rs | 16 ++++++--- src/dynamics/solver/velocity_constraint.rs | 8 +++-- src/dynamics/solver/velocity_constraint_wide.rs | 18 ++++++---- src/dynamics/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}; -- cgit From 2f2a073ce47eaa17f44d88b9dc6cc56362c374e2 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Tue, 1 Sep 2020 17:05:24 +0200 Subject: 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 { +