aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/counters/collision_detection_counters.rs7
-rw-r--r--src/counters/mod.rs12
-rw-r--r--src/counters/stages_counters.rs9
-rw-r--r--src/dynamics/ccd/toi_entry.rs16
-rw-r--r--src/dynamics/integration_parameters.rs12
-rw-r--r--src/dynamics/rigid_body.rs32
-rw-r--r--src/pipeline/mod.rs2
-rw-r--r--src/pipeline/physics_pipeline.rs145
-rw-r--r--src/pipeline/query_pipeline.rs95
9 files changed, 241 insertions, 89 deletions
diff --git a/src/counters/collision_detection_counters.rs b/src/counters/collision_detection_counters.rs
index d164452..90c5141 100644
--- a/src/counters/collision_detection_counters.rs
+++ b/src/counters/collision_detection_counters.rs
@@ -21,6 +21,13 @@ impl CollisionDetectionCounters {
narrow_phase_time: Timer::new(),
}
}
+
+ /// Resets all the coounters and timers.
+ pub fn reset(&mut self) {
+ self.ncontact_pairs = 0;
+ self.broad_phase_time.reset();
+ self.narrow_phase_time.reset();
+ }
}
impl Display for CollisionDetectionCounters {
diff --git a/src/counters/mod.rs b/src/counters/mod.rs
index c172350..4d4d05d 100644
--- a/src/counters/mod.rs
+++ b/src/counters/mod.rs
@@ -114,6 +114,18 @@ impl Counters {
pub fn set_ncontact_pairs(&mut self, n: usize) {
self.cd.ncontact_pairs = n;
}
+
+ /// Resets all the counters and timers.
+ pub fn reset(&mut self) {
+ if self.enabled {
+ self.step_time.reset();
+ self.custom.reset();
+ self.stages.reset();
+ self.cd.reset();
+ self.solver.reset();
+ self.ccd.reset();
+ }
+ }
}
macro_rules! measure_method {
diff --git a/src/counters/stages_counters.rs b/src/counters/stages_counters.rs
index 856b759..b61adb7 100644
--- a/src/counters/stages_counters.rs
+++ b/src/counters/stages_counters.rs
@@ -27,6 +27,15 @@ impl StagesCounters {
ccd_time: Timer::new(),
}
}
+
+ /// Resets all the counters and timers.
+ pub fn reset(&mut self) {
+ self.update_time.reset();
+ self.collision_detection_time.reset();
+ self.island_construction_time.reset();
+ self.solver_time.reset();
+ self.ccd_time.reset();
+ }
}
impl Display for StagesCounters {
diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs
index 6679a91..3916a4b 100644
--- a/src/dynamics/ccd/toi_entry.rs
+++ b/src/dynamics/ccd/toi_entry.rs
@@ -1,4 +1,4 @@
-use crate::dynamics::{IntegrationParameters, RigidBody, RigidBodyHandle};
+use crate::dynamics::{RigidBody, RigidBodyHandle};
use crate::geometry::{Collider, ColliderHandle};
use crate::math::Real;
use parry::query::{NonlinearRigidMotion, QueryDispatcher};
@@ -36,7 +36,6 @@ impl TOIEntry {
}
pub fn try_from_colliders<QD: ?Sized + QueryDispatcher>(
- params: &IntegrationParameters,
query_dispatcher: &QD,
ch1: ColliderHandle,
ch2: ColliderHandle,
@@ -56,16 +55,11 @@ impl TOIEntry {
let vel12 = linvel2 - linvel1;
let thickness = c1.shape().ccd_thickness() + c2.shape().ccd_thickness();
-
- if params.dt * vel12.norm() < thickness {
- return None;
- }
-
let is_intersection_test = c1.is_sensor() || c2.is_sensor();
// Compute the TOI.
- let mut motion1 = Self::body_motion(params.dt, b1);
- let mut motion2 = Self::body_motion(params.dt, b2);
+ let mut motion1 = Self::body_motion(b1);
+ let mut motion2 = Self::body_motion(b2);
if let Some(t) = frozen1 {
motion1.freeze(t);
@@ -114,8 +108,8 @@ impl TOIEntry {
))
}
- fn body_motion(dt: Real, body: &RigidBody) -> NonlinearRigidMotion {
- if body.should_resolve_ccd(dt) {
+ fn body_motion(body: &RigidBody) -> NonlinearRigidMotion {
+ if body.is_ccd_active() {
NonlinearRigidMotion::new(
0.0,
body.position,
diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index 136e345..615bfee 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/src/dynamics/integration_parameters.rs
@@ -6,6 +6,17 @@ use crate::math::Real;
pub struct IntegrationParameters {
/// The timestep length (default: `1.0 / 60.0`)
pub dt: Real,
+ /// Minimum timestep size when using CCD with multiple substeps (default `1.0 / 60.0 / 100.0`)
+ ///
+ /// When CCD with multiple substeps is enabled, the timestep is subdivided
+ /// into smaller pieces. This timestep subdivision won't generate timestep
+ /// lengths smaller than `min_dt`.
+ ///
+ /// Setting this to a large value will reduce the opportunity to performing
+ /// CCD substepping, resulting in potentially more time dropped by the
+ /// motion-clamping mechanism. Setting this to an very small value may lead
+ /// to numerical instabilities.
+ pub min_ccd_dt: Real,
// /// If `true` and if rapier is compiled with the `parallel` feature, this will enable rayon-based multithreading (default: `true`).
// ///
@@ -195,6 +206,7 @@ impl Default for IntegrationParameters {
fn default() -> Self {
Self {
dt: 1.0 / 60.0,
+ min_ccd_dt: 1.0 / 60.0 / 100.0,
// multithreading_enabled: true,
return_after_ccd_substep: false,
erp: 0.2,
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 3c6bd65..08c5629 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -5,7 +5,7 @@ use crate::geometry::{
use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector,
};
-use crate::utils::{self, WCross, WDot};
+use crate::utils::{self, WAngularInertia, WCross, WDot};
use na::ComplexField;
use num::Zero;
@@ -37,6 +37,7 @@ bitflags::bitflags! {
const ROTATION_LOCKED_Y = 1 << 2;
const ROTATION_LOCKED_Z = 1 << 3;
const CCD_ENABLED = 1 << 4;
+ const CCD_ACTIVE = 1 << 5;
}
}
@@ -204,12 +205,20 @@ impl RigidBody {
self.flags.contains(RigidBodyFlags::CCD_ENABLED)
}
- pub(crate) fn is_moving_fast(&self, dt: Real) -> bool {
- self.is_dynamic() && self.linvel.norm() * dt > self.ccd_thickness
+ // 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 {
+ 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);
+ self.flags.set(RigidBodyFlags::CCD_ACTIVE, ccd_active);
}
- pub(crate) fn should_resolve_ccd(&self, dt: Real) -> bool {
- self.is_ccd_enabled() && self.is_moving_fast(dt)
+ pub(crate) fn is_moving_fast(&self, dt: Real) -> bool {
+ self.is_dynamic() && self.linvel.norm() * dt > self.ccd_thickness
}
/// Sets the rigid-body's mass properties.
@@ -373,6 +382,19 @@ impl RigidBody {
!self.linvel.is_zero() || !self.angvel.is_zero()
}
+ pub(crate) 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
+ .transform_vector(self.torque * dt);
+ let linvel = self.linvel + dlinvel;
+ let angvel = self.angvel + dangvel;
+
+ let com = self.position * self.mass_properties.local_com;
+ let shift = Translation::from(com.coords);
+ shift * Isometry::new(linvel * dt, angvel * dt) * shift.inverse() * self.position
+ }
+
pub(crate) fn integrate_velocity(&self, dt: Real) -> Isometry<Real> {
let com = self.position * self.mass_properties.local_com;
let shift = Translation::from(com.coords);
diff --git a/src/pipeline/mod.rs b/src/pipeline/mod.rs
index fd85cfa..5fad9bc 100644
--- a/src/pipeline/mod.rs
+++ b/src/pipeline/mod.rs
@@ -6,7 +6,7 @@ pub use physics_hooks::{
ContactModificationContext, PairFilterContext, PhysicsHooks, PhysicsHooksFlags,
};
pub use physics_pipeline::PhysicsPipeline;
-pub use query_pipeline::QueryPipeline;
+pub use query_pipeline::{QueryPipeline, QueryPipelineMode};
mod collision_pipeline;
mod event_handler;
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index 296a400..99e1bd9 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -68,8 +68,8 @@ impl PhysicsPipeline {
hooks: &dyn PhysicsHooks,
events: &dyn EventHandler,
) {
- self.counters.stages.collision_detection_time.start();
- self.counters.cd.broad_phase_time.start();
+ self.counters.stages.collision_detection_time.resume();
+ self.counters.cd.broad_phase_time.resume();
// Update broad-phase.
self.broad_phase_events.clear();
@@ -81,7 +81,7 @@ impl PhysicsPipeline {
);
self.counters.cd.broad_phase_time.pause();
- self.counters.cd.narrow_phase_time.start();
+ self.counters.cd.narrow_phase_time.resume();
// Update narrow-phase.
narrow_phase.handle_user_changes(colliders, bodies, events);
@@ -155,7 +155,7 @@ impl PhysicsPipeline {
colliders: &mut ColliderSet,
joints: &mut JointSet,
) {
- self.counters.stages.island_construction_time.start();
+ self.counters.stages.island_construction_time.resume();
bodies.update_active_set_with_contacts(
colliders,
narrow_phase,
@@ -178,15 +178,14 @@ impl PhysicsPipeline {
narrow_phase.select_active_contacts(bodies, &mut manifolds, &mut self.manifold_indices);
joints.select_active_interactions(bodies, &mut self.joint_constraint_indices);
- self.counters.stages.update_time.start();
+ self.counters.stages.update_time.resume();
bodies.foreach_active_dynamic_body_mut_internal(|_, b| {
b.update_world_mass_properties();
b.add_gravity(*gravity)
});
self.counters.stages.update_time.pause();
- self.counters.solver.reset();
- self.counters.stages.solver_time.start();
+ self.counters.stages.solver_time.resume();
if self.solvers.len() < bodies.num_islands() {
self.solvers
.resize_with(bodies.num_islands(), IslandSolver::new);
@@ -259,20 +258,17 @@ impl PhysicsPipeline {
integration_parameters: &IntegrationParameters,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
- ccd_solver: Option<&mut CCDSolver>,
+ ccd_solver: &mut CCDSolver,
events: &dyn EventHandler,
) {
// Handle CCD
- if let Some(ccd_solver) = ccd_solver {
- let impacts = ccd_solver.predict_next_impacts(
- integration_parameters,
- bodies,
- colliders,
- integration_parameters.dt,
- events,
- );
- ccd_solver.clamp_motions(integration_parameters.dt, bodies, &impacts);
- }
+ let impacts = ccd_solver.predict_impacts_at_next_positions(
+ integration_parameters.dt,
+ bodies,
+ colliders,
+ events,
+ );
+ ccd_solver.clamp_motions(integration_parameters.dt, bodies, &impacts);
}
fn advance_to_final_positions(
@@ -317,15 +313,15 @@ impl PhysicsPipeline {
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
- ccd_solver: Option<&mut CCDSolver>,
+ ccd_solver: &mut CCDSolver,
hooks: &dyn PhysicsHooks,
events: &dyn EventHandler,
) {
+ self.counters.reset();
self.counters.step_started();
colliders.handle_user_changes(bodies);
bodies.handle_user_changes(colliders);
- self.interpolate_kinematic_velocities(integration_parameters, bodies);
self.detect_collisions_after_user_modifications(
integration_parameters,
broad_phase,
@@ -335,33 +331,90 @@ impl PhysicsPipeline {
hooks,
events,
);
- self.build_islands_and_solve_constraints(
- gravity,
- integration_parameters,
- narrow_phase,
- bodies,
- colliders,
- joints,
- );
- self.run_ccd_motion_clamping(
- integration_parameters,
- bodies,
- colliders,
- ccd_solver,
- events,
- );
- self.advance_to_final_positions(bodies, colliders);
- self.detect_collisions_after_integration(
- integration_parameters,
- broad_phase,
- narrow_phase,
- bodies,
- colliders,
- hooks,
- events,
- );
- bodies.modified_inactive_set.clear();
+ let mut remaining_time = integration_parameters.dt;
+ let mut remaining_substeps = integration_parameters.max_ccd_substeps;
+ let mut integration_parameters = *integration_parameters;
+
+ let ccd_active = ccd_solver.update_ccd_active_flags(bodies, integration_parameters.dt);
+
+ loop {
+ if ccd_active && remaining_substeps > 1 {
+ // If there are more than one CCD substep, we need to split
+ // the timestep into multiple intervals. First, estimate the
+ // size of the time slice we will integrate for this substep.
+ //
+ // If there is only one or zero CCD substep, there is no need
+ // to split the timetsep interval. So we can just skip this part.
+ if let Some(toi) = ccd_solver.find_first_impact(remaining_time, bodies, colliders) {
+ let original_interval = remaining_time / (remaining_substeps as Real);
+
+ if toi < original_interval {
+ integration_parameters.dt = original_interval;
+ } else {
+ integration_parameters.dt =
+ toi + (remaining_time - toi) / (remaining_substeps as Real);
+ }
+
+ remaining_substeps -= 1;
+ } else {
+ // No impact, don't do any other substep after this one.
+ integration_parameters.dt = remaining_time;
+ remaining_substeps = 1;
+ }
+
+ remaining_time -= integration_parameters.dt;
+
+ // Avoid substep length that are too small.
+ if remaining_time <= integration_parameters.min_ccd_dt {
+ integration_parameters.dt += remaining_time;
+ remaining_substeps = 1;
+ }
+ } else {
+ integration_parameters.dt = remaining_time;
+ remaining_time = 0.0;
+ remaining_substeps = 1;
+ }
+
+ self.interpolate_kinematic_velocities(&integration_parameters, bodies);
+ self.build_islands_and_solve_constraints(
+ gravity,
+ &integration_parameters,
+ narrow_phase,
+ bodies,
+ colliders,
+ joints,
+ );
+
+ // If CCD is enabled, execute the CCD motion clamping.
+ if ccd_active && remaining_substeps > 0 {
+ self.run_ccd_motion_clamping(
+ &integration_parameters,
+ bodies,
+ colliders,
+ ccd_solver,
+ events,
+ );
+ }
+
+ self.advance_to_final_positions(bodies, colliders);
+ self.detect_collisions_after_integration(
+ &integration_parameters,
+ broad_phase,
+ narrow_phase,
+ bodies,
+ colliders,
+ hooks,
+ events,
+ );
+
+ bodies.modified_inactive_set.clear();
+
+ if !ccd_active || remaining_substeps <= 1 {
+ // We executed all the substeps.
+ break;
+ }
+ }
self.counters.step_completed();
}
}
diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs
index e89a03e..5b2cc88 100644
--- a/src/pipeline/query_pipeline.rs
+++ b/src/pipeline/query_pipeline.rs
@@ -38,6 +38,12 @@ struct QueryPipelineAsCompositeShape<'a> {
groups: InteractionGroups,
}
+pub enum QueryPipelineMode {
+ CurrentPosition,
+ SweepTestWithNextPosition,
+ SweepTestWithPredictedPosition { dt: Real },
+}
+
impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> {
type PartShape = dyn Shape;
type PartId = ColliderHandle;
@@ -113,18 +119,40 @@ impl QueryPipeline {
}
/// Update the acceleration structure on the query pipeline.
- pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet, use_swept_aabb: bool) {
+ pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) {
+ self.update_with_mode(bodies, colliders, QueryPipelineMode::CurrentPosition)
+ }
+
+ /// Update the acceleration structure on the query pipeline.
+ pub fn update_with_mode(
+ &mut self,
+ bodies: &RigidBodySet,
+ colliders: &ColliderSet,
+ mode: QueryPipelineMode,
+ ) {
if !self.tree_built {
- if !use_swept_aabb {
- let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb()));
- self.quadtree.clear_and_rebuild(data, self.dilation_factor);
- } else {
- let data = colliders.iter().map(|(h, co)| {
- let next_position =
- bodies[co.parent()].next_position * co.position_wrt_parent();
- (h, co.compute_swept_aabb(&next_position))
- });
- self.quadtree.clear_and_rebuild(data, self.dilation_factor);
+ match mode {
+ QueryPipelineMode::CurrentPosition => {
+ let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb()));
+ self.quadtree.clear_and_rebuild(data, self.dilation_factor);
+ }
+ QueryPipelineMode::SweepTestWithNextPosition => {
+ let data = colliders.iter().map(|(h, co)| {
+ let next_position =
+ bodies[co.parent()].next_position * co.position_wrt_parent();
+ (h, co.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()]
+ .predict_position_using_velocity_and_forces(dt)
+ * co.position_wrt_parent();
+ (h, co.compute_swept_aabb(&next_position))
+ });
+ self.quadtree.clear_and_rebuild(data, self.dilation_factor);
+ }
}
// FIXME: uncomment this once we handle insertion/removals properly.
@@ -141,21 +169,36 @@ impl QueryPipeline {
}
}
- if !use_swept_aabb {
- self.quadtree.update(
- |handle| colliders[*handle].compute_aabb(),
- self.dilation_factor,
- );
- } else {
- self.quadtree.update(
- |handle| {
- let co = &colliders[*handle];
- let next_position =
- bodies[co.parent()].next_position * co.position_wrt_parent();
- co.compute_swept_aabb(&next_position)
- },
- self.dilation_factor,
- );
+ match mode {
+ QueryPipelineMode::CurrentPosition => {
+ self.quadtree.update(
+ |handle| colliders[*handle].compute_aabb(),
+ self.dilation_factor,
+ );
+ }
+ QueryPipelineMode::SweepTestWithNextPosition => {
+ self.quadtree.update(
+ |handle| {
+ let co = &colliders[*handle];
+ let next_position =
+ bodies[co.parent()].next_position * co.position_wrt_parent();
+ co.compute_swept_aabb(&next_position)
+ },
+ self.dilation_factor,
+ );
+ }
+ QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
+ self.quadtree.update(
+ |handle| {
+ let co = &colliders[*handle];
+ let next_position = bodies[co.parent()]
+ .predict_position_using_velocity_and_forces(dt)
+ * co.position_wrt_parent();
+ co.compute_swept_aabb(&next_position)
+ },
+ self.dilation_factor,
+ );
+ }
}
}
generated by cgit (git 2.34.1) at 2026-02-04 19:38:11 +0000