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. --- src/geometry/broad_phase_multi_sap.rs | 4 ++-- src/geometry/collider.rs | 41 ++++++++++++++++++++++++++++++++++- src/geometry/collider_set.rs | 6 ++--- 3 files changed, 44 insertions(+), 7 deletions(-) (limited to 'src/geometry') 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 } -- 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(-) (limited to 'src/geometry') 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 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. --- src/geometry/narrow_phase.rs | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) (limited to 'src/geometry') 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/geometry/contact.rs | 24 +++++++++++--- .../cuboid_cuboid_contact_generator.rs | 38 ++++++++-------------- 2 files changed, 33 insertions(+), 29 deletions(-) (limited to 'src/geometry') 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; -- 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/geometry/collider.rs | 1 + src/geometry/contact.rs | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) (limited to 'src/geometry') 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, ) -- cgit From ff3ae6a7e004ff68a546832e43fff809f3352cf6 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Tue, 1 Sep 2020 17:48:51 +0200 Subject: Run cargofmt + add rustfmt.toml --- src/geometry/contact_generator/ball_polygon_contact_generator.rs | 1 + src/geometry/contact_generator/cuboid_polygon_contact_generator.rs | 1 + src/geometry/proximity_detector/ball_polygon_proximity_detector.rs | 1 + src/geometry/proximity_detector/cuboid_polygon_proximity_detector.rs | 1 + 4 files changed, 4 insertions(+) (limited to 'src/geometry') 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 @@ + -- cgit