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

2
src/dynamics/coefficient_combine_rule.rs
View File

@@ -21,7 +21,7 @@ pub enum CoefficientCombineRule {
}
impl CoefficientCombineRule {
pub fn from_value(val: u8) -> Self {
pub(crate) fn from_value(val: u8) -> Self {
match val {
0 => CoefficientCombineRule::Average,
1 => CoefficientCombineRule::Min,

66
src/dynamics/rigid_body.rs
View File

@@ -113,6 +113,7 @@ pub struct RigidBody {
/// User-defined data associated to this rigid-body.
pub user_data: u128,
pub(crate) ccd_thickness: Real,
pub(crate) ccd_max_dist: Real,
}
impl RigidBody {
@@ -146,6 +147,7 @@ impl RigidBody {
dominance_group: 0,
user_data: 0,
ccd_thickness: Real::MAX,
ccd_max_dist: 0.0,
}
}
@@ -172,8 +174,6 @@ impl RigidBody {
self.linvel += linear_acc * dt;
self.angvel += angular_acc * dt;
self.force = na::zero();
self.torque = na::zero();
}
/// The mass properties of this rigid-body.
@@ -208,17 +208,56 @@ impl RigidBody {
// This is different from `is_ccd_enabled`. This checks that CCD
// is active for this rigid-body, i.e., if it was seen to move fast
// enough to justify a CCD run.
pub(crate) fn is_ccd_active(&self) -> bool {
/// Is CCD active for this rigid-body?
///
/// The CCD is considered active if the rigid-body is moving at
/// a velocity greater than an automatically-computed threshold.
///
/// This is not the same as `self.is_ccd_enabled` which only
/// checks if CCD is allowed to run for this rigid-body or if
/// it is completely disabled (independently from its velocity).
pub fn is_ccd_active(&self) -> bool {
self.flags.contains(RigidBodyFlags::CCD_ACTIVE)
}
pub(crate) fn update_ccd_active_flag(&mut self, dt: Real) {
let ccd_active = self.is_ccd_enabled() && self.is_moving_fast(dt);
pub(crate) fn update_ccd_active_flag(&mut self, dt: Real, include_forces: bool) {
let ccd_active = self.is_ccd_enabled() && self.is_moving_fast(dt, include_forces);
self.flags.set(RigidBodyFlags::CCD_ACTIVE, ccd_active);
}
pub(crate) fn is_moving_fast(&self, dt: Real) -> bool {
self.is_dynamic() && self.linvel.norm() * dt > self.ccd_thickness
pub(crate) fn is_moving_fast(&self, dt: Real, include_forces: bool) -> bool {
if self.is_dynamic() {
// NOTE: for the threshold we don't use the exact CCD thickness. Theoretically, we
// should use `self.ccd_thickness - smallest_contact_dist` where `smallest_contact_dist`
// is the deepest contact (the contact with the largest penetration depth, i.e., the
// negative `dist` with the largest absolute value.
// However, getting this penetration depth assumes querying the contact graph from
// the narrow-phase, which can be pretty expensive. So we use the CCD thickness
// divided by 10 right now. We will see in practice if this value is OK or if we
// should use a smaller (to be less conservative) or larger divisor (to be more conservative).
let threshold = self.ccd_thickness / 10.0;
if include_forces {
let linear_part = (self.linvel + self.force * dt).norm();
#[cfg(feature = "dim2")]
let angular_part = (self.angvel + self.torque * dt).abs() * self.ccd_max_dist;
#[cfg(feature = "dim3")]
let angular_part = (self.angvel + self.torque * dt).norm() * self.ccd_max_dist;
let vel_with_forces = linear_part + angular_part;
vel_with_forces > threshold
} else {
self.max_point_velocity() * dt > threshold
}
} else {
false
}
}
pub(crate) fn max_point_velocity(&self) -> Real {
#[cfg(feature = "dim2")]
return self.linvel.norm() + self.angvel.abs() * self.ccd_max_dist;
#[cfg(feature = "dim3")]
return self.linvel.norm() + self.angvel.norm() * self.ccd_max_dist;
}
/// Sets the rigid-body's mass properties.
@@ -301,6 +340,13 @@ impl RigidBody {
self.ccd_thickness = self.ccd_thickness.min(coll.shape().ccd_thickness());
let shape_bsphere = coll
.shape()
.compute_bounding_sphere(coll.position_wrt_parent());
self.ccd_max_dist = self
.ccd_max_dist
.max(shape_bsphere.center.coords.norm() + shape_bsphere.radius);
let mass_properties = coll
.mass_properties()
.transform_by(coll.position_wrt_parent());
@@ -311,7 +357,7 @@ impl RigidBody {
pub(crate) fn update_colliders_positions(&mut self, colliders: &mut ColliderSet) {
for handle in &self.colliders {
// NOTE: we don't use `get_mut_internal` here because we want to
// NOTE: we use `get_mut_internal_with_modification_tracking` here because we want to
// benefit from the modification tracking to know the colliders
// we need to update the broad-phase and narrow-phase for.
let collider = colliders
@@ -382,7 +428,9 @@ impl RigidBody {
!self.linvel.is_zero() || !self.angvel.is_zero()
}
pub(crate) fn predict_position_using_velocity_and_forces(&self, dt: Real) -> Isometry<Real> {
/// Computes the predict position of this rigid-body after `dt` seconds, taking
/// into account its velocities and external forces applied to it.
pub fn predict_position_using_velocity_and_forces(&self, dt: Real) -> Isometry<Real> {
let dlinvel = self.force * (self.effective_inv_mass * dt);
let dangvel = self
.effective_world_inv_inertia_sqrt

7
src/dynamics/solver/interaction_groups.rs
View File

@@ -157,6 +157,13 @@ impl InteractionGroups {
}
}
pub fn clear(&mut self) {
self.buckets.clear();
self.body_masks.clear();
self.grouped_interactions.clear();
self.nongrouped_interactions.clear();
}
// FIXME: there is a lot of duplicated code with group_manifolds here.
// But we don't refactor just now because we may end up with distinct
// grouping strategies in the future.

8
src/geometry/collider.rs
View File

@@ -660,6 +660,14 @@ impl ColliderBuilder {
/// Sets the initial position (translation and orientation) of the collider to be created,
/// relative to the rigid-body it is attached to.
pub fn position_wrt_parent(mut self, pos: Isometry<Real>) -> Self {
self.delta = pos;
self
}
/// Sets the initial position (translation and orientation) of the collider to be created,
/// relative to the rigid-body it is attached to.
#[deprecated(note = "Use `.position_wrt_parent` instead.")]
pub fn position(mut self, pos: Isometry<Real>) -> Self {
self.delta = pos;
self

31
src/geometry/contact_pair.rs
View File

@@ -1,5 +1,5 @@
use crate::dynamics::{BodyPair, RigidBodyHandle};
use crate::geometry::{ColliderPair, ContactManifold};
use crate::geometry::{ColliderPair, Contact, ContactManifold};
use crate::math::{Point, Real, Vector};
use parry::query::ContactManifoldsWorkspace;
@@ -76,6 +76,35 @@ impl ContactPair {
workspace: None,
}
}
/// Finds the contact with the smallest signed distance.
///
/// If the colliders involved in this contact pair are penetrating, then
/// this returns the contact with the largest penetration depth.
///
/// Returns a reference to the contact, as well as the contact manifold
/// it is part of.
pub fn find_deepest_contact(&self) -> Option<(&ContactManifold, &Contact)> {
let mut deepest = None;
for m2 in &self.manifolds {
let deepest_candidate = m2.find_deepest_contact();
deepest = match (deepest, deepest_candidate) {
(_, None) => deepest,
(None, Some(c2)) => Some((m2, c2)),
(Some((m1, c1)), Some(c2)) => {
if c1.dist <= c2.dist {
Some((m1, c1))
} else {
Some((m2, c2))
}
}
}
}
deepest
}
}
#[derive(Clone, Debug)]

26
src/pipeline/query_pipeline.rs
View File

@@ -38,10 +38,20 @@ struct QueryPipelineAsCompositeShape<'a> {
groups: InteractionGroups,
}
/// Indicates how the colliders position should be taken into account when
/// updating the query pipeline.
pub enum QueryPipelineMode {
/// The `Collider::position` is taken into account.
CurrentPosition,
/// The `RigidBody::next_position * Collider::position_wrt_parent` is taken into account for
/// the colliders positions.
SweepTestWithNextPosition,
SweepTestWithPredictedPosition { dt: Real },
/// The `RigidBody::predict_position_using_velocity_and_forces * Collider::position_wrt_parent`
/// is taken into account for the colliders position.
SweepTestWithPredictedPosition {
/// The time used to integrate the rigid-body's velocity and acceleration.
dt: Real,
},
}
impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> {
@@ -137,19 +147,19 @@ impl QueryPipeline {
self.quadtree.clear_and_rebuild(data, self.dilation_factor);
}
QueryPipelineMode::SweepTestWithNextPosition => {
let data = colliders.iter().map(|(h, co)| {
let data = colliders.iter().map(|(h, c)| {
let next_position =
bodies[co.parent()].next_position * co.position_wrt_parent();
(h, co.compute_swept_aabb(&next_position))
bodies[c.parent()].next_position * c.position_wrt_parent();
(h, c.compute_swept_aabb(&next_position))
});
self.quadtree.clear_and_rebuild(data, self.dilation_factor);
}
QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
let data = colliders.iter().map(|(h, co)| {
let next_position = bodies[co.parent()]
let data = colliders.iter().map(|(h, c)| {
let next_position = bodies[c.parent()]
.predict_position_using_velocity_and_forces(dt)
* co.position_wrt_parent();
(h, co.compute_swept_aabb(&next_position))
* c.position_wrt_parent();
(h, c.compute_swept_aabb(&next_position))
});
self.quadtree.clear_and_rebuild(data, self.dilation_factor);
}