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,
}
#[derive(Clone)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct CCDSolver {
#[cfg_attr(feature = "serde-serialize", serde(skip))]
query_pipeline: QueryPipeline,
}
impl CCDSolver {
pub fn new() -> Self {
Self::with_query_dispatcher(DefaultQueryDispatcher)
}
pub fn with_query_dispatcher<D>(d: D) -> Self
where
D: 'static + QueryDispatcher,
{
CCDSolver {
query_pipeline: QueryPipeline::with_query_dispatcher(d),
}
}
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.max_point_velocity()))
.min(dt);
body.integrate_next_position(toi.max(min_toi));
}
}
}
_ => {}
}
}
pub fn update_ccd_active_flags(
&self,
bodies: &mut RigidBodySet,
dt: Real,
include_forces: bool,
) -> bool {
let mut ccd_active = false;
bodies.foreach_active_dynamic_body_mut_internal(|_, body| {
body.update_ccd_active_flag(dt, include_forces);
ccd_active = ccd_active || body.is_ccd_active();
});
ccd_active
}
pub fn find_first_impact(
&mut self,
dt: Real,
bodies: &RigidBodySet,
colliders: &ColliderSet,
narrow_phase: &NarrowPhase,
) -> Option<Real> {
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;
}
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 {
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()) {
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
}
}
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;
self.query_pipeline.update_with_mode(
bodies,
colliders,
QueryPipelineMode::SweepTestWithNextPosition,
);
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 {
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 {
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,
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 min_overstep == dt && all_toi.is_empty() {
return PredictedImpacts::NoImpacts;
} else if min_overstep > dt {
return PredictedImpacts::ImpactsAfterEndTime(min_overstep);
}
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 {
if should_freeze1 || should_freeze2 {
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 {
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())
{
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 {
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 {
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)
}
}