From 97157c9423f3360c5e941b4065377689221014ae Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Fri, 26 Mar 2021 18:16:27 +0100 Subject: First working version of non-linear CCD based on single-substep motion-clamping. --- src/dynamics/ccd/toi_entry.rs | 147 +++++++++++++++++++++ src/dynamics/mod.rs | 2 + src/dynamics/rigid_body.rs | 110 +++++++++++---- src/dynamics/solver/island_solver.rs | 4 +- .../joint_constraint/ball_position_constraint.rs | 4 +- .../ball_position_constraint_wide.rs | 2 +- .../joint_constraint/fixed_position_constraint.rs | 4 +- .../prismatic_position_constraint.rs | 8 +- .../revolute_position_constraint.rs | 16 +-- src/dynamics/solver/parallel_island_solver.rs | 4 +- src/dynamics/solver/position_solver.rs | 4 +- 11 files changed, 257 insertions(+), 48 deletions(-) create mode 100644 src/dynamics/ccd/toi_entry.rs (limited to 'src/dynamics') diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs new file mode 100644 index 0000000..20f5268 --- /dev/null +++ b/src/dynamics/ccd/toi_entry.rs @@ -0,0 +1,147 @@ +use crate::data::Coarena; +use crate::dynamics::ccd::ccd_solver::CCDContact; +use crate::dynamics::ccd::CCDData; +use crate::dynamics::{IntegrationParameters, RigidBody, RigidBodyHandle}; +use crate::geometry::{Collider, ColliderHandle}; +use crate::math::{Isometry, Real}; +use crate::parry::query::PersistentQueryDispatcher; +use crate::utils::WCross; +use na::{RealField, Unit}; +use parry::query::{NonlinearRigidMotion, QueryDispatcher, TOI}; + +#[derive(Copy, Clone, Debug)] +pub struct TOIEntry { + pub toi: Real, + pub c1: ColliderHandle, + pub b1: RigidBodyHandle, + pub c2: ColliderHandle, + pub b2: RigidBodyHandle, + pub is_intersection_test: bool, + pub timestamp: usize, +} + +impl TOIEntry { + fn new( + toi: Real, + c1: ColliderHandle, + b1: RigidBodyHandle, + c2: ColliderHandle, + b2: RigidBodyHandle, + is_intersection_test: bool, + timestamp: usize, + ) -> Self { + Self { + toi, + c1, + b1, + c2, + b2, + is_intersection_test, + timestamp, + } + } + + pub fn try_from_colliders>( + params: &IntegrationParameters, + query_dispatcher: &QD, + ch1: ColliderHandle, + ch2: ColliderHandle, + c1: &Collider, + c2: &Collider, + b1: &RigidBody, + b2: &RigidBody, + frozen1: Option, + frozen2: Option, + start_time: Real, + end_time: Real, + body_params: &Coarena, + ) -> Option { + assert!(start_time <= end_time); + + let linvel1 = frozen1.is_none() as u32 as Real * b1.linvel; + let linvel2 = frozen2.is_none() as u32 as Real * b2.linvel; + + let vel12 = linvel2 - linvel1; + let thickness = (c1.shape().ccd_thickness() + c2.shape().ccd_thickness()); + + if params.dt * vel12.norm() < thickness { + return None; + } + + let is_intersection_test = c1.is_sensor() || c2.is_sensor(); + + let body_params1 = body_params.get(c1.parent.0)?; + let body_params2 = body_params.get(c2.parent.0)?; + + // Compute the TOI. + let mut motion1 = body_params1.motion(params.dt, b1, 0.0); + let mut motion2 = body_params2.motion(params.dt, b2, 0.0); + + if let Some(t) = frozen1 { + motion1.freeze(t); + } + + if let Some(t) = frozen2 { + motion2.freeze(t); + } + + let mut toi; + let motion_c1 = motion1.prepend(*c1.position_wrt_parent()); + let motion_c2 = motion2.prepend(*c2.position_wrt_parent()); + + // println!("start_time: {}", start_time); + + // If this is just an intersection test (i.e. with sensors) + // then we can stop the TOI search immediately if it starts with + // a penetration because we don't care about the whether the velocity + // at the impact is a separating velocity or not. + // 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 res_toi = query_dispatcher + .nonlinear_time_of_impact( + &motion_c1, + c1.shape(), + &motion_c2, + c2.shape(), + start_time, + end_time, + stop_at_penetration, + ) + .ok(); + + toi = res_toi??; + + Some(Self::new( + toi.toi, + ch1, + c1.parent(), + ch2, + c2.parent(), + is_intersection_test, + 0, + )) + } +} + +impl PartialOrd for TOIEntry { + fn partial_cmp(&self, other: &Self) -> Option { + (-self.toi).partial_cmp(&(-other.toi)) + } +} + +impl Ord for TOIEntry { + fn cmp(&self, other: &Self) -> std::cmp::Ordering { + self.partial_cmp(other).unwrap() + } +} + +impl PartialEq for TOIEntry { + fn eq(&self, other: &Self) -> bool { + self.toi == other.toi + } +} + +impl Eq for TOIEntry {} diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index eab6916..751f6f9 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -18,6 +18,7 @@ pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBui pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet}; pub use parry::mass_properties::MassProperties; // #[cfg(not(feature = "parallel"))] +pub use self::ccd::CCDSolver; pub use self::coefficient_combine_rule::CoefficientCombineRule; pub(crate) use self::joint::JointGraphEdge; pub(crate) use self::rigid_body::RigidBodyChanges; @@ -26,6 +27,7 @@ pub(crate) use self::solver::IslandSolver; #[cfg(feature = "parallel")] pub(crate) use self::solver::ParallelIslandSolver; +mod ccd; mod coefficient_combine_rule; mod integration_parameters; mod joint; diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 7cc7a99..9ac4763 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -36,6 +36,7 @@ bitflags::bitflags! { const ROTATION_LOCKED_X = 1 << 1; const ROTATION_LOCKED_Y = 1 << 2; const ROTATION_LOCKED_Z = 1 << 3; + const CCD_ENABLED = 1 << 4; } } @@ -58,7 +59,16 @@ bitflags::bitflags! { pub struct RigidBody { /// The world-space position of the rigid-body. pub(crate) position: Isometry, - pub(crate) predicted_position: Isometry, + /// The next position of the rigid-body. + /// + /// At the beginning of the timestep, and when the + /// timestep is complete we must have position == next_position + /// except for kinematic bodies. + /// + /// The next_position is updated after the velocity and position + /// resolution. Then it is either validated (ie. we set position := set_position) + /// or clamped by CCD. + pub(crate) next_position: Isometry, /// The local mass properties of the rigid-body. pub(crate) mass_properties: MassProperties, /// The world-space center of mass of the rigid-body. @@ -76,6 +86,10 @@ pub struct RigidBody { pub linear_damping: Real, /// Damping factor for gradually slowing down the angular motion of the rigid-body. pub angular_damping: Real, + /// The maximum linear velocity this rigid-body can reach. + pub max_linear_velocity: Real, + /// The maximum angular velocity this rigid-body can reach. + pub max_angular_velocity: Real, /// Accumulation of external forces (only for dynamic bodies). pub(crate) force: Vector, /// Accumulation of external torques (only for dynamic bodies). @@ -97,13 +111,14 @@ pub struct RigidBody { dominance_group: i8, /// User-defined data associated to this rigid-body. pub user_data: u128, + pub(crate) ccd_thickness: Real, } impl RigidBody { fn new() -> Self { Self { position: Isometry::identity(), - predicted_position: Isometry::identity(), + next_position: Isometry::identity(), mass_properties: MassProperties::zero(), world_com: Point::origin(), effective_inv_mass: 0.0, @@ -115,6 +130,8 @@ impl RigidBody { gravity_scale: 1.0, linear_damping: 0.0, angular_damping: 0.0, + max_linear_velocity: Real::MAX, + max_angular_velocity: 100.0, colliders: Vec::new(), activation: ActivationStatus::new_active(), joint_graph_index: InteractionGraph::<(), ()>::invalid_graph_index(), @@ -127,6 +144,7 @@ impl RigidBody { body_status: BodyStatus::Dynamic, dominance_group: 0, user_data: 0, + ccd_thickness: Real::MAX, } } @@ -176,6 +194,20 @@ impl RigidBody { } } + /// Enables of disable CCD (continuous collision-detection) for this rigid-body. + pub fn enable_ccd(&mut self, enabled: bool) { + self.flags.set(RigidBodyFlags::CCD_ENABLED, enabled) + } + + /// Is CCD (continous collision-detection) enabled for this rigid-body? + pub fn is_ccd_enabled(&self) -> bool { + self.flags.contains(RigidBodyFlags::CCD_ENABLED) + } + + pub(crate) fn should_resolve_ccd(&self, dt: Real) -> bool { + self.is_ccd_enabled() && self.is_dynamic() && self.linvel.norm() * dt > self.ccd_thickness + } + /// Sets the rigid-body's mass properties. /// /// If `wake_up` is `true` then the rigid-body will be woken up if it was @@ -228,8 +260,8 @@ impl RigidBody { /// If this rigid-body is kinematic this value is set by the `set_next_kinematic_position` /// method and is used for estimating the kinematic body velocity at the next timestep. /// For non-kinematic bodies, this value is currently unspecified. - pub fn predicted_position(&self) -> &Isometry { - &self.predicted_position + pub fn next_position(&self) -> &Isometry { + &self.next_position } /// The scale factor applied to the gravity affecting this rigid-body. @@ -254,6 +286,8 @@ impl RigidBody { true, ); + self.ccd_thickness = self.ccd_thickness.min(coll.shape().ccd_thickness()); + let mass_properties = coll .mass_properties() .transform_by(coll.position_wrt_parent()); @@ -265,8 +299,8 @@ impl RigidBody { pub(crate) fn update_colliders_positions(&mut self, colliders: &mut ColliderSet) { for handle in &self.colliders { let collider = &mut colliders[*handle]; + collider.prev_position = self.position; collider.position = self.position * collider.delta; - collider.predicted_position = self.predicted_position * collider.delta; } } @@ -331,18 +365,39 @@ impl RigidBody { !self.linvel.is_zero() || !self.angvel.is_zero() } - fn integrate_velocity(&self, dt: Real) -> Isometry { + pub(crate) fn integrate_velocity(&self, dt: Real) -> Isometry { let com = self.position * self.mass_properties.local_com; let shift = Translation::from(com.coords); shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse() } - pub(crate) fn integrate(&mut self, dt: Real) { + pub(crate) fn position_at_time(&self, dt: Real) -> Isometry { + self.integrate_velocity(dt) * self.position + } + + pub(crate) fn integrate_next_position(&mut self, dt: Real, apply_damping: bool) { // TODO: do we want to apply damping before or after the velocity integration? - self.linvel *= 1.0 / (1.0 + dt * self.linear_damping); - self.angvel *= 1.0 / (1.0 + dt * self.angular_damping); + if apply_damping { + self.linvel *= 1.0 / (1.0 + dt * self.linear_damping); + self.angvel *= 1.0 / (1.0 + dt * self.angular_damping); + + // self.linvel = self.linvel.cap_magnitude(self.max_linear_velocity); + // #[cfg(feature = "dim2")] + // { + // self.angvel = na::clamp( + // self.angvel, + // -self.max_angular_velocity, + // self.max_angular_velocity, + // ); + // } + // #[cfg(feature = "dim3")] + // { + // self.angvel = self.angvel.cap_magnitude(self.max_angular_velocity); + // } + } - self.position = self.integrate_velocity(dt) * self.position; + self.next_position = self.integrate_velocity(dt) * self.position; + let _ = self.next_position.rotation.renormalize(); } /// The linear velocity of this rigid-body. @@ -416,7 +471,8 @@ impl RigidBody { /// put to sleep because it did not move for a while. pub fn set_position(&mut self, pos: Isometry, wake_up: bool) { self.changes.insert(RigidBodyChanges::POSITION); - self.set_position_internal(pos); + self.position = pos; + self.next_position = pos; // TODO: Do we really need to check that the body isn't dynamic? if wake_up && self.is_dynamic() { @@ -424,24 +480,19 @@ impl RigidBody { } } - pub(crate) fn set_position_internal(&mut self, pos: Isometry) { - self.position = pos; - - // TODO: update the predicted position for dynamic bodies too? - if self.is_static() || self.is_kinematic() { - self.predicted_position = pos; - } + pub(crate) fn set_next_position(&mut self, pos: Isometry) { + self.next_position = pos; } /// 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() { - self.predicted_position = pos; + self.next_position = pos; } } - pub(crate) fn compute_velocity_from_predicted_position(&mut self, inv_dt: Real) { - let dpos = self.predicted_position * self.position.inverse(); + pub(crate) fn compute_velocity_from_next_position(&mut self, inv_dt: Real) { + let dpos = self.next_position * self.position.inverse(); #[cfg(feature = "dim2")] { self.angvel = dpos.rotation.angle() * inv_dt; @@ -453,8 +504,8 @@ impl RigidBody { self.linvel = dpos.translation.vector * inv_dt; } - pub(crate) fn update_predicted_position(&mut self, dt: Real) { - self.predicted_position = self.integrate_velocity(dt) * self.position; + pub(crate) fn update_next_position(&mut self, dt: Real) { + self.next_position = self.integrate_velocity(dt) * self.position; } pub(crate) fn update_world_mass_properties(&mut self) { @@ -666,6 +717,7 @@ pub struct RigidBodyBuilder { mass_properties: MassProperties, can_sleep: bool, sleeping: bool, + ccd_enabled: bool, dominance_group: i8, user_data: u128, } @@ -685,6 +737,7 @@ impl RigidBodyBuilder { mass_properties: MassProperties::zero(), can_sleep: true, sleeping: false, + ccd_enabled: false, dominance_group: 0, user_data: 0, } @@ -888,6 +941,12 @@ impl RigidBodyBuilder { self } + /// Enabled continuous collision-detection for this rigid-body. + pub fn ccd_enabled(mut self, enabled: bool) -> Self { + self.ccd_enabled = enabled; + self + } + /// Sets whether or not the rigid-body is to be created asleep. pub fn sleeping(mut self, sleeping: bool) -> Self { self.sleeping = sleeping; @@ -897,8 +956,8 @@ impl RigidBodyBuilder { /// Build a new rigid-body with the parameters configured with this builder. pub fn build(&self) -> RigidBody { let mut rb = RigidBody::new(); - rb.predicted_position = self.position; // FIXME: compute the correct value? - rb.set_position_internal(self.position); + rb.next_position = self.position; // FIXME: compute the correct value? + rb.position = self.position; rb.linvel = self.linvel; rb.angvel = self.angvel; rb.body_status = self.body_status; @@ -909,6 +968,7 @@ impl RigidBodyBuilder { rb.gravity_scale = self.gravity_scale; rb.flags = self.flags; rb.dominance_group = self.dominance_group; + rb.enable_ccd(self.ccd_enabled); if self.can_sleep && self.sleeping { rb.sleep(); diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index d0866bf..6ebf402 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -59,7 +59,7 @@ impl IslandSolver { counters.solver.velocity_update_time.resume(); bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { - rb.integrate(params.dt) + rb.integrate_next_position(params.dt, true) }); counters.solver.velocity_update_time.pause(); @@ -77,7 +77,7 @@ impl IslandSolver { bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { // Since we didn't run the velocity solver we need to integrate the accelerations here rb.integrate_accelerations(params.dt); - rb.integrate(params.dt); + rb.integrate_next_position(params.dt, true); }); counters.solver.velocity_update_time.pause(); } diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs index 744c00d..93996f4 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs @@ -114,7 +114,7 @@ impl BallPositionGroundConstraint { // are the local_anchors. The rb1 and rb2 have // already been flipped by the caller. Self { - anchor1: rb1.predicted_position * cparams.local_anchor2, + anchor1: rb1.next_position * cparams.local_anchor2, im2: rb2.effective_inv_mass, ii2: rb2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor1, @@ -123,7 +123,7 @@ impl BallPositionGroundConstraint { } } else { Self { - anchor1: rb1.predicted_position * cparams.local_anchor1, + anchor1: rb1.next_position * cparams.local_anchor1, im2: rb2.effective_inv_mass, ii2: rb2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor2, 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 043eea7..e9162a4 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs @@ -134,7 +134,7 @@ impl WBallPositionGroundConstraint { cparams: [&BallJoint; SIMD_WIDTH], flipped: [bool; SIMD_WIDTH], ) -> Self { - let position1 = Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]); + let position1 = Isometry::from(array![|ii| rbs1[ii].next_position; SIMD_WIDTH]); let anchor1 = position1 * Point::from(array![|ii| if flipped[ii] { cparams[ii].local_anchor2 diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs index 7e8fc97..c98660f 100644 --- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs @@ -100,10 +100,10 @@ impl FixedPositionGroundConstraint { let local_anchor2; if flipped { - anchor1 = rb1.predicted_position * cparams.local_anchor2; + anchor1 = rb1.next_position * cparams.local_anchor2; local_anchor2 = cparams.local_anchor1; } else { - anchor1 = rb1.predicted_position * cparams.local_anchor1; + anchor1 = rb1.next_position * cparams.local_anchor1; local_anchor2 = cparams.local_anchor2; }; diff --git a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs index ea7e927..ed52a52 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs @@ -119,14 +119,14 @@ impl PrismaticPositionGroundConstraint { let local_axis2; if flipped { - frame1 = rb1.predicted_position * cparams.local_frame2(); + frame1 = rb1.next_position * cparams.local_frame2(); local_frame2 = cparams.local_frame1(); - axis1 = rb1.predicted_position * cparams.local_axis2; + axis1 = rb1.next_position * cparams.local_axis2; local_axis2 = cparams.local_axis1; } else { - frame1 = rb1.predicted_position * cparams.local_frame1(); + frame1 = rb1.next_position * cparams.local_frame1(); local_frame2 = cparams.local_frame2(); - axis1 = rb1.predicted_position * cparams.local_axis1; + axis1 = rb1.next_position * cparams.local_axis1; local_axis2 = cparams.local_axis2; }; diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs index 2da49e6..96391a2 100644 --- a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs @@ -145,23 +145,23 @@ impl RevolutePositionGroundConstraint { let local_basis2; if flipped { - anchor1 = rb1.predicted_position * cparams.local_anchor2; + anchor1 = rb1.next_position * cparams.local_anchor2; local_anchor2 = cparams.local_anchor1; - axis1 = rb1.predicted_position * cparams.local_axis2; + axis1 = rb1.next_position * cparams.local_axis2; local_axis2 = cparams.local_axis1; basis1 = [ - rb1.predicted_position * cparams.basis2[0], - rb1.predicted_position * cparams.basis2[1], + rb1.next_position * cparams.basis2[0], + rb1.next_position * cparams.basis2[1], ]; local_basis2 = cparams.basis1; } else { - anchor1 = rb1.predicted_position * cparams.local_anchor1; + anchor1 = rb1.next_position * cparams.local_anchor1; local_anchor2 = cparams.local_anchor2; - axis1 = rb1.predicted_position * cparams.local_axis1; + axis1 = rb1.next_position * cparams.local_axis1; local_axis2 = cparams.local_axis2; basis1 = [ - rb1.predicted_position * cparams.basis1[0], - rb1.predicted_position * cparams.basis1[1], + rb1.next_position * cparams.basis1[0], + rb1.next_position * cparams.basis1[1], ]; local_basis2 = cparams.basis2; }; diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs index 99c1ec5..f32f49f 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -277,7 +277,7 @@ impl ParallelIslandSolver { rb.linvel += dvel.linear; rb.angvel += rb.effective_world_inv_inertia_sqrt.transform_vector(dvel.angular); rb.integrate(params.dt); - positions[rb.active_set_offset] = rb.position; + positions[rb.active_set_offset] = rb.next_position; } } @@ -298,7 +298,7 @@ impl ParallelIslandSolver { let batch_size = thread.batch_size; for handle in active_bodies[thread.position_writeback_index] { let rb = &mut bodies[handle.0]; - rb.set_position_internal(positions[rb.active_set_offset]); + rb.set_next_position(positions[rb.active_set_offset]); } } }) diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs index df0e3fc..ea92c59 100644 --- a/src/dynamics/solver/position_solver.rs +++ b/src/dynamics/solver/position_solver.rs @@ -25,7 +25,7 @@ impl PositionSolver { self.positions.extend( bodies .iter_active_island(island_id) - .map(|(_, b)| b.position), + .map(|(_, b)| b.next_position), ); for _ in 0..params.max_position_iterations { @@ -39,7 +39,7 @@ impl PositionSolver { } bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { - rb.set_position_internal(self.positions[rb.active_set_offset]) + rb.set_next_position(self.positions[rb.active_set_offset]) }); } } -- cgit From 7306821c460ca3f77e697c89a79393e61c126624 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Sun, 28 Mar 2021 11:26:53 +0200 Subject: Attenuate the warmstart impulse for CCD contacts. CCD contacts result in very strong, instantaneous, impulses. So it is preferable to attenuate their contribution to subsequent timesteps to avoid overshooting. --- src/dynamics/ccd/toi_entry.rs | 39 ++++++++++++---------- src/dynamics/integration_parameters.rs | 3 ++ src/dynamics/rigid_body.rs | 14 +++----- src/dynamics/solver/velocity_constraint.rs | 17 +++++++--- src/dynamics/solver/velocity_constraint_wide.rs | 27 +++++++++++---- src/dynamics/solver/velocity_ground_constraint.rs | 16 ++++++--- .../solver/velocity_ground_constraint_wide.rs | 26 +++++++++++---- 7 files changed, 93 insertions(+), 49 deletions(-) (limited to 'src/dynamics') diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs index 20f5268..6679a91 100644 --- a/src/dynamics/ccd/toi_entry.rs +++ b/src/dynamics/ccd/toi_entry.rs @@ -1,13 +1,7 @@ -use crate::data::Coarena; -use crate::dynamics::ccd::ccd_solver::CCDContact; -use crate::dynamics::ccd::CCDData; use crate::dynamics::{IntegrationParameters, RigidBody, RigidBodyHandle}; use crate::geometry::{Collider, ColliderHandle}; -use crate::math::{Isometry, Real}; -use crate::parry::query::PersistentQueryDispatcher; -use crate::utils::WCross; -use na::{RealField, Unit}; -use parry::query::{NonlinearRigidMotion, QueryDispatcher, TOI}; +use crate::math::Real; +use parry::query::{NonlinearRigidMotion, QueryDispatcher}; #[derive(Copy, Clone, Debug)] pub struct TOIEntry { @@ -41,7 +35,7 @@ impl TOIEntry { } } - pub fn try_from_colliders>( + pub fn try_from_colliders( params: &IntegrationParameters, query_dispatcher: &QD, ch1: ColliderHandle, @@ -54,7 +48,6 @@ impl TOIEntry { frozen2: Option, start_time: Real, end_time: Real, - body_params: &Coarena, ) -> Option { assert!(start_time <= end_time); @@ -62,7 +55,7 @@ impl TOIEntry { let linvel2 = frozen2.is_none() as u32 as Real * b2.linvel; let vel12 = linvel2 - linvel1; - let thickness = (c1.shape().ccd_thickness() + c2.shape().ccd_thickness()); + let thickness = c1.shape().ccd_thickness() + c2.shape().ccd_thickness(); if params.dt * vel12.norm() < thickness { return None; @@ -70,12 +63,9 @@ impl TOIEntry { let is_intersection_test = c1.is_sensor() || c2.is_sensor(); - let body_params1 = body_params.get(c1.parent.0)?; - let body_params2 = body_params.get(c2.parent.0)?; - // Compute the TOI. - let mut motion1 = body_params1.motion(params.dt, b1, 0.0); - let mut motion2 = body_params2.motion(params.dt, b2, 0.0); + let mut motion1 = Self::body_motion(params.dt, b1); + let mut motion2 = Self::body_motion(params.dt, b2); if let Some(t) = frozen1 { motion1.freeze(t); @@ -85,7 +75,6 @@ impl TOIEntry { motion2.freeze(t); } - let mut toi; let motion_c1 = motion1.prepend(*c1.position_wrt_parent()); let motion_c2 = motion2.prepend(*c2.position_wrt_parent()); @@ -112,7 +101,7 @@ impl TOIEntry { ) .ok(); - toi = res_toi??; + let toi = res_toi??; Some(Self::new( toi.toi, @@ -124,6 +113,20 @@ impl TOIEntry { 0, )) } + + fn body_motion(dt: Real, body: &RigidBody) -> NonlinearRigidMotion { + if body.should_resolve_ccd(dt) { + NonlinearRigidMotion::new( + 0.0, + body.position, + body.mass_properties.local_com, + body.linvel, + body.angvel, + ) + } else { + NonlinearRigidMotion::constant_position(body.next_position) + } + } } impl PartialOrd for TOIEntry { diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 8c0f26c..136e345 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -27,6 +27,8 @@ pub struct IntegrationParameters { /// Each cached impulse are multiplied by this coefficient in `[0, 1]` /// when they are re-used to initialize the solver (default `1.0`). pub warmstart_coeff: Real, + /// Correction factor to avoid large warmstart impulse after a strong impact. + pub warmstart_correction_slope: Real, /// 0-1: how much of the velocity to dampen out in the constraint solver? /// (default `1.0`). @@ -200,6 +202,7 @@ impl Default for IntegrationParameters { velocity_solve_fraction: 1.0, velocity_based_erp: 0.0, warmstart_coeff: 1.0, + warmstart_correction_slope: 1.0, allowed_linear_error: 0.005, prediction_distance: 0.002, allowed_angular_error: 0.001, diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 9ac4763..e557df0 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -204,8 +204,12 @@ impl RigidBody { self.flags.contains(RigidBodyFlags::CCD_ENABLED) } + pub fn is_moving_fast(&self, dt: Real) -> bool { + self.is_dynamic() && self.linvel.norm() * dt > self.ccd_thickness + } + pub(crate) fn should_resolve_ccd(&self, dt: Real) -> bool { - self.is_ccd_enabled() && self.is_dynamic() && self.linvel.norm() * dt > self.ccd_thickness + self.is_ccd_enabled() && self.is_moving_fast(dt) } /// Sets the rigid-body's mass properties. @@ -371,10 +375,6 @@ impl RigidBody { shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse() } - pub(crate) fn position_at_time(&self, dt: Real) -> Isometry { - self.integrate_velocity(dt) * self.position - } - pub(crate) fn integrate_next_position(&mut self, dt: Real, apply_damping: bool) { // TODO: do we want to apply damping before or after the velocity integration? if apply_damping { @@ -504,10 +504,6 @@ impl RigidBody { self.linvel = dpos.translation.vector * inv_dt; } - pub(crate) fn update_next_position(&mut self, dt: Real) { - self.next_position = self.integrate_velocity(dt) * self.position; - } - pub(crate) fn update_world_mass_properties(&mut self) { self.world_com = self.mass_properties.world_com(&self.position); self.effective_inv_mass = self.mass_properties.inv_mass; diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 3e8cb61..c339ce4 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -208,6 +208,8 @@ impl VelocityConstraint { let vel1 = rb1.linvel + rb1.angvel.gcross(dp1); let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); + let warmstart_correction; + constraint.limit = manifold_point.friction; constraint.manifold_contact_id[k] = manifold_point.contact_id; @@ -234,12 +236,15 @@ impl VelocityConstraint { rhs += manifold_point.dist.max(0.0) * inv_dt; rhs *= is_bouncy + is_resting * params.velocity_solve_fraction; rhs += is_resting * velocity_based_erp_inv_dt * manifold_point.dist.min(0.0); + warmstart_correction = (params.warmstart_correction_slope + / (rhs - manifold_point.prev_rhs).abs()) + .min(warmstart_coeff); constraint.elements[k].normal_part = VelocityConstraintNormalPart { gcross1, gcross2, rhs, - impulse: manifold_point.data.impulse * warmstart_coeff, + impulse: manifold_point.warmstart_impulse * warmstart_correction, r, }; } @@ -247,10 +252,12 @@ impl VelocityConstraint { // Tangent parts. { #[cfg(feature = "dim3")] - let impulse = - tangent_rot1 * manifold_points[k].data.tangent_impulse * warmstart_coeff; + let impulse = tangent_rot1 + * manifold_points[k].warmstart_tangent_impulse + * warmstart_correction; #[cfg(feature = "dim2")] - let impulse = [manifold_points[k].data.tangent_impulse * warmstart_coeff]; + let impulse = + [manifold_points[k].warmstart_tangent_impulse * warmstart_correction]; constraint.elements[k].tangent_part.impulse = impulse; for j in 0..DIM - 1 { @@ -332,6 +339,8 @@ impl VelocityConstraint { let contact_id = self.manifold_contact_id[k]; let active_contact = &mut manifold.points[contact_id as usize]; active_contact.data.impulse = self.elements[k].normal_part.impulse; + active_contact.data.rhs = self.elements[k].normal_part.rhs; + #[cfg(feature = "dim2")] { active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0]; diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 673af54..7d5f468 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -9,6 +9,7 @@ use crate::math::{ #[cfg(feature = "dim2")] use crate::utils::WBasis; use crate::utils::{WAngularInertia, WCross, WDot}; +use na::SimdComplexField; use num::Zero; use simba::simd::{SimdPartialOrd, SimdValue}; @@ -44,6 +45,7 @@ impl WVelocityConstraint { } let inv_dt = SimdReal::splat(params.inv_dt()); + let warmstart_correction_slope = SimdReal::splat(params.warmstart_correction_slope); let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction); let velocity_based_erp_inv_dt = SimdReal::splat(params.velocity_based_erp_inv_dt()); @@ -123,8 +125,11 @@ impl WVelocityConstraint { let tangent_velocity = Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]); - let impulse = - SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); + let impulse = SimdReal::from( + array![|ii| manifold_points[ii][k].warmstart_impulse; SIMD_WIDTH], + ); + let prev_rhs = + SimdReal::from(array![|ii| manifold_points[ii][k].prev_rhs; SIMD_WIDTH]); let dp1 = point - world_com1; let dp2 = point - world_com2; @@ -132,6 +137,8 @@ impl WVelocityConstraint { let vel1 = linvel1 + angvel1.gcross(dp1); let vel2 = linvel2 + angvel2.gcross(dp2); + let warmstart_correction; + constraint.limit = friction; constraint.manifold_contact_id[k] = array![|ii| manifold_points[ii][k].contact_id; SIMD_WIDTH]; @@ -150,12 +157,15 @@ impl WVelocityConstraint { rhs *= is_bouncy + is_resting * velocity_solve_fraction; rhs += dist.simd_min(SimdReal::zero()) * (velocity_based_erp_inv_dt * is_resting); + warmstart_correction = (warmstart_correction_slope + / (rhs - prev_rhs).simd_abs()) + .simd_min(warmstart_coeff); constraint.elements[k].normal_part = VelocityConstraintNormalPart { gcross1, gcross2, rhs, - impulse: impulse * warmstart_coeff, + impulse: impulse * warmstart_correction, r, }; } @@ -163,14 +173,15 @@ impl WVelocityConstraint { // tangent parts. #[cfg(feature = "dim2")] let impulse = [SimdReal::from( - array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH], - )]; + array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH], + ) * warmstart_correction]; #[cfg(feature = "dim3")] let impulse = tangent_rot1 * na::Vector2::from( - array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH], - ); + array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH], + ) + * warmstart_correction; constraint.elements[k].tangent_part.impulse = impulse; @@ -281,6 +292,7 @@ impl WVelocityConstraint { pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { for k in 0..self.num_contacts as usize { let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into(); + let rhs: [_; SIMD_WIDTH] = self.elements[k].normal_part.rhs.into(); #[cfg(feature = "dim2")] let tangent_impulses: [_; SIMD_WIDTH] = self.elements[k].tangent_part.impulse[0].into(); #[cfg(feature = "dim3")] @@ -292,6 +304,7 @@ impl WVelocityConstraint { let manifold = &mut manifolds_all[self.manifold_id[ii]]; let contact_id = self.manifold_contact_id[k][ii]; let active_contact = &mut manifold.points[contact_id as usize]; + active_contact.data.rhs = rhs[ii]; active_contact.data.impulse = impulses[ii]; #[cfg(feature = "dim2")] diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index b9c5236..0e195d5 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -133,6 +133,7 @@ impl VelocityGroundConstraint { let dp1 = manifold_point.point - rb1.world_com; let vel1 = rb1.linvel + rb1.angvel.gcross(dp1); let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); + let warmstart_correction; constraint.limit = manifold_point.friction; constraint.manifold_contact_id[k] = manifold_point.contact_id; @@ -153,11 +154,14 @@ impl VelocityGroundConstraint { rhs += manifold_point.dist.max(0.0) * inv_dt; rhs *= is_bouncy + is_resting * params.velocity_solve_fraction; rhs += is_resting * velocity_based_erp_inv_dt * manifold_point.dist.min(0.0); + warmstart_correction = (params.warmstart_correction_slope + / (rhs - manifold_point.prev_rhs).abs()) + .min(warmstart_coeff); constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart { gcross2, rhs, - impulse: manifold_point.data.impulse * warmstart_coeff, + impulse: manifold_point.warmstart_impulse * warmstart_correction, r, }; } @@ -165,10 +169,12 @@ impl VelocityGroundConstraint { // Tangent parts. { #[cfg(feature = "dim3")] - let impulse = - tangent_rot1 * manifold_points[k].data.tangent_impulse * warmstart_coeff; + let impulse = tangent_rot1 + * manifold_points[k].warmstart_tangent_impulse + * warmstart_correction; #[cfg(feature = "dim2")] - let impulse = [manifold_points[k].data.tangent_impulse * warmstart_coeff]; + let impulse = + [manifold_points[k].warmstart_tangent_impulse * warmstart_correction]; constraint.elements[k].tangent_part.impulse = impulse; for j in 0..DIM - 1 { @@ -237,6 +243,8 @@ impl VelocityGroundConstraint { let contact_id = self.manifold_contact_id[k]; let active_contact = &mut manifold.points[contact_id as usize]; active_contact.data.impulse = self.elements[k].normal_part.impulse; + active_contact.data.rhs = self.elements[k].normal_part.rhs; + #[cfg(feature = "dim2")] { active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0]; diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index ba1c46a..4237d99 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -10,6 +10,7 @@ use crate::math::{ #[cfg(feature = "dim2")] use crate::utils::WBasis; use crate::utils::{WAngularInertia, WCross, WDot}; +use na::SimdComplexField; use num::Zero; use simba::simd::{SimdPartialOrd, SimdValue}; @@ -77,6 +78,7 @@ impl WVelocityGroundConstraint { let warmstart_multiplier = SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff); + let warmstart_correction_slope = SimdReal::splat(params.warmstart_correction_slope); let num_active_contacts = manifolds[0].data.num_active_contacts(); #[cfg(feature = "dim2")] @@ -118,13 +120,17 @@ impl WVelocityGroundConstraint { let tangent_velocity = Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]); - let impulse = - SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); + let impulse = SimdReal::from( + array![|ii| manifold_points[ii][k].warmstart_impulse; SIMD_WIDTH], + ); + let prev_rhs = + SimdReal::from(array![|ii| manifold_points[ii][k].prev_rhs; SIMD_WIDTH]); let dp1 = point - world_com1; let dp2 = point - world_com2; let vel1 = linvel1 + angvel1.gcross(dp1); let vel2 = linvel2 + angvel2.gcross(dp2); + let warmstart_correction; constraint.limit = friction; constraint.manifold_contact_id[k] = @@ -142,11 +148,14 @@ impl WVelocityGroundConstraint { rhs *= is_bouncy + is_resting * velocity_solve_fraction; rhs += dist.simd_min(SimdReal::zero()) * (velocity_based_erp_inv_dt * is_resting); + warmstart_correction = (warmstart_correction_slope + / (rhs - prev_rhs).simd_abs()) + .simd_min(warmstart_coeff); constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart { gcross2, rhs, - impulse: impulse * warmstart_coeff, + impulse: impulse * warmstart_correction, r, }; } @@ -154,13 +163,14 @@ impl WVelocityGroundConstraint { // tangent parts. #[cfg(feature = "dim2")] let impulse = [SimdReal::from( - array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH], - )]; + array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH], + ) * warmstart_correction]; #[cfg(feature = "dim3")] let impulse = tangent_rot1 * na::Vector2::from( - array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH], - ); + array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH], + ) + * warmstart_correction; constraint.elements[k].tangent_part.impulse = impulse; for j in 0..DIM - 1 { @@ -237,6 +247,7 @@ impl WVelocityGroundConstraint { // FIXME: duplicated code. This is exactly the same as in the non-ground velocity constraint. pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { for k in 0..self.num_contacts as usize { + let rhs: [_; SIMD_WIDTH] = self.elements[k].normal_part.rhs.into(); let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into(); #[cfg(feature = "dim2")] let tangent_impulses: [_; SIMD_WIDTH] = self.elements[k].tangent_part.impulse[0].into(); @@ -249,6 +260,7 @@ impl WVelocityGroundConstraint { let manifold = &mut manifolds_all[self.manifold_id[ii]]; let contact_id = self.manifold_contact_id[k][ii]; let active_contact = &mut manifold.points[contact_id as usize]; + active_contact.data.rhs = rhs[ii]; active_contact.data.impulse = impulses[ii]; #[cfg(feature = "dim2")] -- cgit From 8173e7ada2e3f5c99de53b532adc085a26c1cefd Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 29 Mar 2021 14:54:54 +0200 Subject: Allow collider modification after its insersion to the ColliderSet. --- src/dynamics/coefficient_combine_rule.rs | 10 +++++++ src/dynamics/rigid_body.rs | 14 +++++---- src/dynamics/rigid_body_set.rs | 49 ++++++++++++++++++++++++++------ 3 files changed, 59 insertions(+), 14 deletions(-) (limited to 'src/dynamics') diff --git a/src/dynamics/coefficient_combine_rule.rs b/src/dynamics/coefficient_combine_rule.rs index 2c66888..1bef022 100644 --- a/src/dynamics/coefficient_combine_rule.rs +++ b/src/dynamics/coefficient_combine_rule.rs @@ -21,6 +21,16 @@ pub enum CoefficientCombineRule { } impl CoefficientCombineRule { + pub fn from_value(val: u8) -> Self { + match val { + 0 => CoefficientCombineRule::Average, + 1 => CoefficientCombineRule::Min, + 2 => CoefficientCombineRule::Multiply, + 3 => CoefficientCombineRule::Max, + _ => panic!("Invalid coefficient combine rule."), + } + } + pub(crate) fn combine(coeff1: Real, coeff2: Real, rule_value1: u8, rule_value2: u8) -> Real { let effective_rule = rule_value1.max(rule_value2); diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index e557df0..3c6bd65 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -42,7 +42,7 @@ bitflags::bitflags! { bitflags::bitflags! { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] - /// Flags affecting the behavior of the constraints solver for a given contact manifold. + /// Flags describing how the rigid-body has been modified by the user. pub(crate) struct RigidBodyChanges: u32 { const MODIFIED = 1 << 0; const POSITION = 1 << 1; @@ -204,7 +204,7 @@ impl RigidBody { self.flags.contains(RigidBodyFlags::CCD_ENABLED) } - pub fn is_moving_fast(&self, dt: Real) -> bool { + pub(crate) fn is_moving_fast(&self, dt: Real) -> bool { self.is_dynamic() && self.linvel.norm() * dt > self.ccd_thickness } @@ -302,9 +302,13 @@ impl RigidBody { pub(crate) fn update_colliders_positions(&mut self, colliders: &mut ColliderSet) { for handle in &self.colliders { - let collider = &mut colliders[*handle]; - collider.prev_position = self.position; - collider.position = self.position * collider.delta; + // NOTE: we don't use `get_mut_internal` here because we want to + // benefit from the modification tracking to know the colliders + // we need to update the broad-phase and narrow-phase for. + let collider = colliders + .get_mut_internal_with_modification_tracking(*handle) + .unwrap(); + collider.set_position(self.position * collider.delta); } } diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index f459d4f..2ae298f 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -220,13 +220,17 @@ impl RigidBodySet { /// /// Using this is discouraged in favor of `self.get_mut(handle)` which does not /// suffer form the ABA problem. + #[cfg(not(feature = "dev-remove-slow-accessors"))] pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut RigidBody, RigidBodyHandle)> { - let result = self.bodies.get_unknown_gen_mut(i)?; - if !self.modified_all_bodies && !result.0.changes.contains(RigidBodyChanges::MODIFIED) { - result.0.changes = RigidBodyChanges::MODIFIED; - self.modified_bodies.push(RigidBodyHandle(result.1)); - } - Some((result.0, RigidBodyHandle(result.1))) + let (rb, handle) = self.bodies.get_unknown_gen_mut(i)?; + let handle = RigidBodyHandle(handle); + Self::mark_as_modified( + handle, + rb, + &mut self.modified_bodies, + self.modified_all_bodies, + ); + Some((rb, handle)) } /// Gets the rigid-body with the given handle. @@ -247,6 +251,7 @@ impl RigidBodySet { } /// Gets a mutable reference to the rigid-body with the given handle. + #[cfg(not(feature = "dev-remove-slow-accessors"))] pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> { let result = self.bodies.get_mut(handle.0)?; Self::mark_as_modified( @@ -262,6 +267,22 @@ impl RigidBodySet { self.bodies.get_mut(handle.0) } + // Just a very long name instead of `.get_mut` to make sure + // this is really the method we wanted to use instead of `get_mut_internal`. + pub(crate) fn get_mut_internal_with_modification_tracking( + &mut self, + handle: RigidBodyHandle, + ) -> Option<&mut RigidBody> { + let result = self.bodies.get_mut(handle.0)?; + Self::mark_as_modified( + handle, + result, + &mut self.modified_bodies, + self.modified_all_bodies, + ); + Some(result) + } + pub(crate) fn get2_mut_internal( &mut self, h1: RigidBodyHandle, @@ -276,6 +297,7 @@ impl RigidBodySet { } /// Iterates mutably through all the rigid-bodies on this set. + #[cfg(not(feature = "dev-remove-slow-accessors"))] pub fn iter_mut(&mut self) -> impl Iterator { self.modified_bodies.clear(); self.modified_all_bodies = true; @@ -317,6 +339,7 @@ impl RigidBodySet { /// Applies the given function on all the active dynamic rigid-bodies /// contained by this set. #[inline(always)] + #[cfg(not(feature = "dev-remove-slow-accessors"))] pub fn foreach_active_dynamic_body_mut( &mut self, mut f: impl FnMut(RigidBodyHandle, &mut RigidBody), @@ -467,7 +490,7 @@ impl RigidBodySet { rb.changes = RigidBodyChanges::empty(); } - pub(crate) fn maintain(&mut self, colliders: &mut ColliderSet) { + pub(crate) fn handle_user_changes(&mut self, colliders: &mut ColliderSet) { if self.modified_all_bodies { for (handle, rb) in self.bodies.iter_mut() { Self::maintain_one( @@ -651,8 +674,16 @@ impl Index for RigidBodySet { } } +#[cfg(not(feature = "dev-remove-slow-accessors"))] impl IndexMut for RigidBodySet { - fn index_mut(&mut self, index: RigidBodyHandle) -> &mut RigidBody { - &mut self.bodies[index.0] + fn index_mut(&mut self, handle: RigidBodyHandle) -> &mut RigidBody { + let rb = &mut self.bodies[handle.0]; + Self::mark_as_modified( + handle, + rb, + &mut self.modified_bodies, + self.modified_all_bodies, + ); + rb } } -- cgit From a733f97028f5cd532212572f9561ab64e09f002b Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 29 Mar 2021 17:21:49 +0200 Subject: Implement the ability to run multiple CCD substeps. --- src/dynamics/ccd/toi_entry.rs | 16 +++++----------- src/dynamics/integration_parameters.rs | 12 ++++++++++++ src/dynamics/rigid_body.rs | 32 +++++++++++++++++++++++++++----- 3 files changed, 44 insertions(+), 16 deletions(-) (limited to 'src/dynamics') diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs index 6679a91..3916a4b 100644 --- a/src/dynamics/ccd/toi_entry.rs +++ b/src/dynamics/ccd/toi_entry.rs @@ -1,4 +1,4 @@ -use crate::dynamics::{IntegrationParameters, RigidBody, RigidBodyHandle}; +use crate::dynamics::{RigidBody, RigidBodyHandle}; use crate::geometry::{Collider, ColliderHandle}; use crate::math::Real; use parry::query::{NonlinearRigidMotion, QueryDispatcher}; @@ -36,7 +36,6 @@ impl TOIEntry { } pub fn try_from_colliders( - params: &IntegrationParameters, query_dispatcher: &QD, ch1: ColliderHandle, ch2: ColliderHandle, @@ -56,16 +55,11 @@ impl TOIEntry { let vel12 = linvel2 - linvel1; let thickness = c1.shape().ccd_thickness() + c2.shape().ccd_thickness(); - - if params.dt * vel12.norm() < thickness { - return None; - } - let is_intersection_test = c1.is_sensor() || c2.is_sensor(); // Compute the TOI. - let mut motion1 = Self::body_motion(params.dt, b1); - let mut motion2 = Self::body_motion(params.dt, b2); + let mut motion1 = Self::body_motion(b1); + let mut motion2 = Self::body_motion(b2); if let Some(t) = frozen1 { motion1.freeze(t); @@ -114,8 +108,8 @@ impl TOIEntry { )) } - fn body_motion(dt: Real, body: &RigidBody) -> NonlinearRigidMotion { - if body.should_resolve_ccd(dt) { + fn body_motion(body: &RigidBody) -> NonlinearRigidMotion { + if body.is_ccd_active() { NonlinearRigidMotion::new( 0.0, body.position, diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 136e345..615bfee 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -6,6 +6,17 @@ use crate::math::Real; pub struct IntegrationParameters { /// The timestep length (default: `1.0 / 60.0`) pub dt: Real, + /// Minimum timestep size when using CCD with multiple substeps (default `1.0 / 60.0 / 100.0`) + /// + /// 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`. + /// + /// Setting this to a large value will reduce the opportunity to performing + /// CCD substepping, resulting in potentially more time dropped by the + /// motion-clamping mechanism. Setting this to an very small value may lead + /// 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`). // /// @@ -195,6 +206,7 @@ impl Default for IntegrationParameters { fn default() -> Self { Self { dt: 1.0 / 60.0, + min_ccd_dt: 1.0 / 60.0 / 100.0, // multithreading_enabled: true, return_after_ccd_substep: false, erp: 0.2, diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 3c6bd65..08c5629 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -5,7 +5,7 @@ use crate::geometry::{ use crate::math::{ AngVector, AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, }; -use crate::utils::{self, WCross, WDot}; +use crate::utils::{self, WAngularInertia, WCross, WDot}; use na::ComplexField; use num::Zero; @@ -37,6 +37,7 @@ bitflags::bitflags! { const ROTATION_LOCKED_Y = 1 << 2; const ROTATION_LOCKED_Z = 1 << 3; const CCD_ENABLED = 1 << 4; + const CCD_ACTIVE = 1 << 5; } } @@ -204,12 +205,20 @@ impl RigidBody { self.flags.contains(RigidBodyFlags::CCD_ENABLED) } - pub(crate) fn is_moving_fast(&self, dt: Real) -> bool { - self.is_dynamic() && self.linvel.norm() * dt > self.ccd_thickness + // This is different from `is_ccd_enabled`. This checks that CCD + // is active for this rigid-body, i.e., if it was seen to move fast + // enough to justify a CCD run. + pub(crate) fn is_ccd_active(&self) -> bool { + self.flags.contains(RigidBodyFlags::CCD_ACTIVE) + } + + pub(crate) fn update_ccd_active_flag(&mut self, dt: Real) { + let ccd_active = self.is_ccd_enabled() && self.is_moving_fast(dt); + self.flags.set(RigidBodyFlags::CCD_ACTIVE, ccd_active); } - pub(crate) fn should_resolve_ccd(&self, dt: Real) -> bool { - self.is_ccd_enabled() && self.is_moving_fast(dt) + pub(crate) fn is_moving_fast(&self, dt: Real) -> bool { + self.is_dynamic() && self.linvel.norm() * dt > self.ccd_thickness } /// Sets the rigid-body's mass properties. @@ -373,6 +382,19 @@ impl RigidBody { !self.linvel.is_zero() || !self.angvel.is_zero() } + pub(crate) fn predict_position_using_velocity_and_forces(&self, dt: Real) -> Isometry { + let dlinvel = self.force * (self.effective_inv_mass * dt); + let dangvel = self + .effective_world_inv_inertia_sqrt + .transform_vector(self.torque * dt); + let linvel = self.linvel + dlinvel; + let angvel = self.angvel + dangvel; + + let com = self.position * self.mass_properties.local_com; + let shift = Translation::from(com.coords); + shift * Isometry::new(linvel * dt, angvel * dt) * shift.inverse() * self.position + } + pub(crate) fn integrate_velocity(&self, dt: Real) -> Isometry { let com = self.position * self.mass_properties.local_com; let shift = Translation::from(com.coords); -- cgit From c3a0c67272e09d3bb4f80aca145ff580d0172745 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 29 Mar 2021 17:23:05 +0200 Subject: Add missing files. --- src/dynamics/ccd/ccd_solver.rs | 398 +++++++++++++++++++++++++++++++++++++++++ src/dynamics/ccd/mod.rs | 5 + 2 files changed, 403 insertions(+) create mode 100644 src/dynamics/ccd/ccd_solver.rs create mode 100644 src/dynamics/ccd/mod.rs (limited to 'src/dynamics') diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs new file mode 100644 index 0000000..2d39956 --- /dev/null +++ b/src/dynamics/ccd/ccd_solver.rs @@ -0,0 +1,398 @@ +use super::TOIEntry; +use crate::dynamics::{RigidBodyHandle, RigidBodySet}; +use crate::geometry::{ColliderSet, IntersectionEvent}; +use crate::math::Real; +use crate::parry::utils::SortedPair; +use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode}; +use parry::query::{DefaultQueryDispatcher, QueryDispatcher}; +use parry::utils::hashmap::HashMap; +use std::collections::BinaryHeap; + +pub enum PredictedImpacts { + Impacts(HashMap), + ImpactsAfterEndTime(Real), + NoImpacts, +} + +/// Solver responsible for performing motion-clamping on fast-moving bodies. +pub struct CCDSolver { + query_pipeline: QueryPipeline, +} + +impl CCDSolver { + /// Initializes a new CCD solver + pub fn new() -> Self { + Self::with_query_dispatcher(DefaultQueryDispatcher) + } + + /// Initializes a CCD solver with a custom `QueryDispatcher` used for computing time-of-impacts. + /// + /// Use this constructor in order to use a custom `QueryDispatcher` that is aware of your own + /// user-defined shapes. + pub fn with_query_dispatcher(d: D) -> Self + where + D: 'static + QueryDispatcher, + { + CCDSolver { + query_pipeline: QueryPipeline::with_query_dispatcher(d), + } + } + + /// Apply motion-clamping to the bodies affected by the given `impacts`. + /// + /// The `impacts` should be the result of a previous call to `self.predict_next_impacts`. + pub fn clamp_motions(&self, dt: Real, bodies: &mut RigidBodySet, impacts: &PredictedImpacts) { + match impacts { + PredictedImpacts::Impacts(tois) => { + for (handle, toi) in tois { + if let Some(body) = bodies.get_mut_internal(*handle) { + let min_toi = + (body.ccd_thickness * 0.15 * crate::utils::inv(body.linvel.norm())) + .min(dt); + // println!("Min toi: {}, Toi: {}", min_toi, toi); + body.integrate_next_position(toi.max(min_toi), false); + } + } + } + _ => {} + } + } + + /// Updates the set of bodies that needs CCD to be resolved. + /// + /// Returns `true` if any rigid-body must have CCD resolved. + pub fn update_ccd_active_flags(&self, bodies: &mut RigidBodySet, dt: Real) -> bool { + let mut ccd_active = false; + + bodies.foreach_active_dynamic_body_mut_internal(|_, body| { + body.update_ccd_active_flag(dt); + ccd_active = ccd_active || body.is_ccd_active(); + }); + + ccd_active + } + + /// Find the first time a CCD-enabled body has a non-sensor collider hitting another non-sensor collider. + pub fn find_first_impact( + &mut self, + dt: Real, + bodies: &RigidBodySet, + colliders: &ColliderSet, + ) -> Option { + // Update the query pipeline. + self.query_pipeline.updat