diff options
Diffstat (limited to 'src')
46 files changed, 4059 insertions, 1352 deletions
diff --git a/src/counters/collision_detection_counters.rs b/src/counters/collision_detection_counters.rs index d164452..90c5141 100644 --- a/src/counters/collision_detection_counters.rs +++ b/src/counters/collision_detection_counters.rs @@ -21,6 +21,13 @@ impl CollisionDetectionCounters { narrow_phase_time: Timer::new(), } } + + /// Resets all the coounters and timers. + pub fn reset(&mut self) { + self.ncontact_pairs = 0; + self.broad_phase_time.reset(); + self.narrow_phase_time.reset(); + } } impl Display for CollisionDetectionCounters { diff --git a/src/counters/mod.rs b/src/counters/mod.rs index c172350..4d4d05d 100644 --- a/src/counters/mod.rs +++ b/src/counters/mod.rs @@ -114,6 +114,18 @@ impl Counters { pub fn set_ncontact_pairs(&mut self, n: usize) { self.cd.ncontact_pairs = n; } + + /// Resets all the counters and timers. + pub fn reset(&mut self) { + if self.enabled { + self.step_time.reset(); + self.custom.reset(); + self.stages.reset(); + self.cd.reset(); + self.solver.reset(); + self.ccd.reset(); + } + } } macro_rules! measure_method { diff --git a/src/counters/stages_counters.rs b/src/counters/stages_counters.rs index 856b759..b61adb7 100644 --- a/src/counters/stages_counters.rs +++ b/src/counters/stages_counters.rs @@ -27,6 +27,15 @@ impl StagesCounters { ccd_time: Timer::new(), } } + + /// Resets all the counters and timers. + pub fn reset(&mut self) { + self.update_time.reset(); + self.collision_detection_time.reset(); + self.island_construction_time.reset(); + self.solver_time.reset(); + self.ccd_time.reset(); + } } impl Display for StagesCounters { diff --git a/src/data/coarena.rs b/src/data/coarena.rs index 78cbfa7..c25cc55 100644 --- a/src/data/coarena.rs +++ b/src/data/coarena.rs @@ -29,6 +29,20 @@ impl<T> Coarena<T> { .and_then(|(gg, t)| if g == *gg { Some(t) } else { None }) } + /// Inserts an element into this coarena. + pub fn insert(&mut self, a: Index, value: T) + where + T: Clone + Default, + { + let (i1, g1) = a.into_raw_parts(); + + if self.data.len() <= i1 { + self.data.resize(i1 + 1, (u32::MAX as u64, T::default())); + } + + self.data[i1] = (g1, value); + } + /// Ensure that elements at the two given indices exist in this coarena, and return their reference. /// /// Missing elements are created automatically and initialized with the `default` value. diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs new file mode 100644 index 0000000..134b8a6 --- /dev/null +++ b/src/dynamics/ccd/ccd_solver.rs @@ -0,0 +1,430 @@ +use super::TOIEntry; +use crate::dynamics::{RigidBodyHandle, RigidBodySet}; +use crate::geometry::{ColliderSet, IntersectionEvent, NarrowPhase}; +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<RigidBodyHandle, Real>), + 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: 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) => { + // 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.max_point_velocity())) + .min(dt); + // println!("Min toi: {}, Toi: {}", min_toi, toi); + body.integrate_next_position(toi.max(min_toi)); + } + } + } + _ => {} + } + } + + /// 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, + 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, include_forces); + // println!("CCD is active: {}, for {:?}", ccd_active, handle); + 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, + narrow_phase: &NarrowPhase, + ) -> Option<Real> { + // Update the query pipeline. + self.query_pipeline.update_with_mode( + bodies, + colliders, + QueryPipelineMode::SweepTestWithPredictedPosition { dt }, + ); + + let mut pairs_seen = HashMap::default(); + let mut min_toi = dt; + + for (_, rb1) in bodies.iter_active_dynamic() { + if rb1.is_ccd_active() { + let predicted_body_pos1 = rb1.predict_position_using_velocity_and_forces(dt); + + for ch1 in &rb1.colliders { + let co1 = &colliders[*ch1]; + + if co1.is_sensor() { + continue; // Ignore sensors. + } + + let aabb1 = + co1.compute_swept_aabb(&(predicted_body_pos1 * co1.position_wrt_parent())); + + self.query_pipeline + .colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| { + if *ch1 == *ch2 { + // Ignore self-intersection. + return true; + } + + if pairs_seen + .insert( + SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0), + (), + ) + .is_none() + { + let c1 = colliders.get(*ch1).unwrap(); + let c2 = colliders.get(*ch2).unwrap(); + let bh1 = c1.parent(); + let bh2 = c2.parent(); + + if bh1 == bh2 || (c1.is_sensor() || c2.is_sensor()) { + // Ignore self-intersection and sensors. + 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(); + + if let Some(toi) = TOIEntry::try_from_colliders( + self.query_pipeline.query_dispatcher(), + *ch1, + *ch2, + c1, + c2, + b1, + b2, + None, + None, + 0.0, + min_toi, + smallest_dist, + ) { + min_toi = min_toi.min(toi.toi); + } + } + + true + }); + } + } + } + + if min_toi < dt { + Some(min_toi) + } else { + None + } + } + + /// Outputs the set of bodies as well as their first time-of-impact event. + pub fn predict_impacts_at_next_positions( + &mut self, + dt: Real, + bodies: &RigidBodySet, + colliders: &ColliderSet, + narrow_phase: &NarrowPhase, + events: &dyn EventHandler, + ) -> PredictedImpacts { + let mut frozen = HashMap::<_, Real>::default(); + let mut all_toi = BinaryHeap::new(); + let mut pairs_seen = HashMap::default(); + let mut min_overstep = dt; + + // Update the query pipeline. + self.query_pipeline.update_with_mode( + bodies, + colliders, + QueryPipelineMode::SweepTestWithNextPosition, + ); + + /* + * + * First, collect all TOIs. + * + */ + // TODO: don't iterate through all the colliders. + for (ch1, co1) in colliders.iter() { + let rb1 = &bodies[co1.parent()]; + if rb1.is_ccd_active() { + let aabb = co1.compute_swept_aabb(&(rb1.next_position * co1.position_wrt_parent())); + + self.query_pipeline + .colliders_with_aabb_intersecting_aabb(&aabb, |ch2| { + if ch1 == *ch2 { + // Ignore self-intersection. + return true; + } + + if pairs_seen + .insert( + SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0), + (), + ) + .is_none() + { + let c1 = colliders.get(ch1).unwrap(); + let c2 = colliders.get(*ch2).unwrap(); + let bh1 = c1.parent(); + let bh2 = c2.parent(); + + if bh1 == bh2 { + // Ignore self-intersection. + return true; + } + + 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, + *ch2, + c1, + c2, + b1, + b2, + None, + None, + 0.0, + // 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); + } else { + min_overstep = dt; + all_toi.push(toi); + } + } + } + + true + }); + } + } + + /* + * + * If the smallest TOI is outside of the time interval, return. + * + */ + if min_overstep == dt && all_toi.is_empty() { + return PredictedImpacts::NoImpacts; + } else if min_overstep > dt { + return PredictedImpacts::ImpactsAfterEndTime(min_overstep); + } + + // NOTE: all static bodies (and kinematic bodies?) should be considered as "frozen", this + // may avoid some resweeps. + let mut intersections_to_check = vec![]; + + while let Some(toi) = all_toi.pop() { + assert!(toi.toi <= dt); + + let body1 = bodies.get(toi.b1).unwrap(); + let body2 = bodies.get(toi.b2).unwrap(); + + let mut colliders_to_check = Vec::new(); + let should_freeze1 = body1.is_ccd_active() && !frozen.contains_key(&toi.b1); + let should_freeze2 = body2.is_ccd_active() && !frozen.contains_key(&toi.b2); + + if !should_freeze1 && !should_freeze2 { + continue; + } + + if toi.is_intersection_test { + // NOTE: this test is rendundant 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. + if should_freeze1 || should_freeze2 { + // 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); + } + continue; + } + + if should_freeze1 { + let _ = frozen.insert(toi.b1, toi.toi); + colliders_to_check.extend_from_slice(&body1.colliders); + } + + if should_freeze2 { + let _ = frozen.insert(toi.b2, toi.toi); + colliders_to_check.extend_from_slice(&body2.colliders); + } + + let start_time = toi.toi; + + for ch1 in &colliders_to_check { + let co1 = &colliders[*ch1]; + let rb1 = &bodies[co1.parent]; + let aabb = co1.compute_swept_aabb(&(rb1.next_position * co1.position_wrt_parent())); + + self.query_pipeline + .colliders_with_aabb_intersecting_aabb(&aabb, |ch2| { + let c1 = colliders.get(*ch1).unwrap(); + let c2 = colliders.get(*ch2).unwrap(); + let bh1 = c1.parent(); + let bh2 = c2.parent(); + + if bh1 == bh2 { + // Ignore self-intersection. + return true; + } + + let frozen1 = frozen.get(&bh1); + let frozen2 = frozen.get(&bh2); + + let b1 = bodies.get(bh1).unwrap(); + let b2 = bodies.get(bh2).unwrap(); + + if (frozen1.is_some() || !b1.is_ccd_active()) + && (frozen2.is_some() || !b2.is_ccd_active()) + { + // We already did a resweep. + 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, + *ch2, + c1, + c2, + b1, + b2, + frozen1.copied(), + frozen2.copied(), + start_time, + dt, + smallest_dist, + ) { + all_toi.push(toi); + } + + true + }); + } + } + + for toi in 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 + // reported by the narrow-phase at the next timestep/substep. + // - 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 body1 = &bodies[toi.b1]; + let body2 = &bodies[toi.b2]; + let co1 = &colliders[toi.c1]; + let co2 = &colliders[toi.c2]; + let frozen1 = frozen.get(&toi.b1); + let frozen2 = frozen.get(&toi.b2); + let pos1 = frozen1 + .map(|t| body1.integrate_velocity(*t)) + .unwrap_or(body1.next_position); + let pos2 = frozen2 + .map(|t| body2.integrate_velocity(*t)) + .unwrap_or(body2.next_position); + + let prev_coll_pos12 = co1.position.inv_mul(&co2.position); + let next_coll_pos12 = + (pos1 * co1.position_wrt_parent()).inverse() * (pos2 * co2.position_wrt_parent()); + + let query_dispatcher = self.query_pipeline.query_dispatcher(); + let intersect_before = query_dispatcher + .intersection_test(&prev_coll_pos12, co1.shape(), co2.shape()) + .unwrap_or(false); + + let intersect_after = query_dispatcher + .intersection_test(&next_coll_pos12, co1.shape(), co2.shape()) + .unwrap_or(false); + + if !intersect_before && !intersect_after { + // 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)); + } + } + + PredictedImpacts::Impacts(frozen) + } +} diff --git a/src/dynamics/ccd/mod.rs b/src/dynamics/ccd/mod.rs new file mode 100644 index 0000000..84807fa --- /dev/null +++ b/src/dynamics/ccd/mod.rs @@ -0,0 +1,5 @@ +pub use self::ccd_solver::{CCDSolver, PredictedImpacts}; +pub use self::toi_entry::TOIEntry; + +mod ccd_solver; +mod toi_entry; diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs new file mode 100644 index 0000000..cc6773c --- /dev/null +++ b/src/dynamics/ccd/toi_entry.rs @@ -0,0 +1,163 @@ +use crate::dynamics::{RigidBody, RigidBodyHandle}; +use crate::geometry::{Collider, ColliderHandle}; +use crate::math::Real; +use parry::query::{NonlinearRigidMotion, QueryDispatcher}; + +#[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<QD: ?Sized + QueryDispatcher>( + query_dispatcher: &QD, + ch1: ColliderHandle, + ch2: ColliderHandle, + c1: &Collider, + c2: &Collider, + b1: &RigidBody, + b2: &RigidBody, + frozen1: Option<Real>, + 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 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); + + if let Some(t) = frozen1 { + motion1.freeze(t); + } + + if let Some(t) = frozen2 { + motion2.freeze(t); + } + + 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, + |