aboutsummaryrefslogtreecommitdiff
path: root/src/dynamics
diff options
context:
space:
mode:
Diffstat (limited to 'src/dynamics')
-rw-r--r--src/dynamics/ccd/ccd_solver.rs44
-rw-r--r--src/dynamics/ccd/toi_entry.rs29
-rw-r--r--src/dynamics/coefficient_combine_rule.rs2
-rw-r--r--src/dynamics/rigid_body.rs66
-rw-r--r--src/dynamics/solver/interaction_groups.rs7
5 files changed, 127 insertions, 21 deletions
diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs
index 2d39956..41b195c 100644
--- a/src/dynamics/ccd/ccd_solver.rs
+++ b/src/dynamics/ccd/ccd_solver.rs
@@ -1,6 +1,6 @@
use super::TOIEntry;
use crate::dynamics::{RigidBodyHandle, RigidBodySet};
-use crate::geometry::{ColliderSet, IntersectionEvent};
+use crate::geometry::{ColliderSet, IntersectionEvent, NarrowPhase};
use crate::math::Real;
use crate::parry::utils::SortedPair;
use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode};
@@ -44,11 +44,13 @@ impl CCDSolver {
pub fn clamp_motions(&self, dt: Real, bodies: &mut RigidBodySet, impacts: &PredictedImpacts) {
match impacts {
PredictedImpacts::Impacts(tois) => {
+ // println!("Num to clamp: {}", tois.len());
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);
+ let min_toi = (body.ccd_thickness
+ * 0.15
+ * crate::utils::inv(body.max_point_velocity()))
+ .min(dt);
// println!("Min toi: {}, Toi: {}", min_toi, toi);
body.integrate_next_position(toi.max(min_toi), false);
}
@@ -61,11 +63,18 @@ impl CCDSolver {
/// 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 {
+ pub fn update_ccd_active_flags(
+ &self,
+ bodies: &mut RigidBodySet,
+ dt: Real,
+ include_forces: bool,
+ ) -> bool {
let mut ccd_active = false;
+ // println!("Checking CCD activation");
bodies.foreach_active_dynamic_body_mut_internal(|_, body| {
- body.update_ccd_active_flag(dt);
+ body.update_ccd_active_flag(dt, include_forces);
+ // println!("CCD is active: {}, for {:?}", ccd_active, handle);
ccd_active = ccd_active || body.is_ccd_active();
});
@@ -78,6 +87,7 @@ impl CCDSolver {
dt: Real,
bodies: &RigidBodySet,
colliders: &ColliderSet,
+ narrow_phase: &NarrowPhase,
) -> Option<Real> {
// Update the query pipeline.
self.query_pipeline.update_with_mode(
@@ -127,6 +137,12 @@ impl CCDSolver {
return true;
}
+ let smallest_dist = narrow_phase
+ .contact_pair(*ch1, *ch2)
+ .and_then(|p| p.find_deepest_contact())
+ .map(|c| c.1.dist)
+ .unwrap_or(0.0);
+
let b1 = bodies.get(bh1).unwrap();
let b2 = bodies.get(bh2).unwrap();
@@ -142,6 +158,7 @@ impl CCDSolver {
None,
0.0,
min_toi,
+ smallest_dist,
) {
min_toi = min_toi.min(toi.toi);
}
@@ -166,6 +183,7 @@ impl CCDSolver {
dt: Real,
bodies: &RigidBodySet,
colliders: &ColliderSet,
+ narrow_phase: &NarrowPhase,
events: &dyn EventHandler,
) -> PredictedImpacts {
let mut frozen = HashMap::<_, Real>::default();
@@ -218,6 +236,12 @@ impl CCDSolver {
let b1 = bodies.get(bh1).unwrap();
let b2 = bodies.get(bh2).unwrap();
+ let smallest_dist = narrow_phase
+ .contact_pair(ch1, *ch2)
+ .and_then(|p| p.find_deepest_contact())
+ .map(|c| c.1.dist)
+ .unwrap_or(0.0);
+
if let Some(toi) = TOIEntry::try_from_colliders(
self.query_pipeline.query_dispatcher(),
ch1,
@@ -232,6 +256,7 @@ impl CCDSolver {
// NOTE: we use dt here only once we know that
// there is at least one TOI before dt.
min_overstep,
+ smallest_dist,
) {
if toi.toi > dt {
min_overstep = min_overstep.min(toi.toi);
@@ -331,6 +356,12 @@ impl CCDSolver {
return true;
}
+ let smallest_dist = narrow_phase
+ .contact_pair(*ch1, *ch2)
+ .and_then(|p| p.find_deepest_contact())
+ .map(|c| c.1.dist)
+ .unwrap_or(0.0);
+
if let Some(toi) = TOIEntry::try_from_colliders(
self.query_pipeline.query_dispatcher(),
*ch1,
@@ -343,6 +374,7 @@ impl CCDSolver {
frozen2.copied(),
start_time,
dt,
+ smallest_dist,
) {
all_toi.push(toi);
}
diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs
index 3916a4b..cc6773c 100644
--- a/src/dynamics/ccd/toi_entry.rs
+++ b/src/dynamics/ccd/toi_entry.rs
@@ -47,16 +47,35 @@ impl TOIEntry {
frozen2: Option<Real>,
start_time: Real,
end_time: Real,
+ smallest_contact_dist: Real,
) -> Option<Self> {
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();
+ let linvel1 = frozen1.is_none() as u32 as Real * b1.linvel();
+ let linvel2 = frozen2.is_none() as u32 as Real * b2.linvel();
+ let angvel1 = frozen1.is_none() as u32 as Real * b1.angvel();
+ let angvel2 = frozen2.is_none() as u32 as Real * b2.angvel();
+
+ #[cfg(feature = "dim2")]
+ let vel12 = (linvel2 - linvel1).norm()
+ + angvel1.abs() * b1.ccd_max_dist
+ + angvel2.abs() * b2.ccd_max_dist;
+ #[cfg(feature = "dim3")]
+ let vel12 = (linvel2 - linvel1).norm()
+ + angvel1.norm() * b1.ccd_max_dist
+ + angvel2.norm() * b2.ccd_max_dist;
+
+ // We may be slightly over-conservative by taking the `max(0.0)` here.
+ // But removing the `max` doesn't really affect performances so let's
+ // keep it since more conservatism is good at this stage.
+ let thickness = (c1.shape().ccd_thickness() + c2.shape().ccd_thickness())
+ + smallest_contact_dist.max(0.0);
let is_intersection_test = c1.is_sensor() || c2.is_sensor();
+ if (end_time - start_time) * vel12 < thickness {
+ return None;
+ }
+
// Compute the TOI.
let mut motion1 = Self::body_motion(b1);
let mut motion2 = Self::body_motion(b2);
diff --git a/src/dynamics/coefficient_combine_rule.rs b/src/dynamics/coefficient_combine_rule.rs
index 1bef022..9b3b9ee 100644
--- a/src/dynamics/coefficient_combine_rule.rs
+++ b/src/dynamics/coefficient_combine_rule.rs
@@ -21,7 +21,7 @@ pub enum CoefficientCombineRule {
}
impl CoefficientCombineRule {
- pub fn from_value(val: u8) -> Self {
+ pub(crate) fn from_value(val: u8) -> Self {
match val {
0 => CoefficientCombineRule::Average,
1 => CoefficientCombineRule::Min,
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 08c5629..15ddce3 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -113,6 +113,7 @@ pub struct RigidBody {
/// User-defined data associated to this rigid-body.
pub user_data: u128,
pub(crate) ccd_thickness: Real,
+ pub(crate) ccd_max_dist: Real,
}
impl RigidBody {
@@ -146,6 +147,7 @@ impl RigidBody {
dominance_group: 0,
user_data: 0,
ccd_thickness: Real::MAX,
+ ccd_max_dist: 0.0,
}
}
@@ -172,8 +174,6 @@ impl RigidBody {
self.linvel += linear_acc * dt;
self.angvel += angular_acc * dt;
- self.force = na::zero();
- self.torque = na::zero();
}
/// The mass properties of this rigid-body.
@@ -208,17 +208,56 @@ impl RigidBody {
// 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 {
+ /// Is CCD active for this rigid-body?
+ ///
+ /// The CCD is considered active if the rigid-body is moving at
+ /// a velocity greater than an automatically-computed threshold.
+ ///
+ /// This is not the same as `self.is_ccd_enabled` which only
+ /// checks if CCD is allowed to run for this rigid-body or if
+ /// it is completely disabled (independently from its velocity).
+ pub 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);
+ pub(crate) fn update_ccd_active_flag(&mut self, dt: Real, include_forces: bool) {
+ let ccd_active = self.is_ccd_enabled() && self.is_moving_fast(dt, include_forces);
self.flags.set(RigidBodyFlags::CCD_ACTIVE, ccd_active);
}
- pub(crate) fn is_moving_fast(&self, dt: Real) -> bool {
- self.is_dynamic() && self.linvel.norm() * dt > self.ccd_thickness
+ pub(crate) fn is_moving_fast(&self, dt: Real, include_forces: bool) -> bool {
+ if self.is_dynamic() {
+ // NOTE: for the threshold we don't use the exact CCD thickness. Theoretically, we
+ // should use `self.ccd_thickness - smallest_contact_dist` where `smallest_contact_dist`
+ // is the deepest contact (the contact with the largest penetration depth, i.e., the
+ // negative `dist` with the largest absolute value.
+ // However, getting this penetration depth assumes querying the contact graph from
+ // the narrow-phase, which can be pretty expensive. So we use the CCD thickness
+ // divided by 10 right now. We will see in practice if this value is OK or if we
+ // should use a smaller (to be less conservative) or larger divisor (to be more conservative).
+ let threshold = self.ccd_thickness / 10.0;
+
+ if include_forces {
+ let linear_part = (self.linvel + self.force * dt).norm();
+ #[cfg(feature = "dim2")]
+ let angular_part = (self.angvel + self.torque * dt).abs() * self.ccd_max_dist;
+ #[cfg(feature = "dim3")]
+ let angular_part = (self.angvel + self.torque * dt).norm() * self.ccd_max_dist;
+ let vel_with_forces = linear_part + angular_part;
+ vel_with_forces > threshold
+ } else {
+ self.max_point_velocity() * dt > threshold
+ }
+ } else {
+ false
+ }
+ }
+
+ pub(crate) fn max_point_velocity(&self) -> Real {
+ #[cfg(feature = "dim2")]
+ return self.linvel.norm() + self.angvel.abs() * self.ccd_max_dist;
+ #[cfg(feature = "dim3")]
+ return self.linvel.norm() + self.angvel.norm() * self.ccd_max_dist;
}
/// Sets the rigid-body's mass properties.
@@ -301,6 +340,13 @@ impl RigidBody {
self.ccd_thickness = self.ccd_thickness.min(coll.shape().ccd_thickness());
+ let shape_bsphere = coll
+ .shape()
+ .compute_bounding_sphere(coll.position_wrt_parent());
+ self.ccd_max_dist = self
+ .ccd_max_dist
+ .max(shape_bsphere.center.coords.norm() + shape_bsphere.radius);
+
let mass_properties = coll
.mass_properties()
.transform_by(coll.position_wrt_parent());
@@ -311,7 +357,7 @@ impl RigidBody {
pub(crate) fn update_colliders_positions(&mut self, colliders: &mut ColliderSet) {
for handle in &self.colliders {
- // NOTE: we don't use `get_mut_internal` here because we want to
+ // NOTE: we use `get_mut_internal_with_modification_tracking` 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
@@ -382,7 +428,9 @@ impl RigidBody {
!self.linvel.is_zero() || !self.angvel.is_zero()
}
- pub(crate) fn predict_position_using_velocity_and_forces(&self, dt: Real) -> Isometry<Real> {
+ /// Computes the predict position of this rigid-body after `dt` seconds, taking
+ /// into account its velocities and external forces applied to it.
+ pub fn predict_position_using_velocity_and_forces(&self, dt: Real) -> Isometry<Real> {
let dlinvel = self.force * (self.effective_inv_mass * dt);
let dangvel = self
.effective_world_inv_inertia_sqrt
diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs
index 0f01798..ff4ceed 100644
--- a/src/dynamics/solver/interaction_groups.rs
+++ b/src/dynamics/solver/interaction_groups.rs
@@ -157,6 +157,13 @@ impl InteractionGroups {
}
}
+ pub fn clear(&mut self) {
+ self.buckets.clear();
+ self.body_masks.clear();
+ self.grouped_interactions.clear();
+ self.nongrouped_interactions.clear();
+ }
+
// FIXME: there is a lot of duplicated code with group_manifolds here.
// But we don't refactor just now because we may end up with distinct
// grouping strategies in the future.
generated by cgit (git 2.34.1) at 2026-02-04 21:39:21 +0000