nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Split rigid-bodies and colliders into multiple components

Browse Source
This commit is contained in:
Crozet Sébastien
2021-04-26 17:59:25 +02:00
parent aaf80bfa87
commit c32da78f2a
91 changed files with 5969 additions and 3653 deletions
Download Patch File Download Diff File Expand all files Collapse all files

430
src/dynamics/ccd/ccd_solver.rs
View File

@@ -1,6 +1,12 @@
use super::TOIEntry;
use crate::dynamics::{RigidBodyHandle, RigidBodySet};
use crate::geometry::{ColliderSet, IntersectionEvent, NarrowPhase};
use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
use crate::dynamics::{IslandManager, RigidBodyColliders, RigidBodyForces};
use crate::dynamics::{
RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::geometry::{
ColliderParent, ColliderPosition, ColliderShape, ColliderType, IntersectionEvent, NarrowPhase,
};
use crate::math::Real;
use crate::parry::utils::SortedPair;
use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode};
@@ -44,19 +50,34 @@ impl CCDSolver {
/// 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) {
pub fn clamp_motions<Bodies>(&self, dt: Real, bodies: &mut Bodies, impacts: &PredictedImpacts)
where
Bodies: ComponentSet<RigidBodyCcd>
+ ComponentSetMut<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
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));
}
let (rb_poss, vels, ccd, mprops): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyCcd,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let local_com = &mprops.mass_properties.local_com;
let min_toi = (ccd.ccd_thickness
* 0.15
* crate::utils::inv(ccd.max_point_velocity(vels)))
.min(dt);
// println!("Min toi: {}, Toi: {}", min_toi, toi);
let new_pos = vels.integrate(toi.max(min_toi), &rb_poss.position, &local_com);
bodies.map_mut_internal(handle.0, |rb_poss| {
rb_poss.next_position = new_pos;
});
}
}
_ => {}
@@ -66,34 +87,64 @@ 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(
pub fn update_ccd_active_flags<Bodies>(
&self,
bodies: &mut RigidBodySet,
islands: &IslandManager,
bodies: &mut Bodies,
dt: Real,
include_forces: bool,
) -> bool {
) -> bool
where
Bodies: ComponentSetMut<RigidBodyCcd>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyForces>,
{
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();
});
for handle in islands.active_dynamic_bodies() {
let (ccd, vels, forces): (&RigidBodyCcd, &RigidBodyVelocity, &RigidBodyForces) =
bodies.index_bundle(handle.0);
if ccd.ccd_enabled {
let forces = if include_forces { Some(forces) } else { None };
let moving_fast = ccd.is_moving_fast(dt, vels, forces);
bodies.map_mut_internal(handle.0, |ccd| {
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.
pub fn find_first_impact(
pub fn find_first_impact<Bodies, Colliders>(
&mut self,
dt: Real,
bodies: &RigidBodySet,
colliders: &ColliderSet,
islands: &IslandManager,
bodies: &Bodies,
colliders: &Colliders,
narrow_phase: &NarrowPhase,
) -> Option<Real> {
) -> Option<Real>
where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyCcd>
+ ComponentSet<RigidBodyColliders>
+ ComponentSet<RigidBodyForces>
+ ComponentSet<RigidBodyMassProps>,
Colliders: ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>
+ ComponentSet<ColliderType>,
{
// Update the query pipeline.
self.query_pipeline.update_with_mode(
islands,
bodies,
colliders,
QueryPipelineMode::SweepTestWithPredictedPosition { dt },
@@ -102,19 +153,37 @@ impl CCDSolver {
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 handle in islands.active_dynamic_bodies() {
let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0);
for ch1 in &rb1.colliders {
let co1 = &colliders[*ch1];
if rb_ccd1.ccd_active {
let (rb_pos1, rb_vels1, forces1, rb_mprops1, rb_colliders1): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyForces,
&RigidBodyMassProps,
&RigidBodyColliders,
) = bodies.index_bundle(handle.0);
if co1.is_sensor() {
let predicted_body_pos1 =
rb_pos1.integrate_force_and_velocity(dt, forces1, rb_vels1, rb_mprops1);
for ch1 in &rb_colliders1.0 {
let co_parent1: &ColliderParent = colliders
.get(ch1.0)
.expect("Could not find the ColliderParent component.");
let (co_shape1, co_pos1, co_type1): (
&ColliderShape,
&ColliderPosition,
&ColliderType,
) = colliders.index_bundle(ch1.0);
if co_type1.is_sensor() {
continue; // Ignore sensors.
}
let aabb1 =
co1.compute_swept_aabb(&(predicted_body_pos1 * co1.position_wrt_parent()));
let predicted_collider_pos1 = predicted_body_pos1 * co_parent1.pos_wrt_parent;
let aabb1 = co_shape1.compute_swept_aabb(&co_pos1, &predicted_collider_pos1);
self.query_pipeline
.colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| {
@@ -130,12 +199,17 @@ impl CCDSolver {
)
.is_none()
{
let c1 = colliders.get(*ch1).unwrap();
let c2 = colliders.get(*ch2).unwrap();
let bh1 = c1.parent();
let bh2 = c2.parent();
let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0);
let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0);
let c1: (_, _, _) = colliders.index_bundle(ch1.0);
let c2: (_, _, _) = colliders.index_bundle(ch2.0);
let co_type1: &ColliderType = colliders.index(ch1.0);
let co_type2: &ColliderType = colliders.index(ch1.0);
if bh1 == bh2 || (c1.is_sensor() || c2.is_sensor()) {
let bh1 = co_parent1.map(|p| p.handle);
let bh2 = co_parent2.map(|p| p.handle);
if bh1 == bh2 || (co_type1.is_sensor() || co_type2.is_sensor()) {
// Ignore self-intersection and sensors.
return true;
}
@@ -146,16 +220,15 @@ impl CCDSolver {
.map(|c| c.1.dist)
.unwrap_or(0.0);
let b1 = bodies.get(bh1).unwrap();
let b2 = bodies.get(bh2).unwrap();
let b2 = bh2.map(|h| bodies.index_bundle(h.0));
if let Some(toi) = TOIEntry::try_from_colliders(
self.query_pipeline.query_dispatcher(),
*ch1,
*ch2,
c1,
c2,
b1,
(c1.0, c1.1, c1.2, co_parent1),
(c2.0, c2.1, c2.2, co_parent2),
Some((rb_pos1, rb_vels1, rb_mprops1, rb_ccd1)),
b2,
None,
None,
@@ -181,14 +254,27 @@ impl CCDSolver {
}
/// Outputs the set of bodies as well as their first time-of-impact event.
pub fn predict_impacts_at_next_positions(
pub fn predict_impacts_at_next_positions<Bodies, Colliders>(
&mut self,
dt: Real,
bodies: &RigidBodySet,
colliders: &ColliderSet,
islands: &IslandManager,
bodies: &Bodies,
colliders: &Colliders,
narrow_phase: &NarrowPhase,
events: &dyn EventHandler,
) -> PredictedImpacts {
) -> PredictedImpacts
where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyCcd>
+ ComponentSet<RigidBodyColliders>
+ ComponentSet<RigidBodyForces>
+ ComponentSet<RigidBodyMassProps>,
Colliders: ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>
+ ComponentSet<ColliderType>,
{
let mut frozen = HashMap::<_, Real>::default();
let mut all_toi = BinaryHeap::new();
let mut pairs_seen = HashMap::default();
@@ -196,6 +282,7 @@ impl CCDSolver {
// Update the query pipeline.
self.query_pipeline.update_with_mode(
islands,
bodies,
colliders,
QueryPipelineMode::SweepTestWithNextPosition,
@@ -207,71 +294,94 @@ impl CCDSolver {
*
*/
// 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()));
for handle in islands.active_dynamic_bodies() {
let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0);
self.query_pipeline
.colliders_with_aabb_intersecting_aabb(&aabb, |ch2| {
if ch1 == *ch2 {
// Ignore self-intersection.
return true;
}
if rb_ccd1.ccd_active {
let (rb_pos1, rb_vels1, forces1, rb_mprops1, rb_colliders1): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyForces,
&RigidBodyMassProps,
&RigidBodyColliders,
) = bodies.index_bundle(handle.0);
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();
let predicted_body_pos1 =
rb_pos1.integrate_force_and_velocity(dt, forces1, rb_vels1, rb_mprops1);
if bh1 == bh2 {
for ch1 in &rb_colliders1.0 {
let co_parent1: &ColliderParent = colliders
.get(ch1.0)
.expect("Could not find the ColliderParent component.");
let (co_shape1, co_pos1): (&ColliderShape, &ColliderPosition) =
colliders.index_bundle(ch1.0);
let predicted_collider_pos1 = predicted_body_pos1 * co_parent1.pos_wrt_parent;
let aabb1 = co_shape1.compute_swept_aabb(&co_pos1, &predicted_collider_pos1);
self.query_pipeline
.colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| {
if *ch1 == *ch2 {
// Ignore self-intersection.
return true;
}
let b1 = bodies.get(bh1).unwrap();
let b2 = bodies.get(bh2).unwrap();
if pairs_seen
.insert(
SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0),
(),
)
.is_none()
{
let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0);
let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0);
let c1: (_, _, _) = colliders.index_bundle(ch1.0);
let c2: (_, _, _) = colliders.index_bundle(ch2.0);
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 bh1 = co_parent1.map(|p| p.handle);
let bh2 = co_parent2.map(|p| p.handle);
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);
if bh1 == bh2 {
// Ignore self-intersection.
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 b2 = bh2.map(|h| bodies.index_bundle(h.0));
if let Some(toi) = TOIEntry::try_from_colliders(
self.query_pipeline.query_dispatcher(),
*ch1,
*ch2,
(c1.0, c1.1, c1.2, co_parent1),
(c2.0, c2.1, c2.2, co_parent2),
Some((rb_pos1, rb_vels1, rb_mprops1, rb_ccd1)),
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
});
true
});
}
}
}
@@ -293,19 +403,25 @@ impl CCDSolver {
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 rb1: Option<(&RigidBodyCcd, &RigidBodyColliders)> =
toi.b1.map(|b| bodies.index_bundle(b.0));
let rb2: Option<(&RigidBodyCcd, &RigidBodyColliders)> =
toi.b2.map(|b| bodies.index_bundle(b.0));
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);
let should_freeze1 = rb1.is_some()
&& rb1.unwrap().0.ccd_active
&& !frozen.contains_key(&toi.b1.unwrap());
let should_freeze2 = rb2.is_some()
&& rb2.unwrap().0.ccd_active
&& !frozen.contains_key(&toi.b2.unwrap());
if !should_freeze1 && !should_freeze2 {
continue;
}
if toi.is_intersection_test {
// NOTE: this test is rendundant with the previous `if !should_freeze && ...`
// 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 reasons in the future.
if should_freeze1 || should_freeze2 {
@@ -318,42 +434,51 @@ impl CCDSolver {
}
if should_freeze1 {
let _ = frozen.insert(toi.b1, toi.toi);
colliders_to_check.extend_from_slice(&body1.colliders);
let _ = frozen.insert(toi.b1.unwrap(), toi.toi);
colliders_to_check.extend_from_slice(&rb1.unwrap().1 .0);
}
if should_freeze2 {
let _ = frozen.insert(toi.b2, toi.toi);
colliders_to_check.extend_from_slice(&body2.colliders);
let _ = frozen.insert(toi.b2.unwrap(), toi.toi);
colliders_to_check.extend_from_slice(&rb2.unwrap().1 .0);
}
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()));
let co_parent1: &ColliderParent = colliders.get(ch1.0).unwrap();
let (co_shape1, co_pos1): (&ColliderShape, &ColliderPosition) =
colliders.index_bundle(ch1.0);
let rb_pos1: &RigidBodyPosition = bodies.index(co_parent1.handle.0);
let co_next_pos1 = rb_pos1.next_position * co_parent1.pos_wrt_parent;
let aabb = co_shape1.compute_swept_aabb(&co_pos1, &co_next_pos1);
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();
let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0);
let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0);
let c1: (_, _, _) = colliders.index_bundle(ch1.0);
let c2: (_, _, _) = colliders.index_bundle(ch2.0);
let bh1 = co_parent1.map(|p| p.handle);
let bh2 = co_parent2.map(|p| p.handle);
if bh1 == bh2 {
// Ignore self-intersection.
return true;
}
let frozen1 = frozen.get(&bh1);
let frozen2 = frozen.get(&bh2);
let frozen1 = bh1.and_then(|h| frozen.get(&h));
let frozen2 = bh2.and_then(|h| frozen.get(&h));
let b1 = bodies.get(bh1).unwrap();
let b2 = bodies.get(bh2).unwrap();
let b1: Option<(_, _, _, &RigidBodyCcd)> =
bh1.map(|h| bodies.index_bundle(h.0));
let b2: Option<(_, _, _, &RigidBodyCcd)> =
bh1.map(|h| bodies.index_bundle(h.0));
if (frozen1.is_some() || !b1.is_ccd_active())
&& (frozen2.is_some() || !b2.is_ccd_active())
if (frozen1.is_some() || !b1.map(|b| b.3.ccd_active).unwrap_or(false))
&& (frozen2.is_some() || !b2.map(|b| b.3.ccd_active).unwrap_or(false))
{
// We already did a resweep.
return true;
@@ -369,8 +494,8 @@ impl CCDSolver {
self.query_pipeline.query_dispatcher(),
*ch1,
*ch2,
c1,
c2,
(c1.0, c1.1, c1.2, co_parent1),
(c2.0, c2.1, c2.2, co_parent2),
b1,
b2,
frozen1.copied(),
@@ -395,30 +520,57 @@ impl CCDSolver {
// - 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 (co_pos1, co_shape1): (&ColliderPosition, &ColliderShape) =
colliders.index_bundle(toi.c1.0);
let (co_pos2, co_shape2): (&ColliderPosition, &ColliderShape) =
colliders.index_bundle(toi.c2.0);
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 co_next_pos1 = if let Some(b1) = toi.b1 {
let co_parent1: &ColliderParent = colliders.get(toi.c1.0).unwrap();
let (rb_pos1, rb_vels1, rb_mprops1): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
) = bodies.index_bundle(b1.0);
let local_com1 = &rb_mprops1.mass_properties.local_com;
let frozen1 = frozen.get(&b1);
let pos1 = frozen1
.map(|t| rb_vels1.integrate(*t, &rb_pos1.position, local_com1))
.unwrap_or(rb_pos1.next_position);
pos1 * co_parent1.pos_wrt_parent
} else {
co_pos1.0
};
let co_next_pos2 = if let Some(b2) = toi.b2 {
let co_parent2: &ColliderParent = colliders.get(toi.c2.0).unwrap();
let (rb_pos2, rb_vels2, rb_mprops2): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
) = bodies.index_bundle(b2.0);
let local_com2 = &rb_mprops2.mass_properties.local_com;
let frozen2 = frozen.get(&b2);
let pos2 = frozen2
.map(|t| rb_vels2.integrate(*t, &rb_pos2.position, local_com2))
.unwrap_or(rb_pos2.next_position);
pos2 * co_parent2.pos_wrt_parent
} else {
co_pos2.0
};
let prev_coll_pos12 = co_pos1.inv_mul(&co_pos2);
let next_coll_pos12 = co_next_pos1.inv_mul(&co_next_pos2);
let query_dispatcher = self.query_pipeline.query_dispatcher();
let intersect_before = query_dispatcher
.intersection_test(&prev_coll_pos12, co1.shape(), co2.shape())
.intersection_test(&prev_coll_pos12, co_shape1.as_ref(), co_shape2.as_ref())
.unwrap_or(false);
let intersect_after = query_dispatcher
.intersection_test(&next_coll_pos12, co1.shape(), co2.shape())
.intersection_test(&next_coll_pos12, co_shape1.as_ref(), co_shape2.as_ref())
.unwrap_or(false);
if !intersect_before && !intersect_after {

112
src/dynamics/ccd/toi_entry.rs
View File

@@ -1,5 +1,9 @@
use crate::dynamics::{RigidBody, RigidBodyHandle};
use crate::geometry::{Collider, ColliderHandle};
use crate::dynamics::{
RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::geometry::{
ColliderHandle, ColliderParent, ColliderPosition, ColliderShape, ColliderType,
};
use crate::math::Real;
use parry::query::{NonlinearRigidMotion, QueryDispatcher};
@@ -7,9 +11,9 @@ use parry::query::{NonlinearRigidMotion, QueryDispatcher};
pub struct TOIEntry {
pub toi: Real,
pub c1: ColliderHandle,
pub b1: RigidBodyHandle,
pub b1: Option<RigidBodyHandle>,
pub c2: ColliderHandle,
pub b2: RigidBodyHandle,
pub b2: Option<RigidBodyHandle>,
pub is_intersection_test: bool,
pub timestamp: usize,
}
@@ -18,9 +22,9 @@ impl TOIEntry {
fn new(
toi: Real,
c1: ColliderHandle,
b1: RigidBodyHandle,
b1: Option<RigidBodyHandle>,
c2: ColliderHandle,
b2: RigidBodyHandle,
b2: Option<RigidBodyHandle>,
is_intersection_test: bool,
timestamp: usize,
) -> Self {
@@ -39,10 +43,30 @@ impl TOIEntry {
query_dispatcher: &QD,
ch1: ColliderHandle,
ch2: ColliderHandle,
c1: &Collider,
c2: &Collider,
b1: &RigidBody,
b2: &RigidBody,
c1: (
&ColliderType,
&ColliderShape,
&ColliderPosition,
Option<&ColliderParent>,
),
c2: (
&ColliderType,
&ColliderShape,
&ColliderPosition,
Option<&ColliderParent>,
),
b1: Option<(
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyCcd,
)>,
b2: Option<(
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyCcd,
)>,
frozen1: Option<Real>,
frozen2: Option<Real>,
start_time: Real,
@@ -50,35 +74,46 @@ impl TOIEntry {
smallest_contact_dist: Real,
) -> Option<Self> {
assert!(start_time <= end_time);
if b1.is_none() && b2.is_none() {
return None;
}
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();
let (co_type1, co_shape1, co_pos1, co_parent1) = c1;
let (co_type2, co_shape2, co_pos2, co_parent2) = c2;
let linvel1 =
frozen1.is_none() as u32 as Real * b1.map(|b| b.1.linvel).unwrap_or(na::zero());
let linvel2 =
frozen2.is_none() as u32 as Real * b2.map(|b| b.1.linvel).unwrap_or(na::zero());
let angvel1 =
frozen1.is_none() as u32 as Real * b1.map(|b| b.1.angvel).unwrap_or(na::zero());
let angvel2 =
frozen2.is_none() as u32 as Real * b2.map(|b| b.1.angvel).unwrap_or(na::zero());
#[cfg(feature = "dim2")]
let vel12 = (linvel2 - linvel1).norm()
+ angvel1.abs() * b1.ccd_max_dist
+ angvel2.abs() * b2.ccd_max_dist;
+ angvel1.abs() * b1.map(|b| b.3.ccd_max_dist).unwrap_or(0.0)
+ angvel2.abs() * b2.map(|b| b.3.ccd_max_dist).unwrap_or(0.0);
#[cfg(feature = "dim3")]
let vel12 = (linvel2 - linvel1).norm()
+ angvel1.norm() * b1.ccd_max_dist
+ angvel2.norm() * b2.ccd_max_dist;
+ angvel1.norm() * b1.map(|b| b.3.ccd_max_dist).unwrap_or(0.0)
+ angvel2.norm() * b2.map(|b| b.3.ccd_max_dist).unwrap_or(0.0);
// 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())
let thickness = (co_shape1.0.ccd_thickness() + co_shape2.0.ccd_thickness())
+ smallest_contact_dist.max(0.0);
let is_intersection_test = c1.is_sensor() || c2.is_sensor();
let is_intersection_test = co_type1.is_sensor() || co_type2.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);
let identity = NonlinearRigidMotion::identity();
let mut motion1 = b1.map(Self::body_motion).unwrap_or(identity);
let mut motion2 = b2.map(Self::body_motion).unwrap_or(identity);
if let Some(t) = frozen1 {
motion1.freeze(t);
@@ -88,8 +123,8 @@ impl TOIEntry {
motion2.freeze(t);
}
let motion_c1 = motion1.prepend(*c1.position_wrt_parent());
let motion_c2 = motion2.prepend(*c2.position_wrt_parent());
let motion_c1 = motion1.prepend(co_parent1.map(|p| p.pos_wrt_parent).unwrap_or(co_pos1.0));
let motion_c2 = motion2.prepend(co_parent2.map(|p| p.pos_wrt_parent).unwrap_or(co_pos2.0));
// println!("start_time: {}", start_time);
@@ -105,9 +140,9 @@ impl TOIEntry {
let res_toi = query_dispatcher
.nonlinear_time_of_impact(
&motion_c1,
c1.shape(),
co_shape1.as_ref(),
&motion_c2,
c2.shape(),
co_shape2.as_ref(),
start_time,
end_time,
stop_at_penetration,
@@ -119,24 +154,31 @@ impl TOIEntry {
Some(Self::new(
toi.toi,
ch1,
c1.parent(),
co_parent1.map(|p| p.handle),
ch2,
c2.parent(),
co_parent2.map(|p| p.handle),
is_intersection_test,
0,
))
}
fn body_motion(body: &RigidBody) -> NonlinearRigidMotion {
if body.is_ccd_active() {
fn body_motion(
(poss, vels, mprops, ccd): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyCcd,
),
) -> NonlinearRigidMotion {
if ccd.ccd_active {
NonlinearRigidMotion::new(
body.position,
body.mass_properties.local_com,
body.linvel,
body.angvel,
poss.position,
mprops.mass_properties.local_com,
vels.linvel,
vels.angvel,
)
} else {
NonlinearRigidMotion::constant_position(body.next_position)
NonlinearRigidMotion::constant_position(poss.next_position)
}
}
}