diff options
Diffstat (limited to 'src/dynamics')
51 files changed, 4295 insertions, 2739 deletions
diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index ff463c7..7e95f08 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -1,9 +1,16 @@ 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}; +use crate::prelude::{ActiveEvents, ColliderFlags}; use parry::query::{DefaultQueryDispatcher, QueryDispatcher}; use parry::utils::hashmap::HashMap; use std::collections::BinaryHeap; @@ -44,19 +51,33 @@ 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.local_mprops.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,65 @@ 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> + + ComponentSet<ColliderFlags>, + { // Update the query pipeline. self.query_pipeline.update_with_mode( + islands, bodies, colliders, QueryPipelineMode::SweepTestWithPredictedPosition { dt }, @@ -102,19 +154,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 ch1 in &rb1.colliders { - let co1 = &colliders[*ch1]; - - if co1.is_sensor() { + for handle in islands.active_dynamic_bodies() { + let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0); + + 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); + + let predicted_body_pos1 = + rb_pos1.integrate_forces_and_velocities(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,13 +200,23 @@ impl CCDSolver { ) .is_none() { - let c1 = colliders.get(*ch1).unwrap(); - let c2 = colliders.get(*ch2).unwrap(); - let bh1 = c1.parent(); - let bh2 = c2.parent(); - - if bh1 == bh2 || (c1.is_sensor() || c2.is_sensor()) { - // Ignore self-intersection and sensors. + let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); + let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0); + let c1: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch1.0); + let c2: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch2.0); + let co_type1: &ColliderType = colliders.index(ch1.0); + let co_type2: &ColliderType = colliders.index(ch1.0); + + let bh1 = co_parent1.map(|p| p.handle); + let bh2 = co_parent2.map(|p| p.handle); + + // Ignore self-intersection and sensors and apply collision groups filter. + if bh1 == bh2 // Ignore self-intersection. + || (co_type1.is_sensor() || co_type2.is_sensor()) // Ignore sensors. + || !c1.3.collision_groups.test(c2.3.collision_groups) // Apply collision groups. + || !c1.3.solver_groups.test(c2.3.solver_groups) + // Apply solver groups. + { return true; } @@ -146,16 +226,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, c1.3, co_parent1), + (c2.0, c2.1, c2.2, c2.3, co_parent2), + Some((rb_pos1, rb_vels1, rb_mprops1, rb_ccd1)), b2, None, None, @@ -181,14 +260,29 @@ 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> + + ComponentSet<ColliderFlags> + + ComponentSet<ColliderFlags>, + { let mut frozen = HashMap::<_, Real>::default(); let mut all_toi = BinaryHeap::new(); let mut pairs_seen = HashMap::default(); @@ -196,6 +290,7 @@ impl CCDSolver { // Update the query pipeline. self.query_pipeline.update_with_mode( + islands, bodies, colliders, QueryPipelineMode::SweepTestWithNextPosition, @@ -207,71 +302,96 @@ 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); + + 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); + + let predicted_body_pos1 = + rb_pos1.integrate_forces_and_velocities(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): (&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(&aabb, |ch2| { - if ch1 == *ch2 { - // Ignore self-intersection. - return true; - } - - 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(); - - if bh1 == bh2 { + 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(); - - 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, - *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 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: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch1.0); + let c2: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch2.0); + + let bh1 = co_parent1.map(|p| p.handle); + let bh2 = co_parent2.map(|p| p.handle); + + // Ignore self-intersections and apply groups filter. + if bh1 == bh2 || !c1.3.collision_groups.test(c2.3.collision_groups) + { + 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 = bh1.map(|h| bodies.index_bundle(h.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, c1.3, co_parent1), + (c2.0, c2.1, c2.2, c2.3, co_parent2), + 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); + } } } - } - true - }); + true + }); + } } } @@ -288,72 +408,89 @@ impl CCDSolver { // NOTE: all static bodies (and kinematic bodies?) should be considered as "frozen", this // may avoid some resweeps. - let mut intersections_to_check = vec![]; + let mut pseudo_intersections_to_check = vec![]; while let Some(toi) = all_toi.pop() { assert!(toi.toi <= dt); - let body1 = bodies.get(toi.b1).unwrap(); - let body2 = b |