diff options
Diffstat (limited to 'src/dynamics')
47 files changed, 4025 insertions, 2662 deletions
diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index ff463c7..b348283 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -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,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.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 +86,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 +152,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,12 +198,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 +219,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 +253,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 +281,7 @@ impl CCDSolver { // Update the query pipeline. self.query_pipeline.update_with_mode( + islands, bodies, colliders, QueryPipelineMode::SweepTestWithNextPosition, @@ -207,71 +293,95 @@ 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: (_, _, _) = 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 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, co_parent1), + (c2.0, c2.1, c2.2, 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 + }); + } } } @@ -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,53 @@ 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; + // NOTE: the 1 and 2 indices (e.g., `ch1`, `ch2`) bellow are unrelated to the + // ones we used above. 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)> = + bh2.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 +496,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 +522,57 @@ impl CCDSolver { // - If the intersection isn't active anymore, and it |