From ad5c10672e36f47fbdb0667bccd79c59ff3a97cc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 22 Feb 2021 17:51:40 +0100 Subject: [PATCH 01/10] Use contact ids instead of contact reordering in order to identify the impulse writeback location. --- src/dynamics/solver/velocity_constraint.rs | 19 +++++++++--------- .../solver/velocity_constraint_wide.rs | 16 ++++++++------- .../solver/velocity_ground_constraint.rs | 18 ++++++++--------- .../solver/velocity_ground_constraint_wide.rs | 16 ++++++++------- src/geometry/contact_pair.rs | 8 ++++++++ src/geometry/narrow_phase.rs | 20 +++++++------------ 6 files changed, 52 insertions(+), 45 deletions(-) diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 2cac93d..63139cc 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -124,7 +124,7 @@ pub(crate) struct VelocityConstraint { pub mj_lambda1: usize, pub mj_lambda2: usize, pub manifold_id: ContactManifoldIndex, - pub manifold_contact_id: usize, + pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS], pub num_contacts: u8, pub elements: [VelocityConstraintElement; MAX_MANIFOLD_POINTS], } @@ -168,7 +168,7 @@ impl VelocityConstraint { mj_lambda1, mj_lambda2, manifold_id, - manifold_contact_id: l * MAX_MANIFOLD_POINTS, + manifold_contact_id: [0; MAX_MANIFOLD_POINTS], num_contacts: manifold_points.len() as u8, }; @@ -211,7 +211,7 @@ impl VelocityConstraint { constraint.mj_lambda1 = mj_lambda1; constraint.mj_lambda2 = mj_lambda2; constraint.manifold_id = manifold_id; - constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS; + constraint.manifold_contact_id = [0; MAX_MANIFOLD_POINTS]; constraint.num_contacts = manifold_points.len() as u8; } @@ -224,6 +224,8 @@ impl VelocityConstraint { let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); constraint.limit = manifold_point.friction; + constraint.manifold_contact_id[k] = manifold_point.contact_id; + // Normal part. { let gcross1 = rb1 @@ -382,19 +384,18 @@ impl VelocityConstraint { pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { let manifold = &mut manifolds_all[self.manifold_id]; - let k_base = self.manifold_contact_id; for k in 0..self.num_contacts as usize { - let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()]; - active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse; + let contact_id = self.manifold_contact_id[k]; + let active_contact = &mut manifold.points[contact_id as usize]; + active_contact.data.impulse = self.elements[k].normal_part.impulse; #[cfg(feature = "dim2")] { - active_contacts[k_base + k].data.tangent_impulse = - self.elements[k].tangent_part[0].impulse; + active_contacts.data.tangent_impulse = self.elements[k].tangent_part[0].impulse; } #[cfg(feature = "dim3")] { - active_contacts[k_base + k].data.tangent_impulse = [ + active_contact.data.tangent_impulse = [ self.elements[k].tangent_part[0].impulse, self.elements[k].tangent_part[1].impulse, ]; diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 3e068e7..8838011 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -55,7 +55,7 @@ pub(crate) struct WVelocityConstraint { pub mj_lambda1: [usize; SIMD_WIDTH], pub mj_lambda2: [usize; SIMD_WIDTH], pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], - pub manifold_contact_id: usize, + pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS], } impl WVelocityConstraint { @@ -116,7 +116,7 @@ impl WVelocityConstraint { mj_lambda1, mj_lambda2, manifold_id, - manifold_contact_id: l, + manifold_contact_id: [[0; SIMD_WIDTH]; MAX_MANIFOLD_POINTS], num_contacts: num_points as u8, }; @@ -141,6 +141,8 @@ impl WVelocityConstraint { let vel2 = linvel2 + angvel2.gcross(dp2); constraint.limit = friction; + constraint.manifold_contact_id[k] = + array![|ii| manifold_points[ii][k].contact_id; SIMD_WIDTH]; // Normal part. { @@ -332,17 +334,17 @@ 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 = &mut manifold.points[..manifold.data.num_active_contacts()]; - active_contacts[k_base + k].data.impulse = impulses[ii]; + let contact_id = self.manifold_contact_id[k][ii]; + let active_contact = &mut manifold.points[contact_id as usize]; + active_contact.data.impulse = impulses[ii]; #[cfg(feature = "dim2")] { - active_contacts[k_base + k].data.tangent_impulse = tangent_impulses[ii]; + active_contact.data.tangent_impulse = tangent_impulses[ii]; } #[cfg(feature = "dim3")] { - active_contacts[k_base + k].data.tangent_impulse = + active_contact.data.tangent_impulse = [tangent_impulses[ii], bitangent_impulses[ii]]; } } diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 88f864a..6f61b17 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -49,7 +49,7 @@ pub(crate) struct VelocityGroundConstraint { pub limit: Real, pub mj_lambda2: usize, pub manifold_id: ContactManifoldIndex, - pub manifold_contact_id: usize, + pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS], pub num_contacts: u8, pub elements: [VelocityGroundConstraintElement; MAX_MANIFOLD_POINTS], } @@ -92,7 +92,7 @@ impl VelocityGroundConstraint { limit: 0.0, mj_lambda2, manifold_id, - manifold_contact_id: l * MAX_MANIFOLD_POINTS, + manifold_contact_id: [0; MAX_MANIFOLD_POINTS], num_contacts: manifold_points.len() as u8, }; @@ -133,7 +133,7 @@ impl VelocityGroundConstraint { constraint.limit = 0.0; constraint.mj_lambda2 = mj_lambda2; constraint.manifold_id = manifold_id; - constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS; + constraint.manifold_contact_id = [0; MAX_MANIFOLD_POINTS]; constraint.num_contacts = manifold_points.len() as u8; } @@ -145,6 +145,7 @@ impl VelocityGroundConstraint { let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); constraint.limit = manifold_point.friction; + constraint.manifold_contact_id[k] = manifold_point.contact_id; // Normal part. { @@ -267,19 +268,18 @@ impl VelocityGroundConstraint { // FIXME: duplicated code. This is exactly the same as in the non-ground velocity constraint. pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { let manifold = &mut manifolds_all[self.manifold_id]; - let k_base = self.manifold_contact_id; for k in 0..self.num_contacts as usize { - let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()]; - active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse; + let contact_id = self.manifold_contact_id[k]; + let active_contact = &mut manifold.points[contact_id as usize]; + active_contact.data.impulse = self.elements[k].normal_part.impulse; #[cfg(feature = "dim2")] { - active_contacts[k_base + k].data.tangent_impulse = - self.elements[k].tangent_part[0].impulse; + active_contact.data.tangent_impulse = self.elements[k].tangent_part[0].impulse; } #[cfg(feature = "dim3")] { - active_contacts[k_base + k].data.tangent_impulse = [ + active_contact.data.tangent_impulse = [ self.elements[k].tangent_part[0].impulse, self.elements[k].tangent_part[1].impulse, ]; diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index 29ba5e9..6f279d8 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -51,7 +51,7 @@ pub(crate) struct WVelocityGroundConstraint { pub limit: SimdReal, pub mj_lambda2: [usize; SIMD_WIDTH], pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], - pub manifold_contact_id: usize, + pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS], } impl WVelocityGroundConstraint { @@ -111,7 +111,7 @@ impl WVelocityGroundConstraint { limit: SimdReal::splat(0.0), mj_lambda2, manifold_id, - manifold_contact_id: l, + manifold_contact_id: [[0; SIMD_WIDTH]; MAX_MANIFOLD_POINTS], num_contacts: num_points as u8, }; @@ -135,6 +135,8 @@ impl WVelocityGroundConstraint { let vel2 = linvel2 + angvel2.gcross(dp2); constraint.limit = friction; + constraint.manifold_contact_id[k] = + array![|ii| manifold_points[ii][k].contact_id; SIMD_WIDTH]; // Normal part. { @@ -281,17 +283,17 @@ 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 = &mut manifold.points[..manifold.data.num_active_contacts()]; - active_contacts[k_base + k].data.impulse = impulses[ii]; + let contact_id = self.manifold_contact_id[k][ii]; + let active_contact = &mut manifold.points[contact_id as usize]; + active_contact.data.impulse = impulses[ii]; #[cfg(feature = "dim2")] { - active_contacts[k_base + k].data.tangent_impulse = tangent_impulses[ii]; + active_contact.data.tangent_impulse = tangent_impulses[ii]; } #[cfg(feature = "dim3")] { - active_contacts[k_base + k].data.tangent_impulse = + active_contact.data.tangent_impulse = [tangent_impulses[ii], bitangent_impulses[ii]]; } } diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index 462d3ef..c94a2cf 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -110,6 +110,8 @@ pub struct ContactManifoldData { #[derive(Copy, Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct SolverContact { + /// The index of the manifold contact used to generate this solver contact. + pub contact_id: u8, /// The world-space contact point. pub point: Point, /// The distance between the two original contacts points along the contact normal. @@ -203,3 +205,9 @@ impl ContactManifoldData { // manifold.data.warmstart_multiplier = Self::min_warmstart_multiplier() // } } + +/// A contact manifold that can be modified by the user. +pub struct ModifiableContactManifold<'a> { + manifold: &'a super::ContactManifold, + solver_contacts: &'a mut Vec, +} diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 640ce12..9513fef 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -546,15 +546,17 @@ impl NarrowPhase { manifold.data.solver_flags = solver_flags; manifold.data.normal = world_pos1 * manifold.local_n1; - // Sort contacts to keep only these with distances bellow - // the prediction, and generate solver contacts. - let mut first_inactive_index = manifold.points.len(); + // Generate solver contacts. + for (contact_id, contact) in manifold.points.iter().enumerate() { + assert!( + contact_id <= u8::MAX as usize, + "A contact manifold cannot contain more than 255 contacts currently." + ); - 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 { + contact_id: contact_id as u8, point: world_pos1 * contact.local_p1 + manifold.data.normal * contact.dist / 2.0, dist: contact.dist, @@ -570,14 +572,6 @@ impl NarrowPhase { has_any_active_contact = true; continue; } - - // 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; } } From 00706e8b360e132cb88a7b393dcedadf35403379 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 23 Feb 2021 11:24:54 +0100 Subject: [PATCH 02/10] Introduce the PhysicsHook trait used for both contact filtering and contact modification. --- examples3d/fountain3.rs | 6 +- src/geometry/contact_pair.rs | 14 ++-- src/geometry/mod.rs | 2 +- src/geometry/narrow_phase.rs | 63 ++++++++++++++---- src/geometry/pair_filter.rs | 103 +++++++++++++++++++++++++---- src/pipeline/collision_pipeline.rs | 16 ++--- src/pipeline/physics_pipeline.rs | 11 ++- src_testbed/harness/mod.rs | 3 +- 8 files changed, 160 insertions(+), 58 deletions(-) diff --git a/examples3d/fountain3.rs b/examples3d/fountain3.rs index 5acc2e8..caaa21b 100644 --- a/examples3d/fountain3.rs +++ b/examples3d/fountain3.rs @@ -23,16 +23,14 @@ pub fn init_world(testbed: &mut Testbed) { let handle = bodies.insert(rigid_body); let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); colliders.insert(collider, handle, &mut bodies); - let mut k = 0; // Callback that will be executed on the main loop to handle proximities. - testbed.add_callback(move |mut window, mut graphics, physics, _, _| { - k += 1; + testbed.add_callback(move |mut window, mut graphics, physics, _, run_state| { let rigid_body = RigidBodyBuilder::new_dynamic() .translation(0.0, 10.0, 0.0) .build(); let handle = physics.bodies.insert(rigid_body); - let collider = match k % 3 { + let collider = match run_state.timestep_id % 3 { 0 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(), 1 => ColliderBuilder::cone(rad, rad).build(), _ => ColliderBuilder::cuboid(rad, rad, rad).build(), diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index c94a2cf..11e0188 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -9,7 +9,10 @@ bitflags::bitflags! { pub struct SolverFlags: u32 { /// The constraint solver will take this contact manifold into /// account for force computation. - const COMPUTE_IMPULSES = 0b01; + const COMPUTE_IMPULSES = 0b001; + /// The user-defined physics hooks will be used to + /// modify the solver contacts of this contact manifold. + const MODIFY_SOLVER_CONTACTS = 0b010; } } @@ -104,6 +107,8 @@ pub struct ContactManifoldData { /// The contacts that will be seen by the constraints solver for computing forces. #[cfg_attr(feature = "serde-serialize", serde(skip))] pub solver_contacts: Vec, + /// A user-defined piece of data. + pub user_data: u32, } /// A contact seen by the constraints solver for computing forces. @@ -165,6 +170,7 @@ impl ContactManifoldData { solver_flags, normal: Vector::zeros(), solver_contacts: Vec::new(), + user_data: 0, } } @@ -205,9 +211,3 @@ impl ContactManifoldData { // manifold.data.warmstart_multiplier = Self::min_warmstart_multiplier() // } } - -/// A contact manifold that can be modified by the user. -pub struct ModifiableContactManifold<'a> { - manifold: &'a super::ContactManifold, - solver_contacts: &'a mut Vec, -} diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 861763e..2997e24 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -10,7 +10,7 @@ pub use self::interaction_graph::{ }; pub use self::interaction_groups::InteractionGroups; pub use self::narrow_phase::NarrowPhase; -pub use self::pair_filter::{ContactPairFilter, IntersectionPairFilter, PairFilterContext}; +pub use self::pair_filter::{PairFilterContext, PhysicsHooks}; pub use parry::query::TrackedContact; diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 9513fef..e929e0f 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -4,10 +4,11 @@ use rayon::prelude::*; use crate::data::pubsub::Subscription; use crate::data::Coarena; use crate::dynamics::{BodyPair, CoefficientCombineRule, RigidBodySet}; +use crate::geometry::pair_filter::{ContactModificationContext, PhysicsHooksFlags}; use crate::geometry::{ BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactData, ContactEvent, - ContactManifoldData, ContactPairFilter, IntersectionEvent, IntersectionPairFilter, - PairFilterContext, RemovedCollider, SolverContact, SolverFlags, + ContactManifoldData, IntersectionEvent, PairFilterContext, PhysicsHooks, RemovedCollider, + SolverContact, SolverFlags, }; use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph}; use crate::math::{Real, Vector}; @@ -387,11 +388,13 @@ impl NarrowPhase { &mut self, bodies: &RigidBodySet, colliders: &ColliderSet, - pair_filter: Option<&dyn IntersectionPairFilter>, + hooks: &dyn PhysicsHooks, events: &dyn EventHandler, ) { let nodes = &self.intersection_graph.graph.nodes; let query_dispatcher = &*self.query_dispatcher; + let active_hooks = hooks.active_hooks(); + par_iter_mut!(&mut self.intersection_graph.graph.edges).for_each(|edge| { let handle1 = nodes[edge.source().index()].weight; let handle2 = nodes[edge.target().index()].weight; @@ -415,12 +418,15 @@ impl NarrowPhase { return; } - if pair_filter.is_none() && !rb1.is_dynamic() && !rb2.is_dynamic() { + if !active_hooks.contains(PhysicsHooksFlags::FILTER_INTERSECTION_PAIR) + && !rb1.is_dynamic() + && !rb2.is_dynamic() + { // Default filtering rule: no intersection between two non-dynamic bodies. return; } - if let Some(filter) = pair_filter { + if active_hooks.contains(PhysicsHooksFlags::FILTER_INTERSECTION_PAIR) { let context = PairFilterContext { rigid_body1: rb1, rigid_body2: rb2, @@ -430,7 +436,7 @@ impl NarrowPhase { collider2: co2, }; - if !filter.filter_intersection_pair(&context) { + if !hooks.filter_intersection_pair(&context) { // No intersection allowed. return; } @@ -458,10 +464,11 @@ impl NarrowPhase { prediction_distance: Real, bodies: &RigidBodySet, colliders: &ColliderSet, - pair_filter: Option<&dyn ContactPairFilter>, + hooks: &dyn PhysicsHooks, events: &dyn EventHandler, ) { let query_dispatcher = &*self.query_dispatcher; + let active_hooks = hooks.active_hooks(); par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| { let pair = &mut edge.weight; @@ -485,12 +492,16 @@ impl NarrowPhase { return; } - if pair_filter.is_none() && !rb1.is_dynamic() && !rb2.is_dynamic() { + if !active_hooks.contains(PhysicsHooksFlags::FILTER_CONTACT_PAIR) + && !rb1.is_dynamic() + && !rb2.is_dynamic() + { // Default filtering rule: no contact between two non-dynamic bodies. return; } - let mut solver_flags = if let Some(filter) = pair_filter { + let mut solver_flags = if active_hooks.contains(PhysicsHooksFlags::FILTER_CONTACT_PAIR) + { let context = PairFilterContext { rigid_body1: rb1, rigid_body2: rb2, @@ -500,7 +511,7 @@ impl NarrowPhase { collider2: co2, }; - if let Some(solver_flags) = filter.filter_contact_pair(&context) { + if let Some(solver_flags) = hooks.filter_contact_pair(&context) { solver_flags } else { // No contact allowed. @@ -566,13 +577,39 @@ impl NarrowPhase { data: contact.data, }; - // TODO: apply the user-defined contact modification/removal, if needed. - manifold.data.solver_contacts.push(solver_contact); has_any_active_contact = true; - continue; } } + + // Apply the user-defined contact modification. + if active_hooks.contains(PhysicsHooksFlags::MODIFY_SOLVER_CONTACTS) + && manifold + .data + .solver_flags + .contains(SolverFlags::MODIFY_SOLVER_CONTACTS) + { + let mut modifiable_solver_contacts = + std::mem::replace(&mut manifold.data.solver_contacts, Vec::new()); + let mut modifiable_user_data = manifold.data.user_data; + + let mut context = ContactModificationContext { + rigid_body1: rb1, + rigid_body2: rb2, + collider_handle1: pair.pair.collider1, + collider_handle2: pair.pair.collider2, + collider1: co1, + collider2: co2, + manifold, + solver_contacts: &mut modifiable_solver_contacts, + user_data: &mut modifiable_user_data, + }; + + hooks.modify_solver_contacts(&mut context); + + manifold.data.solver_contacts = modifiable_solver_contacts; + manifold.data.user_data = modifiable_user_data; + } } if has_any_active_contact != pair.has_any_active_contact { diff --git a/src/geometry/pair_filter.rs b/src/geometry/pair_filter.rs index 25f300f..a30145e 100644 --- a/src/geometry/pair_filter.rs +++ b/src/geometry/pair_filter.rs @@ -1,5 +1,5 @@ use crate::dynamics::RigidBody; -use crate::geometry::{Collider, ColliderHandle, SolverFlags}; +use crate::geometry::{Collider, ColliderHandle, ContactManifold, SolverContact, SolverFlags}; /// Context given to custom collision filters to filter-out collisions. pub struct PairFilterContext<'a> { @@ -17,14 +17,54 @@ pub struct PairFilterContext<'a> { pub collider2: &'a Collider, } -/// User-defined filter for potential contact pairs detected by the broad-phase. -/// -/// This can be used to apply custom logic in order to decide whether two colliders -/// should have their contact computed by the narrow-phase, and if these contact -/// should be solved by the constraints solver -pub trait ContactPairFilter: Send + Sync { +pub struct ContactModificationContext<'a> { + /// The first collider involved in the potential collision. + pub rigid_body1: &'a RigidBody, + /// The first collider involved in the potential collision. + pub rigid_body2: &'a RigidBody, + /// The first collider involved in the potential collision. + pub collider_handle1: ColliderHandle, + /// The first collider involved in the potential collision. + pub collider_handle2: ColliderHandle, + /// The first collider involved in the potential collision. + pub collider1: &'a Collider, + /// The first collider involved in the potential collision. + pub collider2: &'a Collider, + /// The contact manifold. + pub manifold: &'a ContactManifold, + /// The solver contacts that can be modified. + pub solver_contacts: &'a mut Vec, + /// User-defined data attached to the manifold. + // NOTE: we keep this a &'a mut u32 to emphasize the + // fact that this can be modified. + pub user_data: &'a mut u32, +} + +bitflags::bitflags! { + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] + /// Flags affecting the behavior of the constraints solver for a given contact manifold. + pub struct PhysicsHooksFlags: u32 { + /// If set, Rapier will call `PhysicsHooks::filter_contact_pair` whenever relevant. + const FILTER_CONTACT_PAIR = 0b0001; + /// If set, Rapier will call `PhysicsHooks::filter_intersection_pair` whenever relevant. + const FILTER_INTERSECTION_PAIR = 0b0010; + /// If set, Rapier will call `PhysicsHooks::modify_solver_contact` whenever relevant. + const MODIFY_SOLVER_CONTACTS = 0b0100; + } +} + +/// User-defined functions called by the physics engines during one timestep in order to customize its behavior. +pub trait PhysicsHooks: Send + Sync { + /// The sets of hooks that must be taken into account. + fn active_hooks(&self) -> PhysicsHooksFlags; + /// Applies the contact pair filter. /// + /// User-defined filter for potential contact pairs detected by the broad-phase. + /// This can be used to apply custom logic in order to decide whether two colliders + /// should have their contact computed by the narrow-phase, and if these contact + /// should be solved by the constraints solver + /// /// Note that using a contact pair filter will replace the default contact filtering /// which consists of preventing contact computation between two non-dynamic bodies. /// @@ -39,15 +79,14 @@ pub trait ContactPairFilter: Send + Sync { /// `Some(SolverFlags::empty())` then the constraints solver will ignore these /// contacts. fn filter_contact_pair(&self, context: &PairFilterContext) -> Option; -} -/// User-defined filter for potential intersection pairs detected by the broad-phase. -/// -/// This can be used to apply custom logic in order to decide whether two colliders -/// should have their intersection computed by the narrow-phase. -pub trait IntersectionPairFilter: Send + Sync { /// Applies the intersection pair filter. /// + /// User-defined filter for potential intersection pairs detected by the broad-phase. + /// + /// This can be used to apply custom logic in order to decide whether two colliders + /// should have their intersection computed by the narrow-phase. + /// /// Note that using an intersection pair filter will replace the default intersection filtering /// which consists of preventing intersection computation between two non-dynamic bodies. /// @@ -58,4 +97,42 @@ pub trait IntersectionPairFilter: Send + Sync { /// If this return `true` then the narrow-phase will compute intersection /// information for this pair. fn filter_intersection_pair(&self, context: &PairFilterContext) -> bool; + + /// Modifies the set of contacts seen by the constraints solver. + /// + /// By default, the content of `solver_contacts` is computed from `manifold.points`. + /// This method will be called on each contact manifold which have the flag `SolverFlags::MODIFY_CONTACTS` set. + /// This method can be used to modify the set of solver contacts seen by the constraints solver: contacts + /// can be removed and modified. + /// + /// Note that if all the contacts have to be ignored by the constraint solver, you may simply + /// do `context.solver_contacts.clear()`. + /// + /// Modifying the solver contacts allow you to achieve various effects, including: + /// - Simulating conveyor belts by setting the `surface_velocity` of a solver contact. + /// - Simulating shapes with multiply materials by modifying the friction and restitution + /// coefficient depending of the features in contacts. + /// - Simulating one-way platforms depending on the contact normal. + /// + /// Each contact manifold is given a `u32` user-defined data that is persistent between + /// timesteps (as long as the contact manifold exists). This user-defined data is initialized + /// as 0 and can be modified in `context.user_data`. + fn modify_solver_contacts(&self, context: &mut ContactModificationContext); +} + +impl PhysicsHooks for () { + /// The sets of hooks that must be taken into account. + fn active_hooks(&self) -> PhysicsHooksFlags { + PhysicsHooksFlags::empty() + } + + fn filter_contact_pair(&self, _: &PairFilterContext) -> Option { + None + } + + fn filter_intersection_pair(&self, _: &PairFilterContext) -> bool { + false + } + + fn modify_solver_contacts(&self, _: &mut ContactModificationContext) {} } diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index 866c3a5..6f02d98 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -2,8 +2,7 @@ use crate::dynamics::{JointSet, RigidBodySet}; use crate::geometry::{ - BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactPairFilter, - IntersectionPairFilter, NarrowPhase, + BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, NarrowPhase, PhysicsHooks, }; use crate::math::Real; use crate::pipeline::EventHandler; @@ -44,8 +43,7 @@ impl CollisionPipeline { narrow_phase: &mut NarrowPhase, bodies: &mut RigidBodySet, colliders: &mut ColliderSet, - contact_pair_filter: Option<&dyn ContactPairFilter>, - proximity_pair_filter: Option<&dyn IntersectionPairFilter>, + hooks: &dyn PhysicsHooks, events: &dyn EventHandler, ) { bodies.maintain(colliders); @@ -58,14 +56,8 @@ impl CollisionPipeline { narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events); - narrow_phase.compute_contacts( - prediction_distance, - bodies, - colliders, - contact_pair_filter, - events, - ); - narrow_phase.compute_intersections(bodies, colliders, proximity_pair_filter, events); + narrow_phase.compute_contacts(prediction_distance, bodies, colliders, hooks, events); + narrow_phase.compute_intersections(bodies, colliders, hooks, events); bodies.update_active_set_with_contacts( colliders, diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 293fa9d..c030b1d 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -7,8 +7,8 @@ use crate::dynamics::{IntegrationParameters, JointSet, RigidBodySet}; #[cfg(feature = "parallel")] use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver}; use crate::geometry::{ - BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, - ContactPairFilter, IntersectionPairFilter, NarrowPhase, + BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, NarrowPhase, + PhysicsHooks, }; use crate::math::{Real, Vector}; use crate::pipeline::EventHandler; @@ -69,8 +69,7 @@ impl PhysicsPipeline { bodies: &mut RigidBodySet, colliders: &mut ColliderSet, joints: &mut JointSet, - contact_pair_filter: Option<&dyn ContactPairFilter>, - proximity_pair_filter: Option<&dyn IntersectionPairFilter>, + hooks: &dyn PhysicsHooks, events: &dyn EventHandler, ) { self.counters.step_started(); @@ -115,10 +114,10 @@ impl PhysicsPipeline { integration_parameters.prediction_distance, bodies, colliders, - contact_pair_filter, + hooks, events, ); - narrow_phase.compute_intersections(bodies, colliders, proximity_pair_filter, events); + narrow_phase.compute_intersections(bodies, colliders, hooks, events); // println!("Compute contact time: {}", instant::now() - t); self.counters.stages.island_construction_time.start(); diff --git a/src_testbed/harness/mod.rs b/src_testbed/harness/mod.rs index afdc23a..53aa893 100644 --- a/src_testbed/harness/mod.rs +++ b/src_testbed/harness/mod.rs @@ -192,8 +192,7 @@ impl Harness { &mut self.physics.bodies, &mut self.physics.colliders, &mut self.physics.joints, - None, - None, + &(), &self.event_handler, ); From 4ca32a9546beca788104041134f0afbe96c5e871 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 23 Feb 2021 15:43:43 +0100 Subject: [PATCH 03/10] Add one-way platform + conveyor belt demos. --- examples2d/one_way_platforms2.rs | 143 ++++++++++++++++++ examples3d/one_way_platforms3.rs | 143 ++++++++++++++++++ .../physics_hooks.rs} | 10 +- 3 files changed, 295 insertions(+), 1 deletion(-) create mode 100644 examples2d/one_way_platforms2.rs create mode 100644 examples3d/one_way_platforms3.rs rename src/{geometry/pair_filter.rs => pipeline/physics_hooks.rs} (93%) diff --git a/examples2d/one_way_platforms2.rs b/examples2d/one_way_platforms2.rs new file mode 100644 index 0000000..c7f58e8 --- /dev/null +++ b/examples2d/one_way_platforms2.rs @@ -0,0 +1,143 @@ +use na::{Point2, Vector2}; +use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier2d::geometry::{ColliderBuilder, ColliderHandle, ColliderSet}; +use rapier2d::pipeline::{ContactModificationContext, PhysicsHooks, PhysicsHooksFlags}; +use rapier_testbed2d::Testbed; + +struct OneWayPlatformHook { + platform1: ColliderHandle, + platform2: ColliderHandle, +} + +impl PhysicsHooks for OneWayPlatformHook { + fn active_hooks(&self) -> PhysicsHooksFlags { + PhysicsHooksFlags::MODIFY_SOLVER_CONTACTS + } + + fn modify_solver_contacts(&self, context: &mut ContactModificationContext) { + // The allowed normal for the first platform is its local +y axis, and the + // allowed normal for the second platform is its local -y axis. + // + // Now we have to be careful because the `manifold.local_n1` normal points + // toward the outside of the shape of `context.co1`. So we need to flip the + // allowed normal direction if the platform is in `context.collider_handle2`. + // + // Therefore: + // - If context.collider_handle1 == self.platform1 then the allowed normal is +y. + // - If context.collider_handle2 == self.platform1 then the allowed normal is -y. + // - If context.collider_handle1 == self.platform2 then its allowed normal +y needs to be flipped to -y. + // - If context.collider_handle2 == self.platform2 then the allowed normal -y needs to be flipped to +y. + let mut allowed_local_n1 = Vector2::zeros(); + + if context.collider_handle1 == self.platform1 { + allowed_local_n1 = Vector2::y(); + } else if context.collider_handle2 == self.platform1 { + // Flip the allowed direction. + allowed_local_n1 = -Vector2::y(); + } + + if context.collider_handle1 == self.platform2 { + allowed_local_n1 = -Vector2::y(); + } else if context.collider_handle2 == self.platform2 { + // Flip the allowed direction. + allowed_local_n1 = -Vector2::y(); + } + + // Call the helper function that simulates one-way platforms. + context.update_as_oneway_platform(&allowed_local_n1, 0.1); + + // Set the surface velocity of the accepted contacts. + let surface_velocity = if context.collider_handle1 == self.platform1 + || context.collider_handle2 == self.platform2 + { + -12.0 + } else { + 12.0 + }; + + for contact in context.solver_contacts.iter_mut() { + contact.surface_velocity.x = surface_velocity; + } + } +} + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground + */ + let rigid_body = RigidBodyBuilder::new_static().build(); + let handle = bodies.insert(rigid_body); + + let collider = ColliderBuilder::cuboid(25.0, 0.5) + .translation(30.0, 2.0) + .modify_contacts(true) + .build(); + let platform1 = colliders.insert(collider, handle, &mut bodies); + let collider = ColliderBuilder::cuboid(25.0, 0.5) + .translation(-30.0, -2.0) + .modify_contacts(true) + .build(); + let platform2 = colliders.insert(collider, handle, &mut bodies); + + /* + * Setup the one-way platform hook. + */ + let physics_hooks = OneWayPlatformHook { + platform1, + platform2, + }; + + /* + * Spawn cubes at regular intervals and apply a custom gravity + * depending on their position. + */ + testbed.add_callback(move |mut window, mut graphics, physics, _, run_state| { + if run_state.timestep_id % 50 == 0 && physics.bodies.len() <= 7 { + // Spawn a new cube. + let collider = ColliderBuilder::cuboid(1.5, 2.0).build(); + let body = RigidBodyBuilder::new_dynamic() + .translation(20.0, 10.0) + .build(); + let handle = physics.bodies.insert(body); + physics + .colliders + .insert(collider, handle, &mut physics.bodies); + + if let (Some(graphics), Some(window)) = (&mut graphics, &mut window) { + graphics.add(window, handle, &physics.bodies, &physics.colliders); + } + } + + physics.bodies.foreach_active_dynamic_body_mut(|_, body| { + if body.position().translation.y > 1.0 { + body.set_gravity_scale(1.0, false); + } else if body.position().translation.y < -1.0 { + body.set_gravity_scale(-1.0, false); + } + }); + }); + + /* + * Set up the testbed. + */ + testbed.set_world_with_params( + bodies, + colliders, + joints, + Vector2::new(0.0, -9.81), + physics_hooks, + ); + testbed.look_at(Point2::new(0.0, 0.0), 20.0); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]); + testbed.run() +} diff --git a/examples3d/one_way_platforms3.rs b/examples3d/one_way_platforms3.rs new file mode 100644 index 0000000..b3182a4 --- /dev/null +++ b/examples3d/one_way_platforms3.rs @@ -0,0 +1,143 @@ +use na::{Point3, Vector3}; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderHandle, ColliderSet}; +use rapier3d::pipeline::{ContactModificationContext, PhysicsHooks, PhysicsHooksFlags}; +use rapier_testbed3d::Testbed; + +struct OneWayPlatformHook { + platform1: ColliderHandle, + platform2: ColliderHandle, +} + +impl PhysicsHooks for OneWayPlatformHook { + fn active_hooks(&self) -> PhysicsHooksFlags { + PhysicsHooksFlags::MODIFY_SOLVER_CONTACTS + } + + fn modify_solver_contacts(&self, context: &mut ContactModificationContext) { + // The allowed normal for the first platform is its local +y axis, and the + // allowed normal for the second platform is its local -y axis. + // + // Now we have to be careful because the `manifold.local_n1` normal points + // toward the outside of the shape of `context.co1`. So we need to flip the + // allowed normal direction if the platform is in `context.collider_handle2`. + // + // Therefore: + // - If context.collider_handle1 == self.platform1 then the allowed normal is +y. + // - If context.collider_handle2 == self.platform1 then the allowed normal is -y. + // - If context.collider_handle1 == self.platform2 then its allowed normal +y needs to be flipped to -y. + // - If context.collider_handle2 == self.platform2 then the allowed normal -y needs to be flipped to +y. + let mut allowed_local_n1 = Vector3::zeros(); + + if context.collider_handle1 == self.platform1 { + allowed_local_n1 = Vector3::y(); + } else if context.collider_handle2 == self.platform1 { + // Flip the allowed direction. + allowed_local_n1 = -Vector3::y(); + } + + if context.collider_handle1 == self.platform2 { + allowed_local_n1 = -Vector3::y(); + } else if context.collider_handle2 == self.platform2 { + // Flip the allowed direction. + allowed_local_n1 = -Vector3::y(); + } + + // Call the helper function that simulates one-way platforms. + context.update_as_oneway_platform(&allowed_local_n1, 0.1); + + // Set the surface velocity of the accepted contacts. + let surface_velocity = if context.collider_handle1 == self.platform1 + || context.collider_handle2 == self.platform2 + { + -12.0 + } else { + 12.0 + }; + + for contact in context.solver_contacts.iter_mut() { + contact.surface_velocity.z = surface_velocity; + } + } +} + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground + */ + let rigid_body = RigidBodyBuilder::new_static().build(); + let handle = bodies.insert(rigid_body); + + let collider = ColliderBuilder::cuboid(9.0, 0.5, 25.0) + .translation(0.0, 2.0, 30.0) + .modify_contacts(true) + .build(); + let platform1 = colliders.insert(collider, handle, &mut bodies); + let collider = ColliderBuilder::cuboid(9.0, 0.5, 25.0) + .translation(0.0, -2.0, -30.0) + .modify_contacts(true) + .build(); + let platform2 = colliders.insert(collider, handle, &mut bodies); + + /* + * Setup the one-way platform hook. + */ + let physics_hooks = OneWayPlatformHook { + platform1, + platform2, + }; + + /* + * Spawn cubes at regular intervals and apply a custom gravity + * depending on their position. + */ + testbed.add_callback(move |mut window, mut graphics, physics, _, run_state| { + if run_state.timestep_id % 50 == 0 && physics.bodies.len() <= 7 { + // Spawn a new cube. + let collider = ColliderBuilder::cuboid(1.0, 2.0, 1.5).build(); + let body = RigidBodyBuilder::new_dynamic() + .translation(0.0, 6.0, 20.0) + .build(); + let handle = physics.bodies.insert(body); + physics + .colliders + .insert(collider, handle, &mut physics.bodies); + + if let (Some(graphics), Some(window)) = (&mut graphics, &mut window) { + graphics.add(window, handle, &physics.bodies, &physics.colliders); + } + } + + physics.bodies.foreach_active_dynamic_body_mut(|_, body| { + if body.position().translation.y > 1.0 { + body.set_gravity_scale(1.0, false); + } else if body.position().translation.y < -1.0 { + body.set_gravity_scale(-1.0, false); + } + }); + }); + + /* + * Set up the testbed. + */ + testbed.set_world_with_params( + bodies, + colliders, + joints, + Vector3::new(0.0, -9.81, 0.0), + physics_hooks, + ); + testbed.look_at(Point3::new(-100.0, 0.0, 0.0), Point3::origin()); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]); + testbed.run() +} diff --git a/src/geometry/pair_filter.rs b/src/pipeline/physics_hooks.rs similarity index 93% rename from src/geometry/pair_filter.rs rename to src/pipeline/physics_hooks.rs index a30145e..12b658f 100644 --- a/src/geometry/pair_filter.rs +++ b/src/pipeline/physics_hooks.rs @@ -60,6 +60,9 @@ pub trait PhysicsHooks: Send + Sync { /// Applies the contact pair filter. /// + /// Note that this method will only be called if `self.active_hooks()` + /// contains the `PhysicsHooksFlags::FILTER_CONTACT_PAIR` flags. + /// /// User-defined filter for potential contact pairs detected by the broad-phase. /// This can be used to apply custom logic in order to decide whether two colliders /// should have their contact computed by the narrow-phase, and if these contact @@ -82,6 +85,9 @@ pub trait PhysicsHooks: Send + Sync { /// Applies the intersection pair filter. /// + /// Note that this method will only be called if `self.active_hooks()` + /// contains the `PhysicsHooksFlags::FILTER_INTERSECTION_PAIR` flags. + /// /// User-defined filter for potential intersection pairs detected by the broad-phase. /// /// This can be used to apply custom logic in order to decide whether two colliders @@ -100,6 +106,9 @@ pub trait PhysicsHooks: Send + Sync { /// Modifies the set of contacts seen by the constraints solver. /// + /// Note that this method will only be called if `self.active_hooks()` + /// contains the `PhysicsHooksFlags::MODIFY_SOLVER_CONTACTS` flags. + /// /// By default, the content of `solver_contacts` is computed from `manifold.points`. /// This method will be called on each contact manifold which have the flag `SolverFlags::MODIFY_CONTACTS` set. /// This method can be used to modify the set of solver contacts seen by the constraints solver: contacts @@ -121,7 +130,6 @@ pub trait PhysicsHooks: Send + Sync { } impl PhysicsHooks for () { - /// The sets of hooks that must be taken into account. fn active_hooks(&self) -> PhysicsHooksFlags { PhysicsHooksFlags::empty() } From 3fdb4cd6d3468733056e886c991bae551a83240d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 23 Feb 2021 15:47:24 +0100 Subject: [PATCH 04/10] Properly take the tangent_velocity into account in the velocity solver. --- src/dynamics/solver/velocity_constraint.rs | 5 +++-- src/dynamics/solver/velocity_constraint_wide.rs | 4 +++- src/dynamics/solver/velocity_ground_constraint.rs | 12 +++++++----- .../solver/velocity_ground_constraint_wide.rs | 15 +++++++++------ 4 files changed, 22 insertions(+), 14 deletions(-) diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 63139cc..3a751df 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -273,7 +273,8 @@ impl VelocityConstraint { + rb2.effective_inv_mass + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); - let rhs = (vel1 - vel2).dot(&tangents1[j]); + let rhs = + (vel1 - vel2 + manifold_point.tangent_velocity).dot(&tangents1[j]); #[cfg(feature = "dim2")] let impulse = manifold_point.data.tangent_impulse * warmstart_coeff; #[cfg(feature = "dim3")] @@ -391,7 +392,7 @@ impl VelocityConstraint { active_contact.data.impulse = self.elements[k].normal_part.impulse; #[cfg(feature = "dim2")] { - active_contacts.data.tangent_impulse = self.elements[k].tangent_part[0].impulse; + active_contact.data.tangent_impulse = self.elements[k].tangent_part[0].impulse; } #[cfg(feature = "dim3")] { diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 8838011..97fa261 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -130,6 +130,8 @@ impl WVelocityConstraint { ); let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]); let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); + let tangent_velocity = + Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]); let impulse = SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); @@ -181,7 +183,7 @@ impl WVelocityConstraint { let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); let r = SimdReal::splat(1.0) / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); - let rhs = (vel1 - vel2).dot(&tangents1[j]); + let rhs = (vel1 - vel2 + tangent_velocity).dot(&tangents1[j]); constraint.elements[k].tangent_parts[j] = WVelocityConstraintElementPart { gcross1, diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 6f61b17..ddd5628 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -68,11 +68,11 @@ impl VelocityGroundConstraint { let mut rb2 = &bodies[manifold.data.body_pair.body2]; let flipped = !rb2.is_dynamic(); - let force_dir1 = if flipped { + let (force_dir1, flipped_multiplier) = if flipped { std::mem::swap(&mut rb1, &mut rb2); - manifold.data.normal + (manifold.data.normal, -1.0) } else { - -manifold.data.normal + (-manifold.data.normal, 1.0) }; let mj_lambda2 = rb2.active_set_offset; @@ -123,7 +123,7 @@ impl VelocityGroundConstraint { .as_nongrouped_ground_mut() .unwrap() } else { - unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable. + unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable. }; #[cfg(target_arch = "wasm32")] @@ -179,7 +179,9 @@ impl VelocityGroundConstraint { .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-tangents1[j])); let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2)); - let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]); + let rhs = (vel1 - vel2 + + flipped_multiplier * manifold_point.tangent_velocity) + .dot(&tangents1[j]); #[cfg(feature = "dim2")] let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff; #[cfg(feature = "dim3")] diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index 6f279d8..6e7216a 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -66,15 +66,17 @@ impl WVelocityGroundConstraint { let inv_dt = SimdReal::splat(params.inv_dt()); let mut rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH]; let mut rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH]; - let mut flipped = [false; SIMD_WIDTH]; + let mut flipped = [1.0; SIMD_WIDTH]; for ii in 0..SIMD_WIDTH { if !rbs2[ii].is_dynamic() { std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]); - flipped[ii] = true; + flipped[ii] = -1.0; } } + let flipped_sign = SimdReal::from(flipped); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); let ii2: AngularInertia = AngularInertia::from( array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], @@ -89,9 +91,8 @@ impl WVelocityGroundConstraint { let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let force_dir1 = Vector::from( - array![|ii| if flipped[ii] { manifolds[ii].data.normal } else { -manifolds[ii].data.normal }; SIMD_WIDTH], - ); + let normal1 = Vector::from(array![|ii| manifolds[ii].data.normal; SIMD_WIDTH]); + let force_dir1 = normal1 * -flipped_sign; let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -125,6 +126,8 @@ impl WVelocityGroundConstraint { ); let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]); let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); + let tangent_velocity = + Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]); let impulse = SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); @@ -170,7 +173,7 @@ impl WVelocityGroundConstraint { let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2)); - let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]); + let rhs = (vel1 - vel2 + tangent_velocity * flipped_sign).dot(&tangents1[j]); constraint.elements[k].tangent_parts[j] = WVelocityGroundConstraintElementPart { From b3405e56721dac2ebb22a7709cb7430fde7b4859 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 23 Feb 2021 15:47:44 +0100 Subject: [PATCH 05/10] Add a method to modify all the active dynamic bodies on the RigidBodySet. --- src/dynamics/rigid_body_set.rs | 42 ++++++++++++++++++++++++++++++---- 1 file changed, 38 insertions(+), 4 deletions(-) diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index 79d5827..f459d4f 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -234,13 +234,27 @@ impl RigidBodySet { self.bodies.get(handle.0) } + fn mark_as_modified( + handle: RigidBodyHandle, + rb: &mut RigidBody, + modified_bodies: &mut Vec, + modified_all_bodies: bool, + ) { + if !modified_all_bodies && !rb.changes.contains(RigidBodyChanges::MODIFIED) { + rb.changes = RigidBodyChanges::MODIFIED; + modified_bodies.push(handle); + } + } + /// Gets a mutable reference to the rigid-body with the given handle. pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> { let result = self.bodies.get_mut(handle.0)?; - if !self.modified_all_bodies && !result.changes.contains(RigidBodyChanges::MODIFIED) { - result.changes = RigidBodyChanges::MODIFIED; - self.modified_bodies.push(handle); - } + Self::mark_as_modified( + handle, + result, + &mut self.modified_bodies, + self.modified_all_bodies, + ); Some(result) } @@ -300,6 +314,26 @@ impl RigidBodySet { .filter_map(move |h| Some((*h, bodies.get(h.0)?))) } + /// Applies the given function on all the active dynamic rigid-bodies + /// contained by this set. + #[inline(always)] + pub fn foreach_active_dynamic_body_mut( + &mut self, + mut f: impl FnMut(RigidBodyHandle, &mut RigidBody), + ) { + for handle in &self.active_dynamic_set { + if let Some(rb) = self.bodies.get_mut(handle.0) { + Self::mark_as_modified( + *handle, + rb, + &mut self.modified_bodies, + self.modified_all_bodies, + ); + f(*handle, rb) + } + } + } + #[inline(always)] pub(crate) fn foreach_active_body_mut_internal( &mut self, From f8bf96fdc8a0cfd7324d589736d41057a6c1bfe8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 23 Feb 2021 15:48:04 +0100 Subject: [PATCH 06/10] Add a helper function for one-way platforms. --- src/pipeline/physics_hooks.rs | 75 ++++++++++++++++++++++++++++++-- src/pipeline/physics_pipeline.rs | 3 +- 2 files changed, 73 insertions(+), 5 deletions(-) diff --git a/src/pipeline/physics_hooks.rs b/src/pipeline/physics_hooks.rs index 12b658f..d7b7b7e 100644 --- a/src/pipeline/physics_hooks.rs +++ b/src/pipeline/physics_hooks.rs @@ -1,5 +1,7 @@ use crate::dynamics::RigidBody; use crate::geometry::{Collider, ColliderHandle, ContactManifold, SolverContact, SolverFlags}; +use crate::math::{Real, Vector}; +use na::ComplexField; /// Context given to custom collision filters to filter-out collisions. pub struct PairFilterContext<'a> { @@ -17,6 +19,7 @@ pub struct PairFilterContext<'a> { pub collider2: &'a Collider, } +/// Context given to custom contact modifiers to modify the contacts seen by the constrainst solver. pub struct ContactModificationContext<'a> { /// The first collider involved in the potential collision. pub rigid_body1: &'a RigidBody, @@ -40,6 +43,68 @@ pub struct ContactModificationContext<'a> { pub user_data: &'a mut u32, } +impl<'a> ContactModificationContext<'a> { + /// Helper function to update `self` to emulate a oneway-platform. + /// + /// The "oneway" behavior will only allow contacts between two colliders + /// if the local contact normal of the first collider involved in the contact + /// is almost aligned with the provided `allowed_local_n1` direction. + /// + /// To make this method work properly it must be called as part of the + /// `PhysicsHooks::modify_solver_contacts` method at each timestep, for each + /// contact manifold involving a one-way platform. The `self.user_data` field + /// must not be modified from the outside of this method. + pub fn update_as_oneway_platform( + &mut self, + allowed_local_n1: &Vector, + allowed_angle: Real, + ) { + const CONTACT_CONFIGURATION_UNKNOWN: u32 = 0; + const CONTACT_CURRENTLY_ALLOWED: u32 = 1; + const CONTACT_CURRENTLY_FORBIDDEN: u32 = 2; + + let cang = ComplexField::cos(allowed_angle); + + // Test the allowed normal with the local-space contact normal that + // points towards the exterior of context.collider1. + let contact_is_ok = self.manifold.local_n1.dot(&allowed_local_n1) >= cang; + + match *self.user_data { + CONTACT_CONFIGURATION_UNKNOWN => { + if contact_is_ok { + // The contact is close enough to the allowed normal. + *self.user_data = CONTACT_CURRENTLY_ALLOWED; + } else { + // The contact normal isn't close enough to the allowed + // normal, so remove all the contacts and mark further contacts + // as forbidden. + self.solver_contacts.clear(); + *self.user_data = CONTACT_CURRENTLY_FORBIDDEN; + } + } + CONTACT_CURRENTLY_FORBIDDEN => { + // Contacts are forbidden so we need to continue forbidding contacts + // until all the contacts are non-penetrating again. In that case, if + // the contacts are OK wrt. the contact normal, then we can mark them as allowed. + if contact_is_ok && self.solver_contacts.iter().all(|c| c.dist > 0.0) { + *self.user_data = CONTACT_CURRENTLY_ALLOWED; + } else { + // Discard all the contacts. + self.solver_contacts.clear(); + } + } + CONTACT_CURRENTLY_ALLOWED => { + // We allow all the contacts right now. The configuration becomes + // uncertain again when the contact manifold no longer contains any contact. + if self.solver_contacts.is_empty() { + *self.user_data = CONTACT_CONFIGURATION_UNKNOWN; + } + } + _ => unreachable!(), + } + } +} + bitflags::bitflags! { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// Flags affecting the behavior of the constraints solver for a given contact manifold. @@ -81,7 +146,9 @@ pub trait PhysicsHooks: Send + Sync { /// will be taken into account by the constraints solver. If this returns /// `Some(SolverFlags::empty())` then the constraints solver will ignore these /// contacts. - fn filter_contact_pair(&self, context: &PairFilterContext) -> Option; + fn filter_contact_pair(&self, _context: &PairFilterContext) -> Option { + None + } /// Applies the intersection pair filter. /// @@ -102,7 +169,9 @@ pub trait PhysicsHooks: Send + Sync { /// not compute any intersection information for it. /// If this return `true` then the narrow-phase will compute intersection /// information for this pair. - fn filter_intersection_pair(&self, context: &PairFilterContext) -> bool; + fn filter_intersection_pair(&self, _context: &PairFilterContext) -> bool { + false + } /// Modifies the set of contacts seen by the constraints solver. /// @@ -126,7 +195,7 @@ pub trait PhysicsHooks: Send + Sync { /// Each contact manifold is given a `u32` user-defined data that is persistent between /// timesteps (as long as the contact manifold exists). This user-defined data is initialized /// as 0 and can be modified in `context.user_data`. - fn modify_solver_contacts(&self, context: &mut ContactModificationContext); + fn modify_solver_contacts(&self, _context: &mut ContactModificationContext) {} } impl PhysicsHooks for () { diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index c030b1d..e92fb08 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -8,10 +8,9 @@ use crate::dynamics::{IntegrationParameters, JointSet, RigidBodySet}; use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver}; use crate::geometry::{ BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, NarrowPhase, - PhysicsHooks, }; use crate::math::{Real, Vector}; -use crate::pipeline::EventHandler; +use crate::pipeline::{EventHandler, PhysicsHooks}; /// The physics pipeline, responsible for stepping the whole physics simulation. /// From babcab0bed23fadd23181ccc58aae34fb80d01d8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 23 Feb 2021 15:49:23 +0100 Subject: [PATCH 07/10] Update the testbed to use PhysicsHooks. --- examples2d/all_examples2.rs | 2 ++ examples2d/damping2.rs | 2 +- examples3d/all_examples3.rs | 2 ++ examples3d/damping3.rs | 2 +- src/geometry/collider.rs | 17 ++++++++++++++++- src/geometry/contact_pair.rs | 11 +++++++++-- src/geometry/mod.rs | 2 -- src/geometry/narrow_phase.rs | 16 ++++++++-------- src/pipeline/collision_pipeline.rs | 6 ++---- src/pipeline/mod.rs | 4 ++++ src_testbed/harness/mod.rs | 14 ++++++++------ src_testbed/physics/mod.rs | 4 +++- src_testbed/testbed.rs | 10 +++++----- 13 files changed, 61 insertions(+), 31 deletions(-) diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index 5040c8a..f4430aa 100644 --- a/examples2d/all_examples2.rs +++ b/examples2d/all_examples2.rs @@ -18,6 +18,7 @@ mod debug_box_ball2; mod heightfield2; mod joints2; mod locked_rotations2; +mod one_way_platforms2; mod platform2; mod polyline2; mod pyramid2; @@ -65,6 +66,7 @@ pub fn main() { ("Heightfield", heightfield2::init_world), ("Joints", joints2::init_world), ("Locked rotations", locked_rotations2::init_world), + ("One-way platforms", one_way_platforms2::init_world), ("Platform", platform2::init_world), ("Polyline", polyline2::init_world), ("Pyramid", pyramid2::init_world), diff --git a/examples2d/damping2.rs b/examples2d/damping2.rs index 43ca2b6..68fd77d 100644 --- a/examples2d/damping2.rs +++ b/examples2d/damping2.rs @@ -40,6 +40,6 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ - testbed.set_world_with_gravity(bodies, colliders, joints, Vector2::zeros()); + testbed.set_world_with_params(bodies, colliders, joints, Vector2::zeros(), ()); testbed.look_at(Point2::new(3.0, 2.0), 50.0); } diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 9cbf4c3..8a71665 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -29,6 +29,7 @@ mod heightfield3; mod joints3; mod keva3; mod locked_rotations3; +mod one_way_platforms3; mod platform3; mod primitives3; mod restitution3; @@ -83,6 +84,7 @@ pub fn main() { ("Heightfield", heightfield3::init_world), ("Joints", joints3::init_world), ("Locked rotations", locked_rotations3::init_world), + ("One-way platforms", one_way_platforms3::init_world), ("Platform", platform3::init_world), ("Restitution", restitution3::init_world), ("Sensor", sensor3::init_world), diff --git a/examples3d/damping3.rs b/examples3d/damping3.rs index 8c68d3b..3847ceb 100644 --- a/examples3d/damping3.rs +++ b/examples3d/damping3.rs @@ -40,6 +40,6 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ - testbed.set_world_with_gravity(bodies, colliders, joints, Vector3::zeros()); + testbed.set_world_with_params(bodies, colliders, joints, Vector3::zeros(), ()); testbed.look_at(Point3::new(2.0, 2.5, 20.0), Point3::new(2.0, 2.5, 0.0)); } diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index ce263f8..4be6d29 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -1,5 +1,5 @@ use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle}; -use crate::geometry::{InteractionGroups, SharedShape}; +use crate::geometry::{InteractionGroups, SharedShape, SolverFlags}; use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector, DIM}; use crate::parry::transformation::vhacd::VHACDParameters; use parry::bounding_volume::AABB; @@ -50,6 +50,7 @@ pub struct Collider { shape: SharedShape, density: Real, pub(crate) flags: ColliderFlags, + pub(crate) solver_flags: SolverFlags, pub(crate) parent: RigidBodyHandle, pub(crate) delta: Isometry, pub(crate) position: Isometry, @@ -159,6 +160,9 @@ pub struct ColliderBuilder { pub delta: Isometry, /// Is this collider a sensor? pub is_sensor: bool, + /// Do we have to always call the contact modifier + /// on this collider? + pub modify_contacts: bool, /// The user-data of the collider being built. pub user_data: u128, /// The collision groups for the collider being built. @@ -182,6 +186,7 @@ impl ColliderBuilder { solver_groups: InteractionGroups::all(), friction_combine_rule: CoefficientCombineRule::Average, restitution_combine_rule: CoefficientCombineRule::Average, + modify_contacts: false, } } @@ -456,6 +461,13 @@ impl ColliderBuilder { self } + /// If set to `true` then the physics hooks will always run to modify + /// contacts involving this collider. + pub fn modify_contacts(mut self, modify_contacts: bool) -> Self { + self.modify_contacts = modify_contacts; + self + } + /// Sets the friction coefficient of the collider this builder will build. pub fn friction(mut self, friction: Real) -> Self { self.friction = friction; @@ -534,6 +546,8 @@ impl ColliderBuilder { flags = flags .with_friction_combine_rule(self.friction_combine_rule) .with_restitution_combine_rule(self.restitution_combine_rule); + let mut solver_flags = SolverFlags::default(); + solver_flags.set(SolverFlags::MODIFY_SOLVER_CONTACTS, self.modify_contacts); Collider { shape: self.shape.clone(), @@ -542,6 +556,7 @@ impl ColliderBuilder { restitution: self.restitution, delta: self.delta, flags, + solver_flags, parent: RigidBodyHandle::invalid(), position: Isometry::identity(), predicted_position: Isometry::identity(), diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index 11e0188..50094ca 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -16,6 +16,12 @@ bitflags::bitflags! { } } +impl Default for SolverFlags { + fn default() -> Self { + SolverFlags::COMPUTE_IMPULSES + } +} + #[derive(Copy, Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// A single contact between two collider. @@ -126,10 +132,11 @@ pub struct SolverContact { pub friction: Real, /// The effective restitution coefficient at this contact point. pub restitution: Real, - /// The artificially add relative velocity at the contact point. + /// The desired tangent relative velocity at the contact point. + /// /// This is set to zero by default. Set to a non-zero value to /// simulate, e.g., conveyor belts. - pub surface_velocity: Vector, + pub tangent_velocity: Vector, /// Associated contact data used to warm-start the constraints /// solver. pub data: ContactData, diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 2997e24..ab04b25 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -10,7 +10,6 @@ pub use self::interaction_graph::{ }; pub use self::interaction_groups::InteractionGroups; pub use self::narrow_phase::NarrowPhase; -pub use self::pair_filter::{PairFilterContext, PhysicsHooks}; pub use parry::query::TrackedContact; @@ -109,4 +108,3 @@ mod contact_pair; mod interaction_graph; mod interaction_groups; mod narrow_phase; -mod pair_filter; diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index e929e0f..d05b19a 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -4,15 +4,15 @@ use rayon::prelude::*; use crate::data::pubsub::Subscription; use crate::data::Coarena; use crate::dynamics::{BodyPair, CoefficientCombineRule, RigidBodySet}; -use crate::geometry::pair_filter::{ContactModificationContext, PhysicsHooksFlags}; use crate::geometry::{ - BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactData, ContactEvent, - ContactManifoldData, IntersectionEvent, PairFilterContext, PhysicsHooks, RemovedCollider, - SolverContact, SolverFlags, + BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ColliderSet, ContactData, + ContactEvent, ContactManifold, ContactManifoldData, ContactPair, InteractionGraph, + IntersectionEvent, RemovedCollider, SolverContact, SolverFlags, }; -use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph}; use crate::math::{Real, Vector}; -use crate::pipeline::EventHandler; +use crate::pipeline::{ + ContactModificationContext, EventHandler, PairFilterContext, PhysicsHooks, PhysicsHooksFlags, +}; use parry::query::{DefaultQueryDispatcher, PersistentQueryDispatcher}; use parry::utils::IsometryOpt; use std::collections::HashMap; @@ -518,7 +518,7 @@ impl NarrowPhase { return; } } else { - SolverFlags::COMPUTE_IMPULSES + co1.solver_flags | co2.solver_flags }; if !co1.solver_groups.test(co2.solver_groups) { @@ -573,7 +573,7 @@ impl NarrowPhase { dist: contact.dist, friction, restitution, - surface_velocity: Vector::zeros(), + tangent_velocity: Vector::zeros(), data: contact.data, }; diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index 6f02d98..a74a6e5 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -1,11 +1,9 @@ //! Physics pipeline structures. use crate::dynamics::{JointSet, RigidBodySet}; -use crate::geometry::{ - BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, NarrowPhase, PhysicsHooks, -}; +use crate::geometry::{BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, NarrowPhase}; use crate::math::Real; -use crate::pipeline::EventHandler; +use crate::pipeline::{EventHandler, PhysicsHooks}; /// The collision pipeline, responsible for performing collision detection between colliders. /// diff --git a/src/pipeline/mod.rs b/src/pipeline/mod.rs index 287de9d..fd85cfa 100644 --- a/src/pipeline/mod.rs +++ b/src/pipeline/mod.rs @@ -2,10 +2,14 @@ pub use collision_pipeline::CollisionPipeline; pub use event_handler::{ChannelEventCollector, EventHandler}; +pub use physics_hooks::{ + ContactModificationContext, PairFilterContext, PhysicsHooks, PhysicsHooksFlags, +}; pub use physics_pipeline::PhysicsPipeline; pub use query_pipeline::QueryPipeline; mod collision_pipeline; mod event_handler; +mod physics_hooks; mod physics_pipeline; mod query_pipeline; diff --git a/src_testbed/harness/mod.rs b/src_testbed/harness/mod.rs index 53aa893..5e75d85 100644 --- a/src_testbed/harness/mod.rs +++ b/src_testbed/harness/mod.rs @@ -7,7 +7,7 @@ use plugin::HarnessPlugin; use rapier::dynamics::{IntegrationParameters, JointSet, RigidBodySet}; use rapier::geometry::{BroadPhase, ColliderSet, NarrowPhase}; use rapier::math::Vector; -use rapier::pipeline::{ChannelEventCollector, PhysicsPipeline, QueryPipeline}; +use rapier::pipeline::{ChannelEventCollector, PhysicsHooks, PhysicsPipeline, QueryPipeline}; pub mod plugin; @@ -111,15 +111,16 @@ impl Harness { } pub fn set_world(&mut self, bodies: RigidBodySet, colliders: ColliderSet, joints: JointSet) { - self.set_world_with_gravity(bodies, colliders, joints, Vector::y() * -9.81) + self.set_world_with_params(bodies, colliders, joints, Vector::y() * -9.81, ()) } - pub fn set_world_with_gravity( + pub fn set_world_with_params( &mut self, bodies: RigidBodySet, colliders: ColliderSet, joints: JointSet, gravity: Vector, + hooks: impl PhysicsHooks + 'static, ) { // println!("Num bodies: {}", bodies.len()); // println!("Num joints: {}", joints.len()); @@ -127,6 +128,8 @@ impl Harness { self.physics.bodies = bodies; self.physics.colliders = colliders; self.physics.joints = joints; + self.physics.hooks = Box::new(hooks); + self.physics.broad_phase = BroadPhase::new(); self.physics.narrow_phase = NarrowPhase::new(); self.state.timestep_id = 0; @@ -176,8 +179,7 @@ impl Harness { &mut physics.bodies, &mut physics.colliders, &mut physics.joints, - None, - None, + &*physics.hooks, event_handler, ); }); @@ -192,7 +194,7 @@ impl Harness { &mut self.physics.bodies, &mut self.physics.colliders, &mut self.physics.joints, - &(), + &*self.physics.hooks, &self.event_handler, ); diff --git a/src_testbed/physics/mod.rs b/src_testbed/physics/mod.rs index 808e9bd..0987e32 100644 --- a/src_testbed/physics/mod.rs +++ b/src_testbed/physics/mod.rs @@ -2,7 +2,7 @@ use crossbeam::channel::Receiver; use rapier::dynamics::{IntegrationParameters, JointSet, RigidBodySet}; use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, IntersectionEvent, NarrowPhase}; use rapier::math::Vector; -use rapier::pipeline::{PhysicsPipeline, QueryPipeline}; +use rapier::pipeline::{PhysicsHooks, PhysicsPipeline, QueryPipeline}; pub struct PhysicsSnapshot { timestep_id: usize, @@ -77,6 +77,7 @@ pub struct PhysicsState { pub query_pipeline: QueryPipeline, pub integration_parameters: IntegrationParameters, pub gravity: Vector, + pub hooks: Box, } impl PhysicsState { @@ -91,6 +92,7 @@ impl PhysicsState { query_pipeline: QueryPipeline::new(), integration_parameters: IntegrationParameters::default(), gravity: Vector::y() * -9.81, + hooks: Box::new(()), } } } diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index b8ef324..bc5cd6c 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -25,6 +25,7 @@ use rapier::geometry::{ColliderHandle, ColliderSet, NarrowPhase}; #[cfg(feature = "dim3")] use rapier::geometry::{InteractionGroups, Ray}; use rapier::math::{Isometry, Vector}; +use rapier::pipeline::PhysicsHooks; #[cfg(all(feature = "dim2", feature = "other-backends"))] use crate::box2d_backend::Box2dWorld; @@ -245,20 +246,19 @@ impl Testbed { } pub fn set_world(&mut self, bodies: RigidBodySet, colliders: ColliderSet, joints: JointSet) { - self.set_world_with_gravity(bodies, colliders, joints, Vector::y() * -9.81) + self.set_world_with_params(bodies, colliders, joints, Vector::y() * -9.81, ()) } - pub fn set_world_with_gravity( + pub fn set_world_with_params( &mut self, bodies: RigidBodySet, colliders: ColliderSet, joints: JointSet, gravity: Vector, + hooks: impl PhysicsHooks + 'static, ) { - println!("Num bodies: {}", bodies.len()); - println!("Num joints: {}", joints.len()); self.harness - .set_world_with_gravity(bodies, colliders, joints, gravity); + .set_world_with_params(bodies, colliders, joints, gravity, hooks); self.state .action_flags From 0f0f2c344fd78c3d1ca9ec07d3523c5d0ea10c82 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 23 Feb 2021 16:02:19 +0100 Subject: [PATCH 08/10] Rename modify_contacts -> modify_solver_contacts. --- examples2d/one_way_platforms2.rs | 4 ++-- examples3d/one_way_platforms3.rs | 4 ++-- src/geometry/collider.rs | 13 ++++++++----- src/pipeline/physics_hooks.rs | 2 +- 4 files changed, 13 insertions(+), 10 deletions(-) diff --git a/examples2d/one_way_platforms2.rs b/examples2d/one_way_platforms2.rs index c7f58e8..2e1788e 100644 --- a/examples2d/one_way_platforms2.rs +++ b/examples2d/one_way_platforms2.rs @@ -77,12 +77,12 @@ pub fn init_world(testbed: &mut Testbed) { let collider = ColliderBuilder::cuboid(25.0, 0.5) .translation(30.0, 2.0) - .modify_contacts(true) + .modify_solver_contacts(true) .build(); let platform1 = colliders.insert(collider, handle, &mut bodies); let collider = ColliderBuilder::cuboid(25.0, 0.5) .translation(-30.0, -2.0) - .modify_contacts(true) + .modify_solver_contacts(true) .build(); let platform2 = colliders.insert(collider, handle, &mut bodies); diff --git a/examples3d/one_way_platforms3.rs b/examples3d/one_way_platforms3.rs index b3182a4..ea70e69 100644 --- a/examples3d/one_way_platforms3.rs +++ b/examples3d/one_way_platforms3.rs @@ -77,12 +77,12 @@ pub fn init_world(testbed: &mut Testbed) { let collider = ColliderBuilder::cuboid(9.0, 0.5, 25.0) .translation(0.0, 2.0, 30.0) - .modify_contacts(true) + .modify_solver_contacts(true) .build(); let platform1 = colliders.insert(collider, handle, &mut bodies); let collider = ColliderBuilder::cuboid(9.0, 0.5, 25.0) .translation(0.0, -2.0, -30.0) - .modify_contacts(true) + .modify_solver_contacts(true) .build(); let platform2 = colliders.insert(collider, handle, &mut bodies); diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 4be6d29..b006f9e 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -162,7 +162,7 @@ pub struct ColliderBuilder { pub is_sensor: bool, /// Do we have to always call the contact modifier /// on this collider? - pub modify_contacts: bool, + pub modify_solver_contacts: bool, /// The user-data of the collider being built. pub user_data: u128, /// The collision groups for the collider being built. @@ -186,7 +186,7 @@ impl ColliderBuilder { solver_groups: InteractionGroups::all(), friction_combine_rule: CoefficientCombineRule::Average, restitution_combine_rule: CoefficientCombineRule::Average, - modify_contacts: false, + modify_solver_contacts: false, } } @@ -463,8 +463,8 @@ impl ColliderBuilder { /// If set to `true` then the physics hooks will always run to modify /// contacts involving this collider. - pub fn modify_contacts(mut self, modify_contacts: bool) -> Self { - self.modify_contacts = modify_contacts; + pub fn modify_solver_contacts(mut self, modify_solver_contacts: bool) -> Self { + self.modify_solver_contacts = modify_solver_contacts; self } @@ -547,7 +547,10 @@ impl ColliderBuilder { .with_friction_combine_rule(self.friction_combine_rule) .with_restitution_combine_rule(self.restitution_combine_rule); let mut solver_flags = SolverFlags::default(); - solver_flags.set(SolverFlags::MODIFY_SOLVER_CONTACTS, self.modify_contacts); + solver_flags.set( + SolverFlags::MODIFY_SOLVER_CONTACTS, + self.modify_solver_contacts, + ); Collider { shape: self.shape.clone(), diff --git a/src/pipeline/physics_hooks.rs b/src/pipeline/physics_hooks.rs index d7b7b7e..11ca485 100644 --- a/src/pipeline/physics_hooks.rs +++ b/src/pipeline/physics_hooks.rs @@ -179,7 +179,7 @@ pub trait PhysicsHooks: Send + Sync { /// contains the `PhysicsHooksFlags::MODIFY_SOLVER_CONTACTS` flags. /// /// By default, the content of `solver_contacts` is computed from `manifold.points`. - /// This method will be called on each contact manifold which have the flag `SolverFlags::MODIFY_CONTACTS` set. + /// This method will be called on each contact manifold which have the flag `SolverFlags::modify_solver_contacts` set. /// This method can be used to modify the set of solver contacts seen by the constraints solver: contacts /// can be removed and modified. /// From a60c6e5fddbf18df8295c0055ebda772b3bb6f60 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 23 Feb 2021 16:26:02 +0100 Subject: [PATCH 09/10] Fix the compilation of tests. --- examples2d/one_way_platforms2.rs | 4 ++-- examples3d/one_way_platforms3.rs | 4 ++-- src/pipeline/physics_pipeline.rs | 6 ++---- 3 files changed, 6 insertions(+), 8 deletions(-) diff --git a/examples2d/one_way_platforms2.rs b/examples2d/one_way_platforms2.rs index 2e1788e..551eaf1 100644 --- a/examples2d/one_way_platforms2.rs +++ b/examples2d/one_way_platforms2.rs @@ -47,7 +47,7 @@ impl PhysicsHooks for OneWayPlatformHook { context.update_as_oneway_platform(&allowed_local_n1, 0.1); // Set the surface velocity of the accepted contacts. - let surface_velocity = if context.collider_handle1 == self.platform1 + let tangent_velocity = if context.collider_handle1 == self.platform1 || context.collider_handle2 == self.platform2 { -12.0 @@ -56,7 +56,7 @@ impl PhysicsHooks for OneWayPlatformHook { }; for contact in context.solver_contacts.iter_mut() { - contact.surface_velocity.x = surface_velocity; + contact.tangent_velocity.x = tangent_velocity; } } } diff --git a/examples3d/one_way_platforms3.rs b/examples3d/one_way_platforms3.rs index ea70e69..173d03d 100644 --- a/examples3d/one_way_platforms3.rs +++ b/examples3d/one_way_platforms3.rs @@ -47,7 +47,7 @@ impl PhysicsHooks for OneWayPlatformHook { context.update_as_oneway_platform(&allowed_local_n1, 0.1); // Set the surface velocity of the accepted contacts. - let surface_velocity = if context.collider_handle1 == self.platform1 + let tangent_velocity = if context.collider_handle1 == self.platform1 || context.collider_handle2 == self.platform2 { -12.0 @@ -56,7 +56,7 @@ impl PhysicsHooks for OneWayPlatformHook { }; for contact in context.solver_contacts.iter_mut() { - contact.surface_velocity.z = surface_velocity; + contact.tangent_velocity.z = tangent_velocity; } } } diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index e92fb08..198c4be 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -278,8 +278,7 @@ mod test { &mut bodies, &mut colliders, &mut joints, - None, - None, + &(), &(), ); } @@ -322,8 +321,7 @@ mod test { &mut bodies, &mut colliders, &mut joints, - None, - None, + &(), &(), ); } From 3cc2738e5fdcb0d25818b550cdff93eab75f1b20 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 23 Feb 2021 16:47:48 +0100 Subject: [PATCH 10/10] Fix warnings in the WASM build. --- src/dynamics/solver/velocity_constraint.rs | 4 ++-- src/dynamics/solver/velocity_ground_constraint.rs | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 3a751df..2de9807 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -152,7 +152,7 @@ impl VelocityConstraint { let force_dir1 = -manifold.data.normal; let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; - for (l, manifold_points) in manifold + for (_l, manifold_points) in manifold .data .solver_contacts .chunks(MAX_MANIFOLD_POINTS) @@ -295,7 +295,7 @@ impl VelocityConstraint { if push { out_constraints.push(AnyVelocityConstraint::Nongrouped(constraint)); } else { - out_constraints[manifold.data.constraint_index + l] = + out_constraints[manifold.data.constraint_index + _l] = AnyVelocityConstraint::Nongrouped(constraint); } } diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index ddd5628..beb07ed 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -78,7 +78,7 @@ impl VelocityGroundConstraint { let mj_lambda2 = rb2.active_set_offset; let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; - for (l, manifold_points) in manifold + for (_l, manifold_points) in manifold .data .solver_contacts .chunks(MAX_MANIFOLD_POINTS) @@ -202,7 +202,7 @@ impl VelocityGroundConstraint { if push { out_constraints.push(AnyVelocityConstraint::NongroupedGround(constraint)); } else { - out_constraints[manifold.data.constraint_index + l] = + out_constraints[manifold.data.constraint_index + _l] = AnyVelocityConstraint::NongroupedGround(constraint); } }