diff options
Diffstat (limited to 'src')
20 files changed, 115 insertions, 205 deletions
diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index c32c402..f36f4ec 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -20,13 +20,9 @@ pub struct BodyPair { } impl BodyPair { - pub(crate) fn new(body1: RigidBodyHandle, body2: RigidBodyHandle) -> Self { + pub fn new(body1: RigidBodyHandle, body2: RigidBodyHandle) -> Self { BodyPair { body1, body2 } } - - pub(crate) fn swap(self) -> Self { - Self::new(self.body2, self.body1) - } } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -479,7 +475,7 @@ impl RigidBodySet { if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) { for inter in contacts { for manifold in &inter.2.manifolds { - if manifold.num_active_contacts > 0 { + if !manifold.data.solver_contacts.is_empty() { let other = crate::utils::other_handle( (inter.0, inter.1), *collider_handle, diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index 18e846b..b5f8173 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -338,7 +338,7 @@ impl InteractionGroups { let mut occupied_mask = 0u128; let max_interaction_points = interaction_indices .iter() - .map(|i| interactions[*i].num_active_contacts) + .map(|i| interactions[*i].data.num_active_contacts()) .max() .unwrap_or(1); @@ -351,7 +351,7 @@ impl InteractionGroups { // FIXME: how could we avoid iterating // on each interaction at every iteration on k? - if interaction.num_active_contacts != k { + if interaction.data.num_active_contacts() != k { continue; } diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs index 1ef0660..843fd1a 100644 --- a/src/dynamics/solver/position_constraint.rs +++ b/src/dynamics/solver/position_constraint.rs @@ -66,9 +66,13 @@ impl PositionConstraint { ) { let rb1 = &bodies[manifold.data.body_pair.body1]; let rb2 = &bodies[manifold.data.body_pair.body2]; - let active_contacts = &manifold.data.solver_contacts[..manifold.num_active_contacts]; - for (l, manifold_points) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() { + for (l, manifold_points) in manifold + .data + .solver_contacts + .chunks(MAX_MANIFOLD_POINTS) + .enumerate() + { let mut local_p1 = [Point::origin(); MAX_MANIFOLD_POINTS]; let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS]; let mut dists = [0.0; MAX_MANIFOLD_POINTS]; diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs index 3a3abb0..7df7d5e 100644 --- a/src/dynamics/solver/position_constraint_wide.rs +++ b/src/dynamics/solver/position_constraint_wide.rs @@ -8,7 +8,7 @@ use crate::math::{ use crate::utils::{WAngularInertia, WCross, WDot}; use num::Zero; -use simba::simd::{SimdBool as _, SimdComplexField, SimdPartialOrd, SimdValue}; +use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue}; pub(crate) struct WPositionConstraint { pub rb1: [usize; SIMD_WIDTH], @@ -55,10 +55,10 @@ impl WPositionConstraint { let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let num_active_contacts = manifolds[0].num_active_contacts(); + let num_active_contacts = manifolds[0].data.num_active_contacts(); for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) { - let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]; SIMD_WIDTH]; + let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..]; SIMD_WIDTH]; let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); let mut constraint = WPositionConstraint { diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs index 80175fe..fcfdcdb 100644 --- a/src/dynamics/solver/position_ground_constraint.rs +++ b/src/dynamics/solver/position_ground_constraint.rs @@ -39,9 +39,12 @@ impl PositionGroundConstraint { manifold.data.normal }; - let active_contacts = &manifold.data.solver_contacts[..manifold.num_active_contacts]; - - for (l, manifold_contacts) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() { + for (l, manifold_contacts) in manifold + .data + .solver_contacts + .chunks(MAX_MANIFOLD_POINTS) + .enumerate() + { let mut p1 = [Point::origin(); MAX_MANIFOLD_POINTS]; let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS]; let mut dists = [0.0; MAX_MANIFOLD_POINTS]; diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs index 76c78d8..dbea1db 100644 --- a/src/dynamics/solver/position_ground_constraint_wide.rs +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -8,7 +8,7 @@ use crate::math::{ use crate::utils::{WAngularInertia, WCross, WDot}; use num::Zero; -use simba::simd::{SimdBool as _, SimdComplexField, SimdPartialOrd, SimdValue}; +use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue}; pub(crate) struct WPositionGroundConstraint { pub rb2: [usize; SIMD_WIDTH], @@ -56,10 +56,10 @@ impl WPositionGroundConstraint { let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let num_active_contacts = manifolds[0].num_active_contacts(); + let num_active_contacts = manifolds[0].data.num_active_contacts(); for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) { - let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]; SIMD_WIDTH]; + let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..]; SIMD_WIDTH]; let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); let mut constraint = WPositionGroundConstraint { diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs index b5a953f..cf23168 100644 --- a/src/dynamics/solver/position_solver.rs +++ b/src/dynamics/solver/position_solver.rs @@ -1,16 +1,6 @@ -use super::{ - AnyJointPositionConstraint, InteractionGroups, PositionConstraint, PositionGroundConstraint, -}; -#[cfg(feature = "simd-is-enabled")] -use super::{WPositionConstraint, WPositionGroundConstraint}; -use crate::dynamics::solver::categorization::categorize_joints; -use crate::dynamics::{ - solver::AnyPositionConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, -}; -use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use super::AnyJointPositionConstraint; +use crate::dynamics::{solver::AnyPositionConstraint, IntegrationParameters, RigidBodySet}; use crate::math::Isometry; -#[cfg(feature = "simd-is-enabled")] -use crate::math::SIMD_WIDTH; pub(crate) struct PositionSolver { positions: Vec<Isometry<f32>>, diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs index 88371ba..92faa3b 100644 --- a/src/dynamics/solver/solver_constraints.rs +++ b/src/dynamics/solver/solver_constraints.rs @@ -9,13 +9,11 @@ use crate::dynamics::solver::{ PositionGroundConstraint, WPositionConstraint, WPositionGroundConstraint, }; use crate::dynamics::{ - solver::{AnyVelocityConstraint, DeltaVel}, - IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, + solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, }; use crate::geometry::{ContactManifold, ContactManifoldIndex}; #[cfg(feature = "simd-is-enabled")] use crate::math::SIMD_WIDTH; -use crate::utils::WAngularInertia; pub(crate) struct SolverConstraints<VelocityConstraint, PositionConstraint> { pub not_ground_interactions: Vec<usize>, diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 4f94fba..6614b48 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -150,9 +150,13 @@ impl VelocityConstraint { let mj_lambda2 = rb2.active_set_offset; let force_dir1 = -manifold.data.normal; let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; - let active_contacts = &manifold.data.solver_contacts[..manifold.num_active_contacts]; - for (l, manifold_points) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() { + for (l, manifold_points) in manifold + .data + .solver_contacts + .chunks(MAX_MANIFOLD_POINTS) + .enumerate() + { #[cfg(not(target_arch = "wasm32"))] let mut constraint = VelocityConstraint { dir1: force_dir1, @@ -383,7 +387,7 @@ impl VelocityConstraint { let k_base = self.manifold_contact_id; for k in 0..self.num_contacts as usize { - let active_contacts = manifold.active_contacts_mut(); + let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()]; active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse; #[cfg(feature = "dim2")] { diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index fddd9ef..c75d926 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -2,8 +2,7 @@ use super::{AnyVelocityConstraint, DeltaVel}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ - AngVector, AngularInertia, Isometry, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, - SIMD_WIDTH, + AngVector, AngularInertia, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; use num::Zero; @@ -100,7 +99,7 @@ impl WVelocityConstraint { let warmstart_multiplier = SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff); - let num_active_contacts = manifolds[0].num_active_contacts(); + let num_active_contacts = manifolds[0].data.num_active_contacts(); for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) { let manifold_points = array![|ii| @@ -333,7 +332,7 @@ impl WVelocityConstraint { for ii in 0..SIMD_WIDTH { let manifold = &mut manifolds_all[self.manifold_id[ii]]; let k_base = self.manifold_contact_id; - let active_contacts = manifold.active_contacts_mut(); + let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()]; active_contacts[k_base + k].data.impulse = impulses[ii]; #[cfg(feature = "dim2")] diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index cbb6bb8..fa2a23e 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -76,9 +76,13 @@ impl VelocityGroundConstraint { let mj_lambda2 = rb2.active_set_offset; let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; - let active_contacts = &manifold.data.solver_contacts[..manifold.num_active_contacts]; - for (l, manifold_points) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() { + for (l, manifold_points) in manifold + .data + .solver_contacts + .chunks(MAX_MANIFOLD_POINTS) + .enumerate() + { #[cfg(not(target_arch = "wasm32"))] let mut constraint = VelocityGroundConstraint { dir1: force_dir1, @@ -268,7 +272,7 @@ impl VelocityGroundConstraint { let k_base = self.manifold_contact_id; for k in 0..self.num_contacts as usize { - let active_contacts = manifold.active_contacts_mut(); + let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()]; active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse; #[cfg(feature = "dim2")] { diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index 3bb7393..feee344 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -2,8 +2,7 @@ use super::{AnyVelocityConstraint, DeltaVel}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ - AngVector, AngularInertia, Isometry, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, - SIMD_WIDTH, + AngVector, AngularInertia, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; use num::Zero; @@ -100,10 +99,10 @@ impl WVelocityGroundConstraint { let warmstart_multiplier = SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff); - let num_active_contacts = manifolds[0].num_active_contacts(); + let num_active_contacts = manifolds[0].data.num_active_contacts(); for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) { - let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]; SIMD_WIDTH]; + let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..]; SIMD_WIDTH]; let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); let mut constraint = WVelocityGroundConstraint { @@ -283,7 +282,7 @@ impl WVelocityGroundConstraint { for ii in 0..SIMD_WIDTH { let manifold = &mut manifolds_all[self.manifold_id[ii]]; let k_base = self.manifold_contact_id; - let active_contacts = manifold.active_contacts_mut(); + let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()]; active_contacts[k_base + k].data.impulse = impulses[ii]; #[cfg(feature = "dim2")] diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index 89f2809..332d809 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -1,20 +1,9 @@ -use super::{ - AnyJointVelocityConstraint, InteractionGroups, VelocityConstraint, VelocityGroundConstraint, -}; -#[cfg(feature = "simd-is-enabled")] -use super::{WVelocityConstraint, WVelocityGroundConstraint}; -use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints}; -use crate::dynamics::solver::{ - AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint, - PositionGroundConstraint, WPositionConstraint, WPositionGroundConstraint, -}; +use super::AnyJointVelocityConstraint; use crate::dynamics::{ solver::{AnyVelocityConstraint, DeltaVel}, - IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, + IntegrationParameters, JointGraphEdge, RigidBodySet, }; -use crate::geometry::{ContactManifold, ContactManifoldIndex}; -#[cfg(feature = "simd-is-enabled")] -use crate::math::SIMD_WIDTH; +use crate::geometry::ContactManifold; use crate::utils::WAngularInertia; pub(crate) struct VelocitySolver { diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index 6b0815b..3ad4f70 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -1,13 +1,7 @@ -use crate::dynamics::{BodyPair, RigidBodyHandle, RigidBodySet}; -use crate::geometry::{Collider, ColliderPair, ColliderSet, Contact, ContactManifold}; -use crate::math::{Isometry, Point, Real, Vector}; +use crate::dynamics::{BodyPair, RigidBodySet}; +use crate::geometry::{Collider, ColliderPair, ContactManifold}; +use crate::math::{Point, Real, Vector}; use cdl::query::ContactManifoldsWorkspace; -use cdl::utils::MaybeSerializableData; -#[cfg(feature = "simd-is-enabled")] -use { - crate::math::{SimdReal, SIMD_WIDTH}, - simba::simd::SimdValue, -}; bitflags::bitflags! { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -19,33 +13,6 @@ bitflags::bitflags! { } } -#[cfg(feature = "simd-is-enabled")] -pub(crate) struct WContact { - pub local_p1: Point<SimdReal>, - pub local_p2: Point<SimdReal>, - pub local_n1: Vector<SimdReal>, - pub local_n2: Vector<SimdReal>, - pub dist: SimdReal, - pub fid1: [u8; SIMD_WIDTH], - pub fid2: [u8; SIMD_WIDTH], -} - -#[cfg(feature = "simd-is-enabled")] -impl WContact { - pub fn extract(&self, i: usize) -> (Contact, Vector<f32>, Vector<f32>) { - let c = Contact { - local_p1: self.local_p1.extract(i), - local_p2: self.local_p2.extract(i), - dist: self.dist.extract(i), - fid1: self.fid1[i], - fid2: self.fid2[i], - data: ContactData::default(), - }; - - (c, self.local_n1.extract(i), self.local_n2.extract(i)) - } -} - #[derive(Copy, Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// A single contact between two collider. @@ -112,40 +79,13 @@ impl ContactPair { /// An active contact is a contact that may result in a non-zero contact force. pub fn has_any_active_contact(&self) -> bool { for manifold in &self.manifolds { - if manifold.num_active_contacts != 0 { + if manifold.data.num_active_contacts() != 0 { return true; } } false } - - pub(crate) fn single_manifold<'a, 'b>( - &'a mut self, - colliders: &'b ColliderSet, - flags: SolverFlags, - ) -> ( - &'b Collider, - &'b Collider, - &'a mut ContactManifold, - Option<&'a mut (dyn MaybeSerializableData)>, - ) { - let coll1 = &colliders[self.pair.collider1]; - let coll2 = &colliders[self.pair.collider2]; - - if self.manifolds.len() == 0 { - let manifold_data = ContactManifoldData::with_subshape_indices(coll1, coll2, flags); - self.manifolds - .push(ContactManifold::with_data((0, 0), manifold_data)); - } - - ( - coll1, - coll2, - &mut self.manifolds[0], - self.workspace.as_mut().map(|w| &mut *w.0), - ) - } } #[derive(Clone, Debug)] @@ -206,22 +146,9 @@ impl ContactManifoldData { } } - pub(crate) fn set_from_colliders( - &mut self, - coll1: &Collider, - coll2: &Collider, - flags: SolverFlags, - ) { - self.body_pair = BodyPair::new(coll1.parent, coll2.parent); - self.solver_flags = flags; - } - - pub(crate) fn with_subshape_indices( - coll1: &Collider, - coll2: &Collider, - solver_flags: SolverFlags, - ) -> Self { - Self::new(BodyPair::new(coll1.parent, coll2.parent), solver_flags) + #[inline] + pub fn num_active_contacts(&self) -> usize { + self.solver_contacts.len() } pub(crate) fn min_warmstart_multiplier() -> f32 { @@ -232,25 +159,25 @@ impl ContactManifoldData { 1.0 // 0.01 } - pub(crate) fn update_warmstart_multiplier(manifold: &mut ContactManifold) { - // In 2D, tall stacks will actually suffer from this - // because oscillation due to inaccuracies in 2D often - // cause contacts to break, which would result in - // a reset of the warmstart multiplier. - if cfg!(feature = "dim2") { - manifold.data.warmstart_multiplier = 1.0; - return; - } - - for pt in &manifold.points { - if pt.data.impulse != 0.0 { - manifold.data.warmstart_multiplier = - (manifold.data.warmstart_multiplier * 2.0).min(1.0); - return; - } - } - - // Reset the multiplier. - manifold.data.warmstart_multiplier = Self::min_warmstart_multiplier() - } + // pub(crate) fn update_warmstart_multiplier(manifold: &mut ContactManifold) { + // // In 2D, tall stacks will actually suffer from this + // // because oscillation due to inaccuracies in 2D often + // // cause contacts to break, which would result in + // // a reset of the warmstart multiplier. + // if cfg!(feature = "dim2") { + // manifold.data.warmstart_multiplier = 1.0; + // return; + // } + // + // for pt in &manifold.points { + // if pt.data.impulse != 0.0 { + // manifold.data.warmstart_multiplier = + // (manifold.data.warmstart_multiplier * 2.0).min(1.0); + // return; + // } + // } + // + // // Reset the multiplier. + // manifold.data.warmstart_multiplier = Self::min_warmstart_multiplier() + // } } diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 23e34cc..2b6d14f 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -81,8 +81,6 @@ impl IntersectionEvent { pub(crate) use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair}; pub(crate) use self::collider_set::RemovedCollider; -#[cfg(feature = "simd-is-enabled")] -pub(crate) use self::contact_pair::WContact; pub use self::interaction_groups::InteractionGroups; pub(crate) use self::narrow_phase::ContactManifoldIndex; pub(crate) use cdl::partitioning::SimdQuadTree; diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 7d89301..5d9cca5 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -3,7 +3,7 @@ use rayon::prelude::*; use crate::data::pubsub::Subscription; use crate::data::Coarena; -use crate::dynamics::RigidBodySet; +use crate::dynamics::{BodyPair, RigidBodySet}; use crate::geometry::{ BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactData, ContactEvent, ContactManifoldData, ContactPairFilter, IntersectionEvent, PairFilterContext, @@ -12,7 +12,7 @@ use crate::geometry::{ use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph}; use crate::math::Vector; use crate::pipeline::EventHandler; -use cdl::query::{DefaultQueryDispatcher, PersistentQueryDispatcher, QueryDispatcher}; +use cdl::query::{DefaultQueryDispatcher, PersistentQueryDispatcher}; use std::collections::HashMap; use std::sync::Arc; @@ -389,7 +389,6 @@ impl NarrowPhase { pub(crate) fn compute_intersections( &mut self, - prediction_distance: f32, bodies: &RigidBodySet, colliders: &ColliderSet, pair_filter: Option<&dyn ProximityPairFilter>, @@ -528,21 +527,40 @@ impl NarrowPhase { // TODO: don't write this everytime? for manifold in &mut pair.manifolds { manifold.data.solver_contacts.clear(); - manifold.data.set_from_colliders(co1, co2, solver_flags); + manifold.data.body_pair = BodyPair::new(co1.parent(), co2.parent()); + manifold.data.solver_flags = solver_flags; manifold.data.normal = co1.position() * manifold.local_n1; - for contact in &manifold.points[..manifold.num_active_contacts] { - let solver_contact = SolverContact { - point: co1.position() * contact.local_p1 - + manifold.data.normal * contact.dist / 2.0, - dist: contact.dist, - friction: (co1.friction + co2.friction) / 2.0, - restitution: (co1.restitution + co2.restitution) / 2.0, - surface_velocity: Vector::zeros(), - data: contact.data, - }; + // Sort contacts and generate solver contacts. + let mut first_inactive_index = manifold.points.len(); + + while manifold.data.num_active_contacts() != first_inactive_index { + let contact = &manifold.points[manifold.data.num_active_contacts()]; + if contact.dist < prediction_distance { + // Generate the solver contact. + let solver_contact = SolverContact { + point: co1.position() * contact.local_p1 + + manifold.data.normal * contact.dist / 2.0, + dist: contact.dist, + friction: (co1.friction + co2.friction) / 2.0, + restitution: (co1.restitution + co2.restitution) / 2.0, + surface_velocity: Vector::zeros(), + data: contact.data, + }; + + // TODO: apply the user-defined contact modification/removal, if needed. + + manifold.data.solver_contacts.push(solver_contact); + continue; + } - manifold.data.solver_contacts.push(solver_contact); + // If we reach this code, then the contact must be ignored by the constraints solver. + // Swap with the last contact. + manifold.points.swap( + manifold.data.num_active_contacts(), + first_inactive_index - 1, + ); + first_inactive_index -= 1; } } }); @@ -569,7 +587,7 @@ impl NarrowPhase { .data .solver_flags .contains(SolverFlags::COMPUTE_IMPULSES) - && manifold.num_active_contacts() != 0 + && manifold.data.num_active_contacts() != 0 && (rb1.is_dynamic() || rb2.is_dynamic()) && (!rb1.is_dynamic() || !rb1.is_sleeping()) && (!rb2.is_dynamic() || !rb2.is_sleeping()) diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index e80b9e8..06c581b 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -64,13 +64,7 @@ impl CollisionPipeline { contact_pair_filter, events, ); - narrow_phase.compute_intersections( - prediction_distance, - bodies, - colliders, - proximity_pair_filter, - events, - ); + narrow_phase.compute_intersections(bodies, colliders, proximity_pair_filter, events); bodies.update_active_set_with_contacts( colliders, diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index ccc60e0..e56f8e0 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -118,13 +118,7 @@ impl PhysicsPipeline { contact_pair_filter, events, ); - narrow_phase.compute_intersections( - integration_parameters.prediction_distance, - bodies, - colliders, - proximity_pair_filter, - events, - ); + narrow_phase.compute_intersections(bodies, colliders, proximity_pair_filter, events); // println!("Compute contact time: {}", instant::now() - t); self.counters.stages.island_construction_time.start(); diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index ea5fe89..9943099 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -2,8 +2,6 @@ use crate::dynamics::RigidBodySet; use crate::geometry::{ Collider, ColliderHandle, ColliderSet, InteractionGroups, Ray, RayIntersection, SimdQuadTree, }; -use cdl::query::TOI; -use cdl::shape::Shape; /// A pipeline for performing queries on all the colliders of a scene. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] diff --git a/src/utils.rs b/src/utils.rs index 1f06ee5..932c6e2 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -1,17 +1,12 @@ //! Miscellaneous utilities. use crate::dynamics::RigidBodyHandle; -use na::{Matrix2, Matrix3, Matrix3x2, Point2, Point3, Scalar, SimdRealField, Vector2, Vector3}; +use na::{Matrix3, Point2, Point3, Scalar, SimdRealField, Vector2, Vector3}; use num::Zero; use simba::simd::SimdValue; use cdl::utils::SdpMatrix3; -use std::ops::{Add, Mul}; -use { - crate::math::{AngularInertia, SimdBool, SimdReal}, - na::SimdPartialOrd, - num::One, -}; +use {crate::math::SimdReal, na::SimdPartialOrd, num::One}; pub(crate) fn inv(val: f32) -> f32 { if val == 0.0 { |