577 lines
23 KiB
Rust
577 lines
23 KiB
Rust
use super::TOIEntry;
|
||
use crate::dynamics::{IslandManager, RigidBodyHandle, RigidBodySet};
|
||
use crate::geometry::{ColliderParent, ColliderSet, CollisionEvent, NarrowPhase};
|
||
use crate::math::Real;
|
||
use crate::parry::utils::SortedPair;
|
||
use crate::pipeline::{EventHandler, QueryFilter, QueryPipeline};
|
||
use crate::prelude::{ActiveEvents, CollisionEventFlags};
|
||
use parry::partitioning::{Bvh, BvhBuildStrategy};
|
||
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.
|
||
#[derive(Clone)]
|
||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||
pub struct CCDSolver {
|
||
// TODO PERF: for now the CCD solver maintains its own bvh for CCD queries.
|
||
// At each frame it get rebuilt.
|
||
// We should consider an alternative to directly use the broad-phase’s.
|
||
#[cfg_attr(feature = "serde-serialize", serde(skip))]
|
||
bvh: Bvh,
|
||
}
|
||
|
||
impl Default for CCDSolver {
|
||
fn default() -> Self {
|
||
Self::new()
|
||
}
|
||
}
|
||
|
||
impl CCDSolver {
|
||
/// Initializes a new CCD solver
|
||
pub fn new() -> Self {
|
||
Self { bvh: Bvh::new() }
|
||
}
|
||
|
||
/// 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) {
|
||
if let PredictedImpacts::Impacts(tois) = impacts {
|
||
for (handle, toi) in tois {
|
||
let rb = bodies.index_mut_internal(*handle);
|
||
let local_com = &rb.mprops.local_mprops.local_com;
|
||
|
||
let min_toi = (rb.ccd.ccd_thickness
|
||
* 0.15
|
||
* crate::utils::inv(rb.ccd.max_point_velocity(&rb.integrated_vels)))
|
||
.min(dt);
|
||
// println!(
|
||
// "Min toi: {}, Toi: {}, thick: {}, max_vel: {}",
|
||
// min_toi,
|
||
// toi,
|
||
// rb.ccd.ccd_thickness,
|
||
// rb.ccd.max_point_velocity(&rb.integrated_vels)
|
||
// );
|
||
let new_pos =
|
||
rb.integrated_vels
|
||
.integrate(toi.max(min_toi), &rb.pos.position, local_com);
|
||
rb.pos.next_position = new_pos;
|
||
}
|
||
}
|
||
}
|
||
|
||
/// 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,
|
||
islands: &IslandManager,
|
||
bodies: &mut RigidBodySet,
|
||
dt: Real,
|
||
include_forces: bool,
|
||
) -> bool {
|
||
let mut ccd_active = false;
|
||
|
||
// println!("Checking CCD activation");
|
||
for handle in islands.active_dynamic_bodies() {
|
||
let rb = bodies.index_mut_internal(*handle);
|
||
|
||
if rb.ccd.ccd_enabled {
|
||
let forces = if include_forces {
|
||
Some(&rb.forces)
|
||
} else {
|
||
None
|
||
};
|
||
let moving_fast = rb.ccd.is_moving_fast(dt, &rb.integrated_vels, forces);
|
||
rb.ccd.ccd_active = moving_fast;
|
||
ccd_active = ccd_active || moving_fast;
|
||
}
|
||
}
|
||
|
||
ccd_active
|
||
}
|
||
|
||
/// Find the first time a CCD-enabled body has a non-sensor collider hitting another non-sensor collider.
|
||
#[profiling::function]
|
||
pub fn find_first_impact(
|
||
&mut self,
|
||
dt: Real,
|
||
islands: &IslandManager,
|
||
bodies: &RigidBodySet,
|
||
colliders: &ColliderSet,
|
||
narrow_phase: &NarrowPhase,
|
||
) -> Option<Real> {
|
||
// Update the query pipeline with the colliders’ predicted positions.
|
||
self.bvh = Bvh::from_iter(
|
||
BvhBuildStrategy::Binned,
|
||
colliders.iter_enabled().map(|(co_handle, co)| {
|
||
let id = co_handle.into_raw_parts().0;
|
||
if let Some(co_parent) = co.parent {
|
||
let rb = &bodies[co_parent.handle];
|
||
let predicted_pos = rb
|
||
.pos
|
||
.integrate_forces_and_velocities(dt, &rb.forces, &rb.vels, &rb.mprops);
|
||
|
||
let next_position = predicted_pos * co_parent.pos_wrt_parent;
|
||
(
|
||
id as usize,
|
||
co.shape.compute_swept_aabb(&co.pos, &next_position),
|
||
)
|
||
} else {
|
||
(id as usize, co.shape.compute_aabb(&co.pos))
|
||
}
|
||
}),
|
||
);
|
||
|
||
let query_pipeline = QueryPipeline {
|
||
// NOTE: the upcast needs at least rust 1.86
|
||
dispatcher: narrow_phase.query_dispatcher(),
|
||
bvh: &self.bvh,
|
||
bodies,
|
||
colliders,
|
||
filter: QueryFilter::default(),
|
||
};
|
||
|
||
let mut pairs_seen = HashMap::default();
|
||
let mut min_toi = dt;
|
||
|
||
for handle in islands.active_dynamic_bodies() {
|
||
let rb1 = &bodies[*handle];
|
||
|
||
if rb1.ccd.ccd_active {
|
||
let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities(
|
||
dt,
|
||
&rb1.forces,
|
||
&rb1.integrated_vels,
|
||
&rb1.mprops,
|
||
);
|
||
|
||
for ch1 in &rb1.colliders.0 {
|
||
let co1 = &colliders[*ch1];
|
||
let co1_parent = co1
|
||
.parent
|
||
.as_ref()
|
||
.expect("Could not find the ColliderParent component.");
|
||
|
||
if co1.is_sensor() {
|
||
continue; // Ignore sensors.
|
||
}
|
||
|
||
let predicted_collider_pos1 = predicted_body_pos1 * co1_parent.pos_wrt_parent;
|
||
let aabb1 = co1
|
||
.shape
|
||
.compute_swept_aabb(&co1.pos, &predicted_collider_pos1);
|
||
|
||
for (ch2, _) in query_pipeline.intersect_aabb_conservative(aabb1) {
|
||
if *ch1 == ch2 {
|
||
// Ignore self-intersection.
|
||
continue;
|
||
}
|
||
|
||
if pairs_seen
|
||
.insert(
|
||
SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0),
|
||
(),
|
||
)
|
||
.is_none()
|
||
{
|
||
let co1 = &colliders[*ch1];
|
||
let co2 = &colliders[ch2];
|
||
|
||
let bh1 = co1.parent.map(|p| p.handle);
|
||
let bh2 = co2.parent.map(|p| p.handle);
|
||
|
||
// Ignore self-intersection and sensors and apply collision groups filter.
|
||
if bh1 == bh2 // Ignore self-intersection.
|
||
|| (co1.is_sensor() || co2.is_sensor()) // Ignore sensors.
|
||
|| !co1.flags.collision_groups.test(co2.flags.collision_groups) // Apply collision groups.
|
||
|| !co1.flags.solver_groups.test(co2.flags.solver_groups)
|
||
// Apply solver groups.
|
||
{
|
||
continue;
|
||
}
|
||
|
||
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 rb2 = bh2.and_then(|h| bodies.get(h));
|
||
|
||
if let Some(toi) = TOIEntry::try_from_colliders(
|
||
narrow_phase.query_dispatcher(),
|
||
*ch1,
|
||
ch2,
|
||
co1,
|
||
co2,
|
||
Some(rb1),
|
||
rb2,
|
||
None,
|
||
None,
|
||
0.0,
|
||
min_toi,
|
||
smallest_dist,
|
||
) {
|
||
min_toi = min_toi.min(toi.toi);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
if min_toi < dt { Some(min_toi) } else { None }
|
||
}
|
||
|
||
/// Outputs the set of bodies as well as their first time-of-impact event.
|
||
#[profiling::function]
|
||
pub fn predict_impacts_at_next_positions(
|
||
&mut self,
|
||
dt: Real,
|
||
islands: &IslandManager,
|
||
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 with the colliders’ `next_position`.
|
||
self.bvh = Bvh::from_iter(
|
||
BvhBuildStrategy::Binned,
|
||
colliders.iter_enabled().map(|(co_handle, co)| {
|
||
let id = co_handle.into_raw_parts().0;
|
||
if let Some(co_parent) = co.parent {
|
||
let rb_next_pos = &bodies[co_parent.handle].pos.next_position;
|
||
let next_position = rb_next_pos * co_parent.pos_wrt_parent;
|
||
(
|
||
id as usize,
|
||
co.shape.compute_swept_aabb(&co.pos, &next_position),
|
||
)
|
||
} else {
|
||
(id as usize, co.shape.compute_aabb(&co.pos))
|
||
}
|
||
}),
|
||
);
|
||
|
||
let query_pipeline = QueryPipeline {
|
||
dispatcher: narrow_phase.query_dispatcher(),
|
||
bvh: &self.bvh,
|
||
bodies,
|
||
colliders,
|
||
filter: QueryFilter::default(),
|
||
};
|
||
|
||
/*
|
||
*
|
||
* First, collect all TOIs.
|
||
*
|
||
*/
|
||
// TODO: don't iterate through all the colliders.
|
||
for handle in islands.active_dynamic_bodies() {
|
||
let rb1 = &bodies[*handle];
|
||
|
||
if rb1.ccd.ccd_active {
|
||
let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities(
|
||
dt,
|
||
&rb1.forces,
|
||
&rb1.integrated_vels,
|
||
&rb1.mprops,
|
||
);
|
||
|
||
for ch1 in &rb1.colliders.0 {
|
||
let co1 = &colliders[*ch1];
|
||
let co_parent1 = co1
|
||
.parent
|
||
.as_ref()
|
||
.expect("Could not find the ColliderParent component.");
|
||
|
||
let predicted_collider_pos1 = predicted_body_pos1 * co_parent1.pos_wrt_parent;
|
||
let aabb1 = co1
|
||
.shape
|
||
.compute_swept_aabb(&co1.pos, &predicted_collider_pos1);
|
||
|
||
for (ch2, _) in query_pipeline.intersect_aabb_conservative(aabb1) {
|
||
if *ch1 == ch2 {
|
||
// Ignore self-intersection.
|
||
continue;
|
||
}
|
||
|
||
if pairs_seen
|
||
.insert(
|
||
SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0),
|
||
(),
|
||
)
|
||
.is_none()
|
||
{
|
||
let co1 = &colliders[*ch1];
|
||
let co2 = &colliders[ch2];
|
||
|
||
let bh1 = co1.parent.map(|p| p.handle);
|
||
let bh2 = co2.parent.map(|p| p.handle);
|
||
|
||
// Ignore self-intersections and apply groups filter.
|
||
if bh1 == bh2
|
||
|| !co1.flags.collision_groups.test(co2.flags.collision_groups)
|
||
{
|
||
continue;
|
||
}
|
||
|
||
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 rb1 = bh1.map(|h| &bodies[h]);
|
||
let rb2 = bh2.map(|h| &bodies[h]);
|
||
|
||
if let Some(toi) = TOIEntry::try_from_colliders(
|
||
query_pipeline.dispatcher,
|
||
*ch1,
|
||
ch2,
|
||
co1,
|
||
co2,
|
||
rb1,
|
||
rb2,
|
||
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);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/*
|
||
*
|
||
* 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 fixed bodies (and kinematic bodies?) should be considered as "frozen", this
|
||
// may avoid some resweeps.
|
||
let mut pseudo_intersections_to_check = vec![];
|
||
|
||
while let Some(toi) = all_toi.pop() {
|
||
assert!(toi.toi <= dt);
|
||
|
||
let rb1 = toi.b1.and_then(|b| bodies.get(b));
|
||
let rb2 = toi.b2.and_then(|b| bodies.get(b));
|
||
|
||
let mut colliders_to_check = Vec::new();
|
||
let should_freeze1 = rb1.is_some()
|
||
&& rb1.unwrap().ccd.ccd_active
|
||
&& !frozen.contains_key(&toi.b1.unwrap());
|
||
let should_freeze2 = rb2.is_some()
|
||
&& rb2.unwrap().ccd.ccd_active
|
||
&& !frozen.contains_key(&toi.b2.unwrap());
|
||
|
||
if !should_freeze1 && !should_freeze2 {
|
||
continue;
|
||
}
|
||
|
||
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 reason 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.
|
||
pseudo_intersections_to_check.push(toi);
|
||
}
|
||
continue;
|
||
}
|
||
|
||
if should_freeze1 {
|
||
let _ = frozen.insert(toi.b1.unwrap(), toi.toi);
|
||
colliders_to_check.extend_from_slice(&rb1.unwrap().colliders.0);
|
||
}
|
||
|
||
if should_freeze2 {
|
||
let _ = frozen.insert(toi.b2.unwrap(), toi.toi);
|
||
colliders_to_check.extend_from_slice(&rb2.unwrap().colliders.0);
|
||
}
|
||
|
||
let start_time = toi.toi;
|
||
|
||
// NOTE: the 1 and 2 indices (e.g., `ch1`, `ch2`) below are unrelated to the
|
||
// ones we used above.
|
||
for ch1 in &colliders_to_check {
|
||
let co1 = &colliders[*ch1];
|
||
let co1_parent = co1.parent.as_ref().unwrap();
|
||
let rb1 = &bodies[co1_parent.handle];
|
||
|
||
let co_next_pos1 = rb1.pos.next_position * co1_parent.pos_wrt_parent;
|
||
let aabb = co1.shape.compute_swept_aabb(&co1.pos, &co_next_pos1);
|
||
|
||
for (ch2, _) in query_pipeline.intersect_aabb_conservative(aabb) {
|
||
let co2 = &colliders[ch2];
|
||
|
||
let bh1 = co1.parent.map(|p| p.handle);
|
||
let bh2 = co2.parent.map(|p| p.handle);
|
||
|
||
// Ignore self-intersection and apply groups filter.
|
||
if bh1 == bh2 || !co1.flags.collision_groups.test(co2.flags.collision_groups) {
|
||
continue;
|
||
}
|
||
|
||
let frozen1 = bh1.and_then(|h| frozen.get(&h));
|
||
let frozen2 = bh2.and_then(|h| frozen.get(&h));
|
||
|
||
let rb1 = bh1.and_then(|h| bodies.get(h));
|
||
let rb2 = bh2.and_then(|h| bodies.get(h));
|
||
|
||
if (frozen1.is_some() || !rb1.map(|b| b.ccd.ccd_active).unwrap_or(false))
|
||
&& (frozen2.is_some() || !rb2.map(|b| b.ccd.ccd_active).unwrap_or(false))
|
||
{
|
||
// We already did a resweep.
|
||
continue;
|
||
}
|
||
|
||
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(
|
||
query_pipeline.dispatcher,
|
||
*ch1,
|
||
ch2,
|
||
co1,
|
||
co2,
|
||
rb1,
|
||
rb2,
|
||
frozen1.copied(),
|
||
frozen2.copied(),
|
||
start_time,
|
||
dt,
|
||
smallest_dist,
|
||
) {
|
||
all_toi.push(toi);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
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
|
||
// 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 co1 = &colliders[toi.c1];
|
||
let co2 = &colliders[toi.c2];
|
||
|
||
if !co1.is_sensor() && !co2.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 = co1.parent.as_ref().unwrap();
|
||
let rb1 = &bodies[b1];
|
||
let local_com1 = &rb1.mprops.local_mprops.local_com;
|
||
let frozen1 = frozen.get(&b1);
|
||
let pos1 = frozen1
|
||
.map(|t| {
|
||
rb1.integrated_vels
|
||
.integrate(*t, &rb1.pos.position, local_com1)
|
||
})
|
||
.unwrap_or(rb1.pos.next_position);
|
||
pos1 * co_parent1.pos_wrt_parent
|
||
} else {
|
||
co1.pos.0
|
||
};
|
||
|
||
let co_next_pos2 = if let Some(b2) = toi.b2 {
|
||
let co_parent2: &ColliderParent = co2.parent.as_ref().unwrap();
|
||
let rb2 = &bodies[b2];
|
||
let local_com2 = &rb2.mprops.local_mprops.local_com;
|
||
let frozen2 = frozen.get(&b2);
|
||
let pos2 = frozen2
|
||
.map(|t| {
|
||
rb2.integrated_vels
|
||
.integrate(*t, &rb2.pos.position, local_com2)
|
||
})
|
||
.unwrap_or(rb2.pos.next_position);
|
||
pos2 * co_parent2.pos_wrt_parent
|
||
} else {
|
||
co2.pos.0
|
||
};
|
||
|
||
let prev_coll_pos12 = co1.pos.inv_mul(&co2.pos);
|
||
let next_coll_pos12 = co_next_pos1.inv_mul(&co_next_pos2);
|
||
|
||
let intersect_before = query_pipeline
|
||
.dispatcher
|
||
.intersection_test(&prev_coll_pos12, co1.shape.as_ref(), co2.shape.as_ref())
|
||
.unwrap_or(false);
|
||
|
||
let intersect_after = query_pipeline
|
||
.dispatcher
|
||
.intersection_test(&next_coll_pos12, co1.shape.as_ref(), co2.shape.as_ref())
|
||
.unwrap_or(false);
|
||
|
||
if !intersect_before
|
||
&& !intersect_after
|
||
&& (co1.flags.active_events | co2.flags.active_events)
|
||
.contains(ActiveEvents::COLLISION_EVENTS)
|
||
{
|
||
// Emit one intersection-started and one intersection-stopped event.
|
||
events.handle_collision_event(
|
||
bodies,
|
||
colliders,
|
||
CollisionEvent::Started(toi.c1, toi.c2, CollisionEventFlags::SENSOR),
|
||
None,
|
||
);
|
||
events.handle_collision_event(
|
||
bodies,
|
||
colliders,
|
||
CollisionEvent::Stopped(toi.c1, toi.c2, CollisionEventFlags::SENSOR),
|
||
None,
|
||
);
|
||
}
|
||
}
|
||
|
||
PredictedImpacts::Impacts(frozen)
|
||
}
|
||
}
|