From 1bef66fea941307a7305ddaebdb0abe3d0cb281f Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 25 May 2021 11:00:13 +0200 Subject: Add prelude + use vectors for setting linvel/translation in builders --- src/dynamics/integration_parameters.rs | 13 +- src/dynamics/joint/fixed_joint.rs | 10 +- src/dynamics/rigid_body.rs | 174 ++++++++++++++++++--- .../joint_constraint/fixed_position_constraint.rs | 34 ++-- .../joint_constraint/fixed_velocity_constraint.rs | 12 +- .../fixed_velocity_constraint_wide.rs | 24 +-- 6 files changed, 189 insertions(+), 78 deletions(-) (limited to 'src/dynamics') diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index e039bfc..4725319 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -10,7 +10,7 @@ pub struct IntegrationParameters { /// /// When CCD with multiple substeps is enabled, the timestep is subdivided /// into smaller pieces. This timestep subdivision won't generate timestep - /// lengths smaller than `min_dt`. + /// lengths smaller than `min_ccd_dt`. /// /// Setting this to a large value will reduce the opportunity to performing /// CCD substepping, resulting in potentially more time dropped by the @@ -18,17 +18,6 @@ pub struct IntegrationParameters { /// to numerical instabilities. pub min_ccd_dt: Real, - // /// If `true` and if rapier is compiled with the `parallel` feature, this will enable rayon-based multithreading (default: `true`). - // /// - // /// This parameter is ignored if rapier is not compiled with is `parallel` feature. - // /// Refer to rayon's documentation regarding how to configure the number of threads with either - // /// `rayon::ThreadPoolBuilder::new().num_threads(4).build_global().unwrap()` or `ThreadPool::install`. - // /// Note that using only one thread with `multithreading_enabled` set to `true` will result on a slower - // /// simulation than setting `multithreading_enabled` to `false`. - // pub multithreading_enabled: bool, - // /// If `true`, the world's `step` method will stop right after resolving exactly one CCD event (default: `false`). - // /// This allows the user to take action during a timestep, in-between two CCD events. - // pub return_after_ccd_substep: bool, /// The Error Reduction Parameter in `[0, 1]` is the proportion of /// the positional error to be corrected at each time step (default: `0.2`). pub erp: Real, diff --git a/src/dynamics/joint/fixed_joint.rs b/src/dynamics/joint/fixed_joint.rs index 2917757..3f87e8d 100644 --- a/src/dynamics/joint/fixed_joint.rs +++ b/src/dynamics/joint/fixed_joint.rs @@ -8,10 +8,10 @@ use crate::math::{Isometry, Real, SpacialVector}; pub struct FixedJoint { /// The frame of reference for the first body affected by this joint, expressed in the local frame /// of the first body. - pub local_anchor1: Isometry, + pub local_frame1: Isometry, /// The frame of reference for the second body affected by this joint, expressed in the local frame /// of the first body. - pub local_anchor2: Isometry, + pub local_frame2: Isometry, /// The impulse applied to the first body affected by this joint. /// /// The impulse applied to the second body affected by this joint is given by `-impulse`. @@ -23,10 +23,10 @@ pub struct FixedJoint { impl FixedJoint { /// Creates a new fixed joint from the frames of reference of both bodies. - pub fn new(local_anchor1: Isometry, local_anchor2: Isometry) -> Self { + pub fn new(local_frame1: Isometry, local_frame2: Isometry) -> Self { Self { - local_anchor1, - local_anchor2, + local_frame1, + local_frame2, impulse: SpacialVector::zeros(), } } diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 824ec92..d53ff98 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -124,6 +124,72 @@ impl RigidBody { self.rb_dominance.effective_group(&self.rb_type) } + #[inline] + /// Locks or unlocks all the rotations of this rigid-body. + pub fn lock_rotations(&mut self, locked: bool, wake_up: bool) { + if self.is_dynamic() { + if wake_up { + self.wake_up(true); + } + + self.rb_mprops + .flags + .set(RigidBodyMassPropsFlags::ROTATION_LOCKED_X, locked); + self.rb_mprops + .flags + .set(RigidBodyMassPropsFlags::ROTATION_LOCKED_Y, locked); + self.rb_mprops + .flags + .set(RigidBodyMassPropsFlags::ROTATION_LOCKED_Z, locked); + self.update_world_mass_properties(); + } + } + + #[inline] + /// Locks or unlocks rotations of this rigid-body along each cartesian axes. + pub fn restrict_rotations( + &mut self, + allow_rotations_x: bool, + allow_rotations_y: bool, + allow_rotations_z: bool, + wake_up: bool, + ) { + if self.is_dynamic() { + if wake_up { + self.wake_up(true); + } + + self.rb_mprops.flags.set( + RigidBodyMassPropsFlags::ROTATION_LOCKED_X, + allow_rotations_x, + ); + self.rb_mprops.flags.set( + RigidBodyMassPropsFlags::ROTATION_LOCKED_Y, + allow_rotations_y, + ); + self.rb_mprops.flags.set( + RigidBodyMassPropsFlags::ROTATION_LOCKED_Z, + allow_rotations_z, + ); + self.update_world_mass_properties(); + } + } + + #[inline] + /// Locks or unlocks all the rotations of this rigid-body. + pub fn lock_translations(&mut self, locked: bool, wake_up: bool) { + if self.is_dynamic() { + if wake_up { + self.wake_up(true); + } + + self.rb_mprops + .flags + .set(RigidBodyMassPropsFlags::TRANSLATION_LOCKED, locked); + self.update_world_mass_properties(); + } + } + /// Are the translations of this rigid-body locked? pub fn is_translation_locked(&self) -> bool { self.rb_mprops @@ -251,6 +317,16 @@ impl RigidBody { self.rb_forces.gravity_scale = scale; } + /// The dominance group of this rigid-body. + pub fn dominance_group(&self) -> i8 { + self.rb_dominance.0 + } + + /// The dominance group of this rigid-body. + pub fn set_dominance_group(&mut self, dominance: i8) { + self.rb_dominance.0 = dominance + } + /// Adds a collider to this rigid-body. // TODO ECS: we keep this public for now just to simply our experiments on bevy_rapier. pub fn add_collider( @@ -279,9 +355,10 @@ impl RigidBody { if let Some(i) = self.rb_colliders.0.iter().position(|e| *e == handle) { self.changes.set(RigidBodyChanges::COLLIDERS, true); self.rb_colliders.0.swap_remove(i); + let mass_properties = coll .mass_properties() - .transform_by(coll.position_wrt_parent()); + .transform_by(coll.position_wrt_parent().unwrap()); self.rb_mprops.mass_properties -= mass_properties; self.update_world_mass_properties(); } @@ -384,6 +461,45 @@ impl RigidBody { &self.rb_pos.position } + /// The translational part of this rigid-body's position. + #[inline] + pub fn translation(&self) -> Vector { + self.rb_pos.position.translation.vector + } + + /// Sets the translational part of this rigid-body's position. + #[inline] + pub fn set_translation(&mut self, translation: Vector, wake_up: bool) { + self.changes.insert(RigidBodyChanges::POSITION); + self.rb_pos.position.translation.vector = translation; + self.rb_pos.next_position.translation.vector = translation; + + // TODO: Do we really need to check that the body isn't dynamic? + if wake_up && self.is_dynamic() { + self.wake_up(true) + } + } + + /// The translational part of this rigid-body's position. + #[inline] + pub fn rotation(&self) -> Rotation { + self.rb_pos.position.rotation + } + + /// Sets the rotational part of this rigid-body's position. + #[inline] + pub fn set_rotation(&mut self, rotation: AngVector, wake_up: bool) { + self.changes.insert(RigidBodyChanges::POSITION); + let rotation = Rotation::new(rotation); + self.rb_pos.position.rotation = rotation; + self.rb_pos.next_position.rotation = rotation; + + // TODO: Do we really need to check that the body isn't dynamic? + if wake_up && self.is_dynamic() { + self.wake_up(true) + } + } + /// Sets the position and `next_kinematic_position` of this rigid body. /// /// This will teleport the rigid-body to the specified position/orientation, @@ -404,6 +520,20 @@ impl RigidBody { } } + /// If this rigid body is kinematic, sets its future translation after the next timestep integration. + pub fn set_next_kinematic_rotation(&mut self, rotation: AngVector) { + if self.is_kinematic() { + self.rb_pos.next_position.rotation = Rotation::new(rotation); + } + } + + /// If this rigid body is kinematic, sets its future orientation after the next timestep integration. + pub fn set_next_kinematic_translation(&mut self, rotation: Rotation) { + if self.is_kinematic() { + self.rb_pos.next_position.rotation = rotation; + } + } + /// If this rigid body is kinematic, sets its future position after the next timestep integration. pub fn set_next_kinematic_position(&mut self, pos: Isometry) { if self.is_kinematic() { @@ -411,6 +541,17 @@ impl RigidBody { } } + /// Predicts the next position of this rigid-body, by integrating its velocity and forces + /// by a time of `dt`. + pub fn predict_position_using_velocity_and_forces(&self, dt: Real) -> Isometry { + self.rb_pos.integrate_forces_and_velocities( + dt, + &self.rb_forces, + &self.rb_vels, + &self.rb_mprops, + ) + } + pub(crate) fn update_world_mass_properties(&mut self) { self.rb_mprops .update_world_mass_properties(&self.rb_pos.position); @@ -618,8 +759,8 @@ impl RigidBodyBuilder { } /// Sets the scale applied to the gravity force affecting the rigid-body to be created. - pub fn gravity_scale(mut self, x: Real) -> Self { - self.gravity_scale = x; + pub fn gravity_scale(mut self, scale_factor: Real) -> Self { + self.gravity_scale = scale_factor; self } @@ -630,19 +771,8 @@ impl RigidBodyBuilder { } /// Sets the initial translation of the rigid-body to be created. - #[cfg(feature = "dim2")] - pub fn translation(mut self, x: Real, y: Real) -> Self { - self.position.translation.x = x; - self.position.translation.y = y; - self - } - - /// Sets the initial translation of the rigid-body to be created. - #[cfg(feature = "dim3")] - pub fn translation(mut self, x: Real, y: Real, z: Real) -> Self { - self.position.translation.x = x; - self.position.translation.y = y; - self.position.translation.z = z; + pub fn translation(mut self, translation: Vector) -> Self { + self.position.translation.vector = translation; self } @@ -811,16 +941,8 @@ impl RigidBodyBuilder { } /// Sets the initial linear velocity of the rigid-body to be created. - #[cfg(feature = "dim2")] - pub fn linvel(mut self, x: Real, y: Real) -> Self { - self.linvel = Vector::new(x, y); - self - } - - /// Sets the initial linear velocity of the rigid-body to be created. - #[cfg(feature = "dim3")] - pub fn linvel(mut self, x: Real, y: Real, z: Real) -> Self { - self.linvel = Vector::new(x, y, z); + pub fn linvel(mut self, linvel: Vector) -> Self { + self.linvel = linvel; self } diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs index 239138f..f8945c3 100644 --- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs @@ -8,8 +8,8 @@ use crate::utils::WAngularInertia; pub(crate) struct FixedPositionConstraint { position1: usize, position2: usize, - local_anchor1: Isometry, - local_anchor2: Isometry, + local_frame1: Isometry, + local_frame2: Isometry, local_com1: Point, local_com2: Point, im1: Real, @@ -38,8 +38,8 @@ impl FixedPositionConstraint { let ang_inv_lhs = (ii1 + ii2).inverse(); Self { - local_anchor1: cparams.local_anchor1, - local_anchor2: cparams.local_anchor2, + local_frame1: cparams.local_frame1, + local_frame2: cparams.local_frame2, position1: ids1.active_set_offset, position2: ids2.active_set_offset, im1, @@ -58,8 +58,8 @@ impl FixedPositionConstraint { let mut position2 = positions[self.position2 as usize]; // Angular correction. - let anchor1 = position1 * self.local_anchor1; - let anchor2 = position2 * self.local_anchor2; + let anchor1 = position1 * self.local_frame1; + let anchor2 = position2 * self.local_frame2; let ang_err = anchor2.rotation * anchor1.rotation.inverse(); #[cfg(feature = "dim3")] let ang_impulse = self @@ -75,8 +75,8 @@ impl FixedPositionConstraint { Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation; // Linear correction. - let anchor1 = position1 * Point::from(self.local_anchor1.translation.vector); - let anchor2 = position2 * Point::from(self.local_anchor2.translation.vector); + let anchor1 = position1 * Point::from(self.local_frame1.translation.vector); + let anchor2 = position2 * Point::from(self.local_frame2.translation.vector); let err = anchor2 - anchor1; let impulse = err * (self.lin_inv_lhs * params.joint_erp); position1.translation.vector += self.im1 * impulse; @@ -91,7 +91,7 @@ impl FixedPositionConstraint { pub(crate) struct FixedPositionGroundConstraint { position2: usize, anchor1: Isometry, - local_anchor2: Isometry, + local_frame2: Isometry, local_com2: Point, im2: Real, ii2: AngularInertia, @@ -109,19 +109,19 @@ impl FixedPositionGroundConstraint { let (mprops2, ids2) = rb2; let anchor1; - let local_anchor2; + let local_frame2; if flipped { - anchor1 = poss1.next_position * cparams.local_anchor2; - local_anchor2 = cparams.local_anchor1; + anchor1 = poss1.next_position * cparams.local_frame2; + local_frame2 = cparams.local_frame1; } else { - anchor1 = poss1.next_position * cparams.local_anchor1; - local_anchor2 = cparams.local_anchor2; + anchor1 = poss1.next_position * cparams.local_frame1; + local_frame2 = cparams.local_frame2; }; Self { anchor1, - local_anchor2, + local_frame2, position2: ids2.active_set_offset, im2: mprops2.effective_inv_mass, ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), @@ -134,13 +134,13 @@ impl FixedPositionGroundConstraint { let mut position2 = positions[self.position2 as usize]; // Angular correction. - let anchor2 = position2 * self.local_anchor2; + let anchor2 = position2 * self.local_frame2; let ang_err = anchor2.rotation * self.anchor1.rotation.inverse(); position2.rotation = ang_err.powf(-params.joint_erp) * position2.rotation; // Linear correction. let anchor1 = Point::from(self.anchor1.translation.vector); - let anchor2 = position2 * Point::from(self.local_anchor2.translation.vector); + let anchor2 = position2 * Point::from(self.local_frame2.translation.vector); let err = anchor2 - anchor1; // NOTE: no need to divide by im2 just to multiply right after. let impulse = err * params.joint_erp; diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs index a0c0739..8bfc1a6 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs @@ -63,8 +63,8 @@ impl FixedVelocityConstraint { let (poss1, vels1, mprops1, ids1) = rb1; let (poss2, vels2, mprops2, ids2) = rb2; - let anchor1 = poss1.position * cparams.local_anchor1; - let anchor2 = poss2.position * cparams.local_anchor2; + let anchor1 = poss1.position * cparams.local_frame1; + let anchor2 = poss2.position * cparams.local_frame2; let im1 = mprops1.effective_inv_mass; let im2 = mprops2.effective_inv_mass; let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); @@ -280,13 +280,13 @@ impl FixedVelocityGroundConstraint { let (anchor1, anchor2) = if flipped { ( - poss1.position * cparams.local_anchor2, - poss2.position * cparams.local_anchor1, + poss1.position * cparams.local_frame2, + poss2.position * cparams.local_frame1, ) } else { ( - poss1.position * cparams.local_anchor1, - poss2.position * cparams.local_anchor2, + poss1.position * cparams.local_frame1, + poss2.position * cparams.local_frame2, ) }; diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs index 84e1fca..0421d49 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs @@ -91,12 +91,12 @@ impl WFixedVelocityConstraint { ]); let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; - let local_anchor1 = Isometry::from(gather![|ii| cparams[ii].local_anchor1]); - let local_anchor2 = Isometry::from(gather![|ii| cparams[ii].local_anchor2]); + let local_frame1 = Isometry::from(gather![|ii| cparams[ii].local_frame1]); + let local_frame2 = Isometry::from(gather![|ii| cparams[ii].local_frame2]); let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]); - let anchor1 = position1 * local_anchor1; - let anchor2 = position2 * local_anchor2; + let anchor1 = position1 * local_frame1; + let anchor2 = position2 * local_frame2; let ii1 = ii1_sqrt.squared(); let ii2 = ii2_sqrt.squared(); let r1 = anchor1.translation.vector - world_com1.coords; @@ -363,20 +363,20 @@ impl WFixedVelocityGroundConstraint { ]); let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; - let local_anchor1 = Isometry::from(gather![|ii| if flipped[ii] { - cparams[ii].local_anchor2 + let local_frame1 = Isometry::from(gather![|ii| if flipped[ii] { + cparams[ii].local_frame2 } else { - cparams[ii].local_anchor1 + cparams[ii].local_frame1 }]); - let local_anchor2 = Isometry::from(gather![|ii| if flipped[ii] { - cparams[ii].local_anchor1 + let local_frame2 = Isometry::from(gather![|ii| if flipped[ii] { + cparams[ii].local_frame1 } else { - cparams[ii].local_anchor2 + cparams[ii].local_frame2 }]); let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]); - let anchor1 = position1 * local_anchor1; - let anchor2 = position2 * local_anchor2; + let anchor1 = position1 * local_frame1; + let anchor2 = position2 * local_frame2; let ii2 = ii2_sqrt.squared(); let r1 = anchor1.translation.vector - world_com1.coords; let r2 = anchor2.translation.vector - world_com2.coords; -- cgit From 826ce5f014281fd04b7a18238f102f2591d0b255 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 1 Jun 2021 12:36:01 +0200 Subject: Rework the event system --- src/dynamics/ccd/ccd_solver.rs | 30 +++++++++----- src/dynamics/ccd/toi_entry.rs | 2 +- src/dynamics/island_manager.rs | 22 +++++------ src/dynamics/rigid_body.rs | 27 ++++++------- src/dynamics/rigid_body_components.rs | 46 +++++++++++++++------- src/dynamics/solver/island_solver.rs | 4 +- .../joint_constraint/ball_position_constraint.rs | 8 ++-- .../ball_position_constraint_wide.rs | 6 +-- .../joint_constraint/fixed_position_constraint.rs | 6 +-- .../generic_position_constraint.rs | 6 +-- .../revolute_position_constraint.rs | 6 +-- src/dynamics/solver/parallel_island_solver.rs | 2 +- 12 files changed, 95 insertions(+), 70 deletions(-) (limited to 'src/dynamics') diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index b348283..354f183 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -10,6 +10,7 @@ use crate::geometry::{ use crate::math::Real; use crate::parry::utils::SortedPair; use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode}; +use crate::prelude::{ActiveEvents, ColliderFlags}; use parry::query::{DefaultQueryDispatcher, QueryDispatcher}; use parry::utils::hashmap::HashMap; use std::collections::BinaryHeap; @@ -66,7 +67,7 @@ impl CCDSolver { &RigidBodyCcd, &RigidBodyMassProps, ) = bodies.index_bundle(handle.0); - let local_com = &mprops.mass_properties.local_com; + let local_com = &mprops.local_mprops.local_com; let min_toi = (ccd.ccd_thickness * 0.15 @@ -272,7 +273,8 @@ impl CCDSolver { Colliders: ComponentSetOption + ComponentSet + ComponentSet - + ComponentSet, + + ComponentSet + + ComponentSet, { let mut frozen = HashMap::<_, Real>::default(); let mut all_toi = BinaryHeap::new(); @@ -522,10 +524,16 @@ impl CCDSolver { // - If the intersection isn't active anymore, and it wasn't intersecting // before, then we need to generate one interaction-start and one interaction-stop // events because it will never be detected by the narrow-phase because of tunneling. - let (co_pos1, co_shape1): (&ColliderPosition, &ColliderShape) = - colliders.index_bundle(toi.c1.0); - let (co_pos2, co_shape2): (&ColliderPosition, &ColliderShape) = - colliders.index_bundle(toi.c2.0); + let (co_pos1, co_shape1, co_flags1): ( + &ColliderPosition, + &ColliderShape, + &ColliderFlags, + ) = colliders.index_bundle(toi.c1.0); + let (co_pos2, co_shape2, co_flags2): ( + &ColliderPosition, + &ColliderShape, + &ColliderFlags, + ) = colliders.index_bundle(toi.c2.0); let co_next_pos1 = if let Some(b1) = toi.b1 { let co_parent1: &ColliderParent = colliders.get(toi.c1.0).unwrap(); @@ -535,7 +543,7 @@ impl CCDSolver { &RigidBodyMassProps, ) = bodies.index_bundle(b1.0); - let local_com1 = &rb_mprops1.mass_properties.local_com; + let local_com1 = &rb_mprops1.local_mprops.local_com; let frozen1 = frozen.get(&b1); let pos1 = frozen1 .map(|t| rb_vels1.integrate(*t, &rb_pos1.position, local_com1)) @@ -553,7 +561,7 @@ impl CCDSolver { &RigidBodyMassProps, ) = bodies.index_bundle(b2.0); - let local_com2 = &rb_mprops2.mass_properties.local_com; + let local_com2 = &rb_mprops2.local_mprops.local_com; let frozen2 = frozen.get(&b2); let pos2 = frozen2 .map(|t| rb_vels2.integrate(*t, &rb_pos2.position, local_com2)) @@ -575,7 +583,11 @@ impl CCDSolver { .intersection_test(&next_coll_pos12, co_shape1.as_ref(), co_shape2.as_ref()) .unwrap_or(false); - if !intersect_before && !intersect_after { + if !intersect_before + && !intersect_after + && (co_flags1.active_events | co_flags2.active_events) + .contains(ActiveEvents::INTERSECTION_EVENTS) + { // Emit one intersection-started and one intersection-stopped event. events.handle_intersection_event(IntersectionEvent::new(toi.c1, toi.c2, true)); events.handle_intersection_event(IntersectionEvent::new(toi.c1, toi.c2, false)); diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs index 4637940..78396c0 100644 --- a/src/dynamics/ccd/toi_entry.rs +++ b/src/dynamics/ccd/toi_entry.rs @@ -173,7 +173,7 @@ impl TOIEntry { if ccd.ccd_active { NonlinearRigidMotion::new( poss.position, - mprops.mass_properties.local_com, + mprops.local_mprops.local_com, vels.linvel, vels.angvel, ) diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs index b79cdb2..34fa3bd 100644 --- a/src/dynamics/island_manager.rs +++ b/src/dynamics/island_manager.rs @@ -229,19 +229,17 @@ impl IslandManager { stack: &mut Vec, ) { for collider_handle in &rb_colliders.0 { - if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) { - for inter in contacts { - for manifold in &inter.2.manifolds { - if !manifold.data.solver_contacts.is_empty() { - let other = crate::utils::select_other( - (inter.0, inter.1), - *collider_handle, - ); - if let Some(other_body) = colliders.get(other.0) { - stack.push(other_body.handle); - } - break; + for inter in narrow_phase.contacts_with(*collider_handle) { + for manifold in &inter.manifolds { + if !manifold.data.solver_contacts.is_empty() { + let other = crate::utils::select_other( + (inter.collider1, inter.collider2), + *collider_handle, + ); + if let Some(other_body) = colliders.get(other.0) { + stack.push(other_body.handle); } + break; } } } diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index d53ff98..07e362d 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -4,8 +4,7 @@ use crate::dynamics::{ RigidBodyMassPropsFlags, RigidBodyPosition, RigidBodyType, RigidBodyVelocity, }; use crate::geometry::{ - Collider, ColliderHandle, ColliderMassProperties, ColliderParent, ColliderPosition, - ColliderShape, + Collider, ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition, ColliderShape, }; use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector}; use crate::utils::{self, WCross}; @@ -48,7 +47,7 @@ impl RigidBody { rb_ccd: RigidBodyCcd::default(), rb_ids: RigidBodyIds::default(), rb_colliders: RigidBodyColliders::default(), - rb_activation: RigidBodyActivation::new_active(), + rb_activation: RigidBodyActivation::active(), changes: RigidBodyChanges::all(), rb_type: RigidBodyType::Dynamic, rb_dominance: RigidBodyDominance::default(), @@ -112,7 +111,7 @@ impl RigidBody { /// The mass properties of this rigid-body. #[inline] pub fn mass_properties(&self) -> &MassProperties { - &self.rb_mprops.mass_properties + &self.rb_mprops.local_mprops } /// The dominance group of this rigid-body. @@ -256,7 +255,7 @@ impl RigidBody { self.wake_up(true); } - self.rb_mprops.mass_properties = props; + self.rb_mprops.local_mprops = props; self.update_world_mass_properties(); } @@ -290,7 +289,7 @@ impl RigidBody { /// /// Returns zero if this rigid body has an infinite mass. pub fn mass(&self) -> Real { - utils::inv(self.rb_mprops.mass_properties.inv_mass) + utils::inv(self.rb_mprops.local_mprops.inv_mass) } /// The predicted position of this rigid-body. @@ -335,7 +334,7 @@ impl RigidBody { co_parent: &ColliderParent, co_pos: &mut ColliderPosition, co_shape: &ColliderShape, - co_mprops: &ColliderMassProperties, + co_mprops: &ColliderMassProps, ) { self.rb_colliders.attach_collider( &mut self.changes, @@ -359,7 +358,7 @@ impl RigidBody { let mass_properties = coll .mass_properties() .transform_by(coll.position_wrt_parent().unwrap()); - self.rb_mprops.mass_properties -= mass_properties; + self.rb_mprops.local_mprops -= mass_properties; self.update_world_mass_properties(); } } @@ -463,8 +462,8 @@ impl RigidBody { /// The translational part of this rigid-body's position. #[inline] - pub fn translation(&self) -> Vector { - self.rb_pos.position.translation.vector + pub fn translation(&self) -> &Vector { + &self.rb_pos.position.translation.vector } /// Sets the translational part of this rigid-body's position. @@ -482,8 +481,8 @@ impl RigidBody { /// The translational part of this rigid-body's position. #[inline] - pub fn rotation(&self) -> Rotation { - self.rb_pos.position.rotation + pub fn rotation(&self) -> &Rotation { + &self.rb_pos.position.rotation } /// Sets the rotational part of this rigid-body's position. @@ -692,7 +691,7 @@ impl RigidBody { pub fn gravitational_potential_energy(&self, dt: Real, gravity: Vector) -> Real { let world_com = self .rb_mprops - .mass_properties + .local_mprops .world_com(&self.rb_pos.position) .coords; @@ -979,7 +978,7 @@ impl RigidBodyBuilder { rb.rb_vels.angvel = self.angvel; rb.rb_type = self.rb_type; rb.user_data = self.user_data; - rb.rb_mprops.mass_properties = self.mass_properties; + rb.rb_mprops.local_mprops = self.mass_properties; rb.rb_mprops.flags = self.mprops_flags; rb.rb_damping.linear_damping = self.linear_damping; rb.rb_damping.angular_damping = self.angular_damping; diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index 2a652f7..b536d07 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -1,7 +1,7 @@ use crate::data::{ComponentSetMut, ComponentSetOption}; use crate::dynamics::MassProperties; use crate::geometry::{ - ColliderChanges, ColliderHandle, ColliderMassProperties, ColliderParent, ColliderPosition, + ColliderChanges, ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition, ColliderShape, InteractionGraph, RigidBodyGraphIndex, }; use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, Translation, Vector}; @@ -166,7 +166,7 @@ impl RigidBodyPosition { mprops: &RigidBodyMassProps, ) -> Isometry { let new_vels = forces.integrate(dt, vels, mprops); - new_vels.integrate(dt, &self.position, &mprops.mass_properties.local_com) + new_vels.integrate(dt, &self.position, &mprops.local_mprops.local_com) } } @@ -208,7 +208,7 @@ pub struct RigidBodyMassProps { /// Flags for locking rotation and translation. pub flags: RigidBodyMassPropsFlags, /// The local mass properties of the rigid-body. - pub mass_properties: MassProperties, + pub local_mprops: MassProperties, /// The world-space center of mass of the rigid-body. pub world_com: Point, /// The inverse mass taking into account translation locking. @@ -222,7 +222,7 @@ impl Default for RigidBodyMassProps { fn default() -> Self { Self { flags: RigidBodyMassPropsFlags::empty(), - mass_properties: MassProperties::zero(), + local_mprops: MassProperties::zero(), world_com: Point::origin(), effective_inv_mass: 0.0, effective_world_inv_inertia_sqrt: AngularInertia::zero(), @@ -239,11 +239,20 @@ impl From for RigidBodyMassProps { } } +impl From for RigidBodyMassProps { + fn from(local_mprops: MassProperties) -> Self { + Self { + local_mprops, + ..Default::default() + } + } +} + impl RigidBodyMassProps { /// The mass of the rigid-body. #[must_use] pub fn mass(&self) -> Real { - crate::utils::inv(self.mass_properties.inv_mass) + crate::utils::inv(self.local_mprops.inv_mass) } /// The effective mass (that takes the potential translation locking into account) of @@ -255,11 +264,10 @@ impl RigidBodyMassProps { /// Update the world-space mass properties of `self`, taking into account the new position. pub fn update_world_mass_properties(&mut self, position: &Isometry) { - self.world_com = self.mass_properties.world_com(&position); - self.effective_inv_mass = self.mass_properties.inv_mass; - self.effective_world_inv_inertia_sqrt = self - .mass_properties - .world_inv_inertia_sqrt(&position.rotation); + self.world_com = self.local_mprops.world_com(&position); + self.effective_inv_mass = self.local_mprops.inv_mass; + self.effective_world_inv_inertia_sqrt = + self.local_mprops.world_inv_inertia_sqrt(&position.rotation); // Take into account translation/rotation locking. if self @@ -665,7 +673,7 @@ impl RigidBodyColliders { co_pos: &mut ColliderPosition, co_parent: &ColliderParent, co_shape: &ColliderShape, - co_mprops: &ColliderMassProperties, + co_mprops: &ColliderMassProps, ) { rb_changes.set( RigidBodyChanges::MODIFIED | RigidBodyChanges::COLLIDERS, @@ -684,7 +692,7 @@ impl RigidBodyColliders { .mass_properties(&**co_shape) .transform_by(&co_parent.pos_wrt_parent); self.0.push(co_handle); - rb_mprops.mass_properties += mass_properties; + rb_mprops.local_mprops += mass_properties; rb_mprops.update_world_mass_properties(&rb_pos.position); } @@ -759,7 +767,7 @@ pub struct RigidBodyActivation { impl Default for RigidBodyActivation { fn default() -> Self { - Self::new_active() + Self::active() } } @@ -770,7 +778,7 @@ impl RigidBodyActivation { } /// Create a new rb_activation status initialised with the default rb_activation threshold and is active. - pub fn new_active() -> Self { + pub fn active() -> Self { RigidBodyActivation { threshold: Self::default_threshold(), energy: Self::default_threshold() * 4.0, @@ -779,7 +787,7 @@ impl RigidBodyActivation { } /// Create a new rb_activation status initialised with the default rb_activation threshold and is inactive. - pub fn new_inactive() -> Self { + pub fn inactive() -> Self { RigidBodyActivation { threshold: Self::default_threshold(), energy: 0.0, @@ -787,6 +795,14 @@ impl RigidBodyActivation { } } + /// Create a new activation status that prevents the rigid-body from sleeping. + pub fn cannot_sleep() -> Self { + RigidBodyActivation { + threshold: -Real::MAX, + ..Self::active() + } + } + /// Returns `true` if the body is not asleep. #[inline] pub fn is_active(&self) -> bool { diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index 1e65341..0c4c323 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -113,7 +113,7 @@ impl IslandSolver { let mut new_poss = *poss; let new_vels = vels.apply_damping(params.dt, damping); new_poss.next_position = - vels.integrate(params.dt, &poss.position, &mprops.mass_properties.local_com); + vels.integrate(params.dt, &poss.position, &mprops.local_mprops.local_com); bodies.set_internal(handle.0, new_vels); bodies.set_internal(handle.0, new_poss); @@ -140,7 +140,7 @@ impl IslandSolver { .integrate(params.dt, vels, mprops) .apply_damping(params.dt, &damping); new_poss.next_position = - vels.integrate(params.dt, &poss.position, &mprops.mass_properties.local_com); + vels.integrate(params.dt, &poss.position, &mprops.local_mprops.local_com); bodies.set_internal(handle.0, new_vels); bodies.set_internal(handle.0, new_poss); diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs index 159cc77..0227960 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs @@ -34,8 +34,8 @@ impl BallPositionConstraint { let (mprops2, ids2) = rb2; Self { - local_com1: mprops1.mass_properties.local_com, - local_com2: mprops2.mass_properties.local_com, + local_com1: mprops1.local_mprops.local_com, + local_com2: mprops2.local_mprops.local_com, im1: mprops1.effective_inv_mass, im2: mprops2.effective_inv_mass, ii1: mprops1.effective_world_inv_inertia_sqrt.squared(), @@ -131,7 +131,7 @@ impl BallPositionGroundConstraint { ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor1, position2: ids2.active_set_offset, - local_com2: mprops2.mass_properties.local_com, + local_com2: mprops2.local_mprops.local_com, } } else { Self { @@ -140,7 +140,7 @@ impl BallPositionGroundConstraint { ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor2, position2: ids2.active_set_offset, - local_com2: mprops2.mass_properties.local_com, + local_com2: mprops2.local_mprops.local_com, } } } diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs index 6b00639..ea462ed 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs @@ -40,8 +40,8 @@ impl WBallPositionConstraint { let (mprops1, ids1) = rbs1; let (mprops2, ids2) = rbs2; - let local_com1 = Point::from(gather![|ii| mprops1[ii].mass_properties.local_com]); - let local_com2 = Point::from(gather![|ii| mprops2[ii].mass_properties.local_com]); + let local_com1 = Point::from(gather![|ii| mprops1[ii].local_mprops.local_com]); + let local_com2 = Point::from(gather![|ii| mprops2[ii].local_mprops.local_com]); let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]); let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); let ii1 = AngularInertia::::from(gather![ @@ -169,7 +169,7 @@ impl WBallPositionGroundConstraint { cparams[ii].local_anchor2 }]); let position2 = gather![|ii| ids2[ii].active_set_offset]; - let local_com2 = Point::from(gather![|ii| mprops2[ii].mass_properties.local_com]); + let local_com2 = Point::from(gather![|ii| mprops2[ii].local_mprops.local_com]); Self { anchor1, diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs index f8945c3..3ab13f7 100644 --- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs @@ -46,8 +46,8 @@ impl FixedPositionConstraint { im2, ii1, ii2, - local_com1: mprops1.mass_properties.local_com, - local_com2: mprops2.mass_properties.local_com, + local_com1: mprops1.local_mprops.local_com, + local_com2: mprops2.local_mprops.local_com, lin_inv_lhs, ang_inv_lhs, } @@ -125,7 +125,7 @@ impl FixedPositionGroundConstraint { position2: ids2.active_set_offset, im2: mprops2.effective_inv_mass, ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), - local_com2: mprops2.mass_properties.local_com, + local_com2: mprops2.local_mprops.local_com, impulse: 0.0, } } diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs index 76901b1..a7b5ea0 100644 --- a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs @@ -41,8 +41,8 @@ impl GenericPositionConstraint { im2, ii1, ii2, - local_com1: rb1.mass_properties.local_com, - local_com2: rb2.mass_properties.local_com, + local_com1: rb1.local_mprops.local_com, + local_com2: rb2.local_mprops.local_com, joint: *joint, } } @@ -215,7 +215,7 @@ impl GenericPositionGroundConstraint { position2: rb2.active_set_offset, im2: rb2.effective_inv_mass, ii2: rb2.effective_world_inv_inertia_sqrt.squared(), - local_com2: rb2.mass_properties.local_com, + local_com2: rb2.local_mprops.local_com, joint: *joint, } } diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs index 1b3e23a..731d0e2 100644 --- a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs @@ -51,8 +51,8 @@ impl RevolutePositionConstraint { ii1, ii2, ang_inv_lhs, - local_com1: mprops1.mass_properties.local_com, - local_com2: mprops2.mass_properties.local_com, + local_com1: mprops1.local_mprops.local_com, + local_com2: mprops2.local_mprops.local_com, local_anchor1: cparams.local_anchor1, local_anchor2: cparams.local_anchor2, local_axis1: cparams.local_axis1, @@ -183,7 +183,7 @@ impl RevolutePositionGroundConstraint { local_anchor2, im2: mprops2.effective_inv_mass, ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), - local_com2: mprops2.mass_properties.local_com, + local_com2: mprops2.local_mprops.local_com, axis1, local_axis2, position2: ids2.active_set_offset, diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs index 9d565c1..e12e7af 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -392,7 +392,7 @@ impl ParallelIslandSolver { let new_rb_vels = new_rb_vels.apply_damping(params.dt, rb_damping); new_rb_pos.next_position = - new_rb_vels.integrate(params.dt, &rb_pos.position, &rb_mprops.mass_properties.local_com); + new_rb_vels.integrate(params.dt, &rb_pos.position, &rb_mprops.local_mprops.local_com); bodies.set_internal(handle.0, new_rb_vels); bodies.set_internal(handle.0, new_rb_pos); -- cgit From 5ef81cda406d796c5d188542b5bd9fbf4aefd4cf Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 1 Jun 2021 14:55:50 +0200 Subject: Add velocity-based kinematic bodies --- src/dynamics/rigid_body.rs | 15 ++++++++++----- src/dynamics/rigid_body_components.rs | 14 +++++++++++--- 2 files changed, 21 insertions(+), 8 deletions(-) (limited to 'src/dynamics') diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 07e362d..63c2221 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -275,7 +275,7 @@ impl RigidBody { /// /// A kinematic body can move freely but is not affected by forces. pub fn is_kinematic(&self) -> bool { - self.rb_type == RigidBodyType::Kinematic + self.rb_type.is_kinematic() } /// Is this rigid body static? @@ -527,9 +527,9 @@ impl RigidBody { } /// If this rigid body is kinematic, sets its future orientation after the next timestep integration. - pub fn set_next_kinematic_translation(&mut self, rotation: Rotation) { + pub fn set_next_kinematic_translation(&mut self, translation: Vector) { if self.is_kinematic() { - self.rb_pos.next_position.rotation = rotation; + self.rb_pos.next_position.translation = translation.into(); } } @@ -748,8 +748,13 @@ impl RigidBodyBuilder { } /// Initializes the builder of a new kinematic rigid body. - pub fn new_kinematic() -> Self { - Self::new(RigidBodyType::Kinematic) + pub fn new_kinematic_velocity_based() -> Self { + Self::new(RigidBodyType::KinematicVelocityBased) + } + + /// Initializes the builder of a new kinematic rigid body. + pub fn new_kinematic_position_based() -> Self { + Self::new(RigidBodyType::KinematicPositionBased) } /// Initializes the builder of a new dynamic rigid body. diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index b536d07..d1a00ce 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -57,13 +57,20 @@ pub enum RigidBodyType { Dynamic, /// A `RigidBodyType::Static` body cannot be affected by external forces. Static, - /// A `RigidBodyType::Kinematic` body cannot be affected by any external forces but can be controlled + /// A `RigidBodyType::KinematicPositionBased` body cannot be affected by any external forces but can be controlled /// by the user at the position level while keeping realistic one-way interaction with dynamic bodies. /// /// One-way interaction means that a kinematic body can push a dynamic body, but a kinematic body /// cannot be pushed by anything. In other words, the trajectory of a kinematic body can only be /// modified by the user and is independent from any contact or joint it is involved in. - Kinematic, + KinematicPositionBased, + /// A `RigidBodyType::KinematicVelocityBased` body cannot be affected by any external forces but can be controlled + /// by the user at the velocity level while keeping realistic one-way interaction with dynamic bodies. + /// + /// One-way interaction means that a kinematic body can push a dynamic body, but a kinematic body + /// cannot be pushed by anything. In other words, the trajectory of a kinematic body can only be + /// modified by the user and is independent from any contact or joint it is involved in. + KinematicVelocityBased, // Semikinematic, // A kinematic that performs automatic CCD with the static environment to avoid traversing it? // Disabled, } @@ -81,7 +88,8 @@ impl RigidBodyType { /// Is this rigid-body kinematic (i.e. can move but is unaffected by forces)? pub fn is_kinematic(self) -> bool { - self == RigidBodyType::Kinematic + self == RigidBodyType::KinematicPositionBased + || self == RigidBodyType::KinematicVelocityBased } } -- cgit From dbb3c8f43b151e48619ed954b20d44dd9e5c2c55 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 1 Jun 2021 14:56:24 +0200 Subject: CCD: take collision groups into account --- src/dynamics/ccd/ccd_solver.rs | 75 +++++++++++++++++++++++++++--------------- src/dynamics/ccd/toi_entry.rs | 24 +++++++++----- 2 files changed, 63 insertions(+), 36 deletions(-) (limited to 'src/dynamics') diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index 354f183..dab4f73 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -10,7 +10,7 @@ use crate::geometry::{ use crate::math::Real; use crate::parry::utils::SortedPair; use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode}; -use crate::prelude::{ActiveEvents, ColliderFlags}; +use crate::prelude::{ActiveEvents, ColliderFlags, ColliderGroups}; use parry::query::{DefaultQueryDispatcher, QueryDispatcher}; use parry::utils::hashmap::HashMap; use std::collections::BinaryHeap; @@ -140,7 +140,8 @@ impl CCDSolver { Colliders: ComponentSetOption + ComponentSet + ComponentSet - + ComponentSet, + + ComponentSet + + ComponentSet, { // Update the query pipeline. self.query_pipeline.update_with_mode( @@ -201,16 +202,21 @@ impl CCDSolver { { let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0); - let c1: (_, _, _) = colliders.index_bundle(ch1.0); - let c2: (_, _, _) = colliders.index_bundle(ch2.0); + let c1: (_, _, _, &ColliderGroups) = colliders.index_bundle(ch1.0); + let c2: (_, _, _, &ColliderGroups) = colliders.index_bundle(ch2.0); let co_type1: &ColliderType = colliders.index(ch1.0); let co_type2: &ColliderType = colliders.index(ch1.0); let bh1 = co_parent1.map(|p| p.handle); let bh2 = co_parent2.map(|p| p.handle); - if bh1 == bh2 || (co_type1.is_sensor() || co_type2.is_sensor()) { - // Ignore self-intersection and sensors. + // Ignore self-intersection and sensors and apply collision groups filter. + if bh1 == bh2 // Ignore self-intersection. + || (co_type1.is_sensor() || co_type2.is_sensor()) // Ignore sensors. + || !c1.3.collision_groups.test(c2.3.collision_groups) // Apply collision groups. + || !c1.3.solver_groups.test(c2.3.solver_groups) + // Apply solver groups. + { return true; } @@ -226,8 +232,8 @@ impl CCDSolver { self.query_pipeline.query_dispatcher(), *ch1, *ch2, - (c1.0, c1.1, c1.2, co_parent1), - (c2.0, c2.1, c2.2, co_parent2), + (c1.0, c1.1, c1.2, c1.3, co_parent1), + (c2.0, c2.1, c2.2, c2.3, co_parent2), Some((rb_pos1, rb_vels1, rb_mprops1, rb_ccd1)), b2, None, @@ -274,7 +280,8 @@ impl CCDSolver { + ComponentSet + ComponentSet + ComponentSet - + ComponentSet, + + ComponentSet + + ComponentSet, { let mut frozen = HashMap::<_, Real>::default(); let mut all_toi = BinaryHeap::new(); @@ -336,14 +343,15 @@ impl CCDSolver { { let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0); - let c1: (_, _, _) = colliders.index_bundle(ch1.0); - let c2: (_, _, _) = colliders.index_bundle(ch2.0); + let c1: (_, _, _, &ColliderGroups) = colliders.index_bundle(ch1.0); + let c2: (_, _, _, &ColliderGroups) = colliders.index_bundle(ch2.0); let bh1 = co_parent1.map(|p| p.handle); let bh2 = co_parent2.map(|p| p.handle); - if bh1 == bh2 { - // Ignore self-intersection. + // Ignore self-intersections and apply groups filter. + if bh1 == bh2 || !c1.3.collision_groups.test(c2.3.collision_groups) + { return true; } @@ -360,8 +368,8 @@ impl CCDSolver { self.query_pipeline.query_dispatcher(), *ch1, *ch2, - (c1.0, c1.1, c1.2, co_parent1), - (c2.0, c2.1, c2.2, co_parent2), + (c1.0, c1.1, c1.2, c1.3, co_parent1), + (c2.0, c2.1, c2.2, c2.3, co_parent2), b1, b2, None, @@ -400,7 +408,7 @@ impl CCDSolver { // NOTE: all static bodies (and kinematic bodies?) should be considered as "frozen", this // may avoid some resweeps. - let mut intersections_to_check = vec![]; + let mut pseudo_intersections_to_check = vec![]; while let Some(toi) = all_toi.pop() { assert!(toi.toi <= dt); @@ -422,7 +430,7 @@ impl CCDSolver { continue; } - if toi.is_intersection_test { + if toi.is_pseudo_intersection_test { // NOTE: this test is redundant with the previous `if !should_freeze && ...` // but let's keep it to avoid tricky regressions if we end up swapping both // `if` for some reasons in the future. @@ -430,7 +438,7 @@ impl CCDSolver { // This is only an intersection so we don't have to freeze and there is no // need to resweep. However we will need to see if we have to generate // intersection events, so push the TOI for further testing. - intersections_to_check.push(toi); + pseudo_intersections_to_check.push(toi); } continue; } @@ -462,14 +470,14 @@ impl CCDSolver { .colliders_with_aabb_intersecting_aabb(&aabb, |ch2| { let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0); - let c1: (_, _, _) = colliders.index_bundle(ch1.0); - let c2: (_, _, _) = colliders.index_bundle(ch2.0); + let c1: (_, _, _, &ColliderGroups) = colliders.index_bundle(ch1.0); + let c2: (_, _, _, &ColliderGroups) = colliders.index_bundle(ch2.0); let bh1 = co_parent1.map(|p| p.handle); let bh2 = co_parent2.map(|p| p.handle); - if bh1 == bh2 { - // Ignore self-intersection. + // Ignore self-intersection and apply groups filter. + if bh1 == bh2 || !c1.3.collision_groups.test(c2.3.collision_groups) { return true; } @@ -498,8 +506,8 @@ impl CCDSolver { self.query_pipeline.query_dispatcher(), *ch1, *ch2, - (c1.0, c1.1, c1.2, co_parent1), - (c2.0, c2.1, c2.2, co_parent2), + (c1.0, c1.1, c1.2, c1.3, co_parent1), + (c2.0, c2.1, c2.2, c2.3, co_parent2), b1, b2, frozen1.copied(), @@ -516,7 +524,7 @@ impl CCDSolver { } } - for toi in intersections_to_check { + for toi in pseudo_intersections_to_check { // See if the intersection is still active once the bodies // reach their final positions. // - If the intersection is still active, don't report it yet. It will be @@ -524,17 +532,30 @@ impl CCDSolver { // - If the intersection isn't active anymore, and it wasn't intersecting // before, then we need to generate one interaction-start and one interaction-stop // events because it will never be detected by the narrow-phase because of tunneling. - let (co_pos1, co_shape1, co_flags1): ( + let (co_type1, co_pos1, co_shape1, co_flags1): ( + &ColliderType, &ColliderPosition, &ColliderShape, &ColliderFlags, ) = colliders.index_bundle(toi.c1.0); - let (co_pos2, co_shape2, co_flags2): ( + let (co_type2, co_pos2, co_shape2, co_flags2): ( + &ColliderType, &ColliderPosition, &ColliderShape, &ColliderFlags, ) = colliders.index_bundle(toi.c2.0); + if !co_type1.is_sensor() && !co_type2.is_sensor() { + // TODO: this happens if we found a TOI between two non-sensor + // colliders with mismatching solver_flags. It is not clear + // what we should do in this case: we could report a + // contact started/contact stopped event for example. But in + // that case, what contact pair should be pass to these events? + // For now we just ignore this special case. Let's wait for an actual + // use-case to come up before we determine what we want to do here. + continue; + } + let co_next_pos1 = if let Some(b1) = toi.b1 { let co_parent1: &ColliderParent = colliders.get(toi.c1.0).unwrap(); let (rb_pos1, rb_vels1, rb_mprops1): ( diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs index 78396c0..918e7c4 100644 --- a/src/dynamics/ccd/toi_entry.rs +++ b/src/dynamics/ccd/toi_entry.rs @@ -2,7 +2,7 @@ use crate::dynamics::{ RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, }; use crate::geometry::{ - ColliderHandle, ColliderParent, ColliderPosition, ColliderShape, ColliderType, + ColliderGroups, ColliderHandle, ColliderParent, ColliderPosition, ColliderShape, ColliderType, }; use crate::math::Real; use parry::query::{NonlinearRigidMotion, QueryDispatcher}; @@ -14,7 +14,9 @@ pub struct TOIEntry { pub b1: Option, pub c2: ColliderHandle, pub b2: Option, - pub is_intersection_test: bool, + // We call this "pseudo" intersection because this also + // includes colliders pairs with mismatching solver_groups. + pub is_pseudo_intersection_test: bool, pub timestamp: usize, } @@ -25,7 +27,7 @@ impl TOIEntry { b1: Option, c2: ColliderHandle, b2: Option, - is_intersection_test: bool, + is_pseudo_intersection_test: bool, timestamp: usize, ) -> Self { Self { @@ -34,7 +36,7 @@ impl TOIEntry { b1, c2, b2, - is_intersection_test, + is_pseudo_intersection_test, timestamp, } } @@ -47,12 +49,14 @@ impl TOIEntry { &ColliderType, &ColliderShape, &ColliderPosition, + &ColliderGroups, Option<&ColliderParent>, ), c2: ( &ColliderType, &ColliderShape, &ColliderPosition, + &ColliderGroups, Option<&ColliderParent>, ), b1: Option<( @@ -78,8 +82,8 @@ impl TOIEntry { return None; } - let (co_type1, co_shape1, co_pos1, co_parent1) = c1; - let (co_type2, co_shape2, co_pos2, co_parent2) = c2; + let (co_type1, co_shape1, co_pos1, co_groups1, co_parent1) = c1; + let (co_type2, co_shape2, co_pos2, co_groups2, co_parent2) = c2; let linvel1 = frozen1.is_none() as u32 as Real * b1.map(|b| b.1.linvel).unwrap_or(na::zero()); @@ -104,7 +108,9 @@ impl TOIEntry { // keep it since more conservatism is good at this stage. let thickness = (co_shape1.0.ccd_thickness() + co_shape2.0.ccd_thickness()) + smallest_contact_dist.max(0.0); - let is_intersection_test = co_type1.is_sensor() || co_type2.is_sensor(); + let is_pseudo_intersection_test = co_type1.is_sensor() + || co_type2.is_sensor() + || !co_groups1.solver_groups.test(co_groups2.solver_groups); if (end_time - start_time) * vel12 < thickness { return None; @@ -135,7 +141,7 @@ impl TOIEntry { // If the TOI search involves two non-sensor colliders then // we don't want to stop the TOI search at the first penetration // because the colliders may be in a separating trajectory. - let stop_at_penetration = is_intersection_test; + let stop_at_penetration = is_pseudo_intersection_test; let res_toi