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

CCD: take angular motion and penetration depth into account in various thresholds.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-03-30 17:08:51 +02:00
parent c3a0c67272
commit d2ee642053
9 changed files with 187 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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);
}

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

@@ -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 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 vel12 = linvel2 - linvel1;
let thickness = c1.shape().ccd_thickness() + c2.shape().ccd_thickness();
#[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);