From 076e88bb527185e7f9e34985236a45a67a48807b Mon Sep 17 00:00:00 2001 From: Joona Aalto Date: Sat, 17 Feb 2024 00:05:03 +0200 Subject: [PATCH 1/3] Store impulses in contacts and refactor contact data --- src/constraints/penetration.rs | 49 ++++---- src/lib.rs | 6 + src/plugins/collision/contact_query.rs | 36 ++++-- src/plugins/collision/mod.rs | 159 +++++++++++++++++++++++++ src/plugins/collision/narrow_phase.rs | 5 + src/plugins/debug/configuration.rs | 41 ++++++- src/plugins/debug/mod.rs | 24 +++- src/plugins/setup/mod.rs | 1 + src/plugins/solver.rs | 70 ++++++++--- 9 files changed, 336 insertions(+), 55 deletions(-) diff --git a/src/constraints/penetration.rs b/src/constraints/penetration.rs index b5deac51..24fd6769 100644 --- a/src/constraints/penetration.rs +++ b/src/constraints/penetration.rs @@ -16,8 +16,14 @@ pub struct PenetrationConstraint { pub entity1: Entity, /// Second entity in the constraint. pub entity2: Entity, + /// The entity of the collider of the first body. + pub collider_entity1: Entity, + /// The entity of the collider of the second body. + pub collider_entity2: Entity, /// Data associated with the contact. pub contact: ContactData, + /// The index of the contact in the manifold. + pub manifold_index: usize, /// Vector from the first entity's center of mass to the contact point in local coordinates. pub r1: Vector, /// Vector from the second entity's center of mass to the contact point in local coordinates. @@ -28,16 +34,10 @@ pub struct PenetrationConstraint { pub tangent_lagrange: Scalar, /// The constraint's compliance, the inverse of stiffness, has the unit meters / Newton. pub compliance: Scalar, - /// The coefficient of [dynamic friction](Friction) in this contact. - pub dynamic_friction_coefficient: Scalar, - /// The coefficient of [static friction](Friction) in this contact. - pub static_friction_coefficient: Scalar, - /// The coefficient of [restitution](Restitution) in this contact. - pub restitution_coefficient: Scalar, - /// Normal force acting along the constraint. - pub normal_force: Vector, - /// Static friction force acting along this constraint. - pub static_friction_force: Vector, + /// The effective [friction](Friction) of the contact. + pub friction: Friction, + /// The effective [restitution](Restitution) of the contact. + pub restitution: Restitution, } impl XpbdConstraint<2> for PenetrationConstraint { @@ -70,10 +70,15 @@ impl XpbdConstraint<2> for PenetrationConstraint { impl PenetrationConstraint { /// Creates a new [`PenetrationConstraint`] with the given bodies and contact data. + /// + /// The `manifold_index` is the index of the contact in a [`ContactManifold`]. pub fn new( body1: &RigidBodyQueryItem, body2: &RigidBodyQueryItem, + collider_entity1: Entity, + collider_entity2: Entity, contact: ContactData, + manifold_index: usize, ) -> Self { let r1 = contact.point1 - body1.center_of_mass.0; let r2 = contact.point2 - body2.center_of_mass.0; @@ -81,17 +86,17 @@ impl PenetrationConstraint { Self { entity1: body1.entity, entity2: body2.entity, + collider_entity1, + collider_entity2, contact, + manifold_index, r1, r2, normal_lagrange: 0.0, tangent_lagrange: 0.0, compliance: 0.0, - dynamic_friction_coefficient: 0.0, - static_friction_coefficient: 0.0, - restitution_coefficient: 0.0, - normal_force: Vector::ZERO, - static_friction_force: Vector::ZERO, + friction: body1.friction.combine(*body2.friction), + restitution: body1.restitution.combine(*body2.restitution), } } @@ -126,8 +131,10 @@ impl PenetrationConstraint { // Apply positional correction to solve overlap self.apply_positional_correction(body1, body2, delta_lagrange, normal, r1, r2); - // Update normal force using the equation f = lambda * n / h^2 - self.normal_force = self.normal_lagrange * normal / dt.powi(2); + // Update normal impulse. + // f = lambda / h^2 + // i = f * h = lambda / h + self.contact.normal_impulse += self.normal_lagrange / dt; } fn solve_friction( @@ -170,7 +177,7 @@ impl PenetrationConstraint { let w = [w1, w2]; // Apply static friction if |delta_x_perp| < mu_s * d - if sliding_len < self.static_friction_coefficient * penetration { + if sliding_len < self.friction.static_coefficient * penetration { // Compute Lagrange multiplier update for static friction let delta_lagrange = self.compute_lagrange_update(lagrange, sliding_len, &gradients, &w, compliance, dt); @@ -179,8 +186,10 @@ impl PenetrationConstraint { // Apply positional correction to handle static friction self.apply_positional_correction(body1, body2, delta_lagrange, tangent, r1, r2); - // Update static friction force using the equation f = lambda * n / h^2 - self.static_friction_force = self.tangent_lagrange * tangent / dt.powi(2); + // Update static friction impulse. + // f = lambda / h^2 + // i = f * h = lambda / h + self.contact.tangent_impulse += self.tangent_lagrange / dt; } } } diff --git a/src/lib.rs b/src/lib.rs index e546b4cf..b8e78fa5 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -693,6 +693,8 @@ pub enum PhysicsStepSet { /// 4. Solve positional and angular constraints /// 5. Update velocities /// 6. Solve velocity constraints (dynamic friction and restitution) +/// 7. Store contact impulses in [`Collisions`]. +/// 8. Apply [`AccumulatedTranslation`] to positions. #[derive(SystemSet, Clone, Copy, Debug, PartialEq, Eq, Hash)] pub enum SubstepSet { /// Responsible for integrating Newton's 2nd law of motion, @@ -736,6 +738,10 @@ pub enum SubstepSet { /// /// See [`SolverPlugin`]. SolveVelocities, + /// Contact impulses computed by the solver are stored in contacts in [`Collisions`]. + /// + /// See [`SolverPlugin`]. + StoreImpulses, /// Responsible for applying translation accumulated during the substep. /// /// See [`SolverPlugin`]. diff --git a/src/plugins/collision/contact_query.rs b/src/plugins/collision/contact_query.rs index 5050c2fc..d6ba310f 100644 --- a/src/plugins/collision/contact_query.rs +++ b/src/plugins/collision/contact_query.rs @@ -68,7 +68,7 @@ pub fn contact( position2: impl Into, rotation2: impl Into, prediction_distance: Scalar, -) -> Result, UnsupportedShape> { +) -> Result, UnsupportedShape> { let rotation1: Rotation = rotation1.into(); let rotation2: Rotation = rotation2.into(); let isometry1 = utils::make_isometry(position1.into(), rotation1); @@ -100,13 +100,13 @@ pub fn contact( return None; } - Some(ContactData { + Some(SingleContact::new( point1, point2, normal1, normal2, - penetration: -contact.dist, - }) + -contact.dist, + )) } else { None } @@ -175,6 +175,9 @@ pub fn contact_manifolds( &mut manifolds, &mut None, ); + + let mut manifold_index = 0; + manifolds .iter() .filter_map(|manifold| { @@ -196,21 +199,30 @@ pub fn contact_manifolds( return None; } - Some(ContactManifold { + let manifold = ContactManifold { normal1, normal2, contacts: manifold .contacts() .iter() - .map(|contact| ContactData { - point1: subpos1.transform_point(&contact.local_p1).into(), - point2: subpos2.transform_point(&contact.local_p2).into(), - normal1, - normal2, - penetration: -contact.dist, + .enumerate() + .map(|(contact_index, contact)| { + ContactData::new( + subpos1.transform_point(&contact.local_p1).into(), + subpos2.transform_point(&contact.local_p2).into(), + normal1, + normal2, + -contact.dist, + contact_index, + ) }) .collect(), - }) + index: manifold_index, + }; + + manifold_index += 1; + + Some(manifold) }) .collect() } diff --git a/src/plugins/collision/mod.rs b/src/plugins/collision/mod.rs index 150dd5c2..beb6c7d0 100644 --- a/src/plugins/collision/mod.rs +++ b/src/plugins/collision/mod.rs @@ -266,6 +266,33 @@ pub struct Contacts { pub during_current_substep: bool, /// True if the bodies were in contact during the previous frame. pub during_previous_frame: bool, + /// The total normal impulse applied to the first body in a collision. + /// + /// To get the corresponding force, multiply the impulse by `Time`. + pub total_normal_impulse: Scalar, + /// The total tangent impulse applied to the first body in a collision. + /// + /// To get the corresponding force, multiply the impulse by `Time`. + #[doc(alias = "total_friction_impulse")] + pub total_tangent_impulse: Scalar, +} + +impl Contacts { + /// The force corresponding to the total normal impulse applied over `delta_time`. + /// + /// Because contacts are solved over several substeps, `delta_time` should + /// typically use `Time`. + pub fn total_normal_force(&self, delta_time: Scalar) -> Scalar { + self.total_normal_impulse / delta_time + } + + /// The force corresponding to the total tangent impulse applied over `delta_time`. + /// + /// Because contacts are solved over several substeps, `delta_time` should + /// typically use `Time`. + pub fn total_tangent_force(&self, delta_time: Scalar) -> Scalar { + self.total_tangent_impulse / delta_time + } } /// A contact manifold between two colliders, containing a set of contact points. @@ -281,6 +308,8 @@ pub struct ContactManifold { /// A contact normal shared by all contacts in this manifold, /// expressed in the local space of the second entity. pub normal2: Vector, + /// The index of the manifold in the collision. + pub index: usize, } impl ContactManifold { @@ -295,6 +324,66 @@ impl ContactManifold { } } +/// Data related to a single contact between two bodies. +/// +/// If you want a contact that belongs to a [contact manifold](ContactManifold) and has more data, +/// see [`ContactData`]. +#[derive(Clone, Copy, Debug, PartialEq)] +#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] +pub struct SingleContact { + /// Contact point on the first entity in local coordinates. + pub point1: Vector, + /// Contact point on the second entity in local coordinates. + pub point2: Vector, + /// A contact normal expressed in the local space of the first entity. + pub normal1: Vector, + /// A contact normal expressed in the local space of the second entity. + pub normal2: Vector, + /// Penetration depth. + pub penetration: Scalar, +} + +impl SingleContact { + /// Creates a new [`SingleContact`]. The contact points and normals should be given in local space. + pub fn new( + point1: Vector, + point2: Vector, + normal1: Vector, + normal2: Vector, + penetration: Scalar, + ) -> Self { + Self { + point1, + point2, + normal1, + normal2, + penetration, + } + } + + /// Returns the global contact point on the first entity, + /// transforming the local point by the given entity position and rotation. + pub fn global_point1(&self, position: &Position, rotation: &Rotation) -> Vector { + position.0 + rotation.rotate(self.point1) + } + + /// Returns the global contact point on the second entity, + /// transforming the local point by the given entity position and rotation. + pub fn global_point2(&self, position: &Position, rotation: &Rotation) -> Vector { + position.0 + rotation.rotate(self.point2) + } + + /// Returns the world-space contact normal pointing towards the exterior of the first entity. + pub fn global_normal1(&self, rotation: &Rotation) -> Vector { + rotation.rotate(self.normal1) + } + + /// Returns the world-space contact normal pointing towards the exterior of the second entity. + pub fn global_normal2(&self, rotation: &Rotation) -> Vector { + rotation.rotate(self.normal2) + } +} + /// Data related to a contact between two bodies. #[derive(Clone, Copy, Debug, PartialEq)] #[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] @@ -309,9 +398,79 @@ pub struct ContactData { pub normal2: Vector, /// Penetration depth. pub penetration: Scalar, + /// The impulse applied to the first body along the normal. + /// + /// To get the corresponding force, multiply the impulse by `Time`. + /// + /// ## Caveats + /// + /// * This will initially be zero after collision detection and will only be computed after constraint solving. + /// It is recommended to access this before or after physics. + /// * The impulse is from a single *substep*. Each physics frame has [`SubstepCount`] substeps. + /// * Impulses in contact events like [`Collision`] currently use the impulse from the *last* substep. + /// + /// The total impulse for a collision including all contacts can be accessed in [`Contacts`] returned by + /// the [`Collision`] event or the [`Collisions`] rsource. + pub normal_impulse: Scalar, + /// The impulse applied to the first body along the tangent. This corresponds to the impulse caused by friction. + /// + /// To get the corresponding force, multiply the impulse by `Time`. + /// + /// ## Caveats + /// + /// * This will initially be zero after collision detection and will only be computed after constraint solving. + /// It is recommended to access this before or after physics. + /// * The impulse is from a single *substep*. Each physics frame has [`SubstepCount`] substeps. + /// * Impulses in contact events like [`Collision`] currently use the impulse from the *last* substep. + /// + /// The total impulse for a collision including all contacts can be accessed in [`Contacts`] returned by + /// the [`Collision`] event or the [`Collisions`] rsource. + #[doc(alias = "friction_impulse")] + pub tangent_impulse: Scalar, + /// The index of the contact in a contact manifold if it is in one. + pub index: usize, } impl ContactData { + /// Creates a new [`ContactData`]. The contact points and normals should be given in local space. + /// + /// The given `index` is the index of the contact in a contact manifold, if it is in one. + pub fn new( + point1: Vector, + point2: Vector, + normal1: Vector, + normal2: Vector, + penetration: Scalar, + index: usize, + ) -> Self { + Self { + point1, + point2, + normal1, + normal2, + penetration, + normal_impulse: 0.0, + tangent_impulse: 0.0, + index, + } + } + + /// The force corresponding to the normal impulse applied over `delta_time`. + /// + /// Because contacts are solved over several substeps, `delta_time` should + /// typically use `Time`. + pub fn normal_force(&self, delta_time: Scalar) -> Scalar { + self.normal_impulse / delta_time + } + + /// The force corresponding to the tangent impulse applied over `delta_time`. + /// + /// Because contacts are solved over several substeps, `delta_time` should + /// typically use `Time`. + pub fn tangent_force(&self, delta_time: Scalar) -> Scalar { + self.tangent_impulse / delta_time + } + /// Returns the global contact point on the first entity, /// transforming the local point by the given entity position and rotation. pub fn global_point1(&self, position: &Position, rotation: &Rotation) -> Vector { diff --git a/src/plugins/collision/narrow_phase.rs b/src/plugins/collision/narrow_phase.rs index cc07b82f..2bb64c72 100644 --- a/src/plugins/collision/narrow_phase.rs +++ b/src/plugins/collision/narrow_phase.rs @@ -197,6 +197,8 @@ fn process_collision_pair( *rotation2, narrow_phase_config.prediction_distance, ), + total_normal_impulse: 0.0, + total_tangent_impulse: 0.0, }; if !contacts.manifolds.is_empty() { @@ -213,6 +215,9 @@ pub fn reset_collision_states( query: Query<(Option<&RigidBody>, Has)>, ) { for contacts in collisions.get_internal_mut().values_mut() { + contacts.total_normal_impulse = 0.0; + contacts.total_tangent_impulse = 0.0; + if let Ok([(rb1, sleeping1), (rb2, sleeping2)]) = query.get_many([contacts.entity1, contacts.entity2]) { diff --git a/src/plugins/debug/configuration.rs b/src/plugins/debug/configuration.rs index f0d14c36..128276bf 100644 --- a/src/plugins/debug/configuration.rs +++ b/src/plugins/debug/configuration.rs @@ -52,6 +52,8 @@ pub struct PhysicsDebugConfig { pub contact_point_color: Option, /// The color of the contact normals. If `None`, the contact normals will not be rendered. pub contact_normal_color: Option, + /// The scale used for contact normals. + pub contact_normal_scale: ContactGizmoScale, /// The color of the lines drawn from the centers of bodies to their joint anchors. pub joint_anchor_color: Option, /// The color of the lines drawn between joint anchors, indicating the separation. @@ -88,6 +90,7 @@ impl Default for PhysicsDebugConfig { sleeping_color_multiplier: Some([1.0, 1.0, 0.4, 1.0]), contact_point_color: None, contact_normal_color: None, + contact_normal_scale: ContactGizmoScale::default(), joint_anchor_color: Some(Color::PINK), joint_separation_color: Some(Color::RED), raycast_color: Some(Color::RED), @@ -102,6 +105,28 @@ impl Default for PhysicsDebugConfig { } } +/// The scale used for contact normals rendered using gizmos. +#[derive(Reflect)] +pub enum ContactGizmoScale { + /// The length of the rendered contact normal is constant. + Constant(Scalar), + /// The length of the rendered contact normal is scaled by the contact force and the given scaling factor. + Scaled(Scalar), +} + +impl Default for ContactGizmoScale { + fn default() -> Self { + #[cfg(feature = "2d")] + { + Self::Scaled(0.025) + } + #[cfg(feature = "3d")] + { + Self::Scaled(0.25) + } + } +} + impl PhysicsDebugConfig { /// Creates a [`PhysicsDebugConfig`] configuration with all rendering options enabled. pub fn all() -> Self { @@ -116,6 +141,7 @@ impl PhysicsDebugConfig { sleeping_color_multiplier: Some([1.0, 1.0, 0.4, 1.0]), contact_point_color: Some(Color::CYAN), contact_normal_color: Some(Color::RED), + contact_normal_scale: ContactGizmoScale::default(), joint_anchor_color: Some(Color::PINK), joint_separation_color: Some(Color::RED), raycast_color: Some(Color::RED), @@ -141,6 +167,7 @@ impl PhysicsDebugConfig { sleeping_color_multiplier: None, contact_point_color: None, contact_normal_color: None, + contact_normal_scale: ContactGizmoScale::default(), joint_anchor_color: None, joint_separation_color: None, raycast_color: None, @@ -233,12 +260,24 @@ impl PhysicsDebugConfig { self } - /// Sets the contact color. + /// Sets the contact point color. pub fn with_contact_point_color(mut self, color: Color) -> Self { self.contact_point_color = Some(color); self } + /// Sets the contact normal color. + pub fn with_contact_normal_color(mut self, color: Color) -> Self { + self.contact_normal_color = Some(color); + self + } + + /// Sets the contact normal scale. + pub fn with_contact_normal_scale(mut self, scale: ContactGizmoScale) -> Self { + self.contact_normal_scale = scale; + self + } + /// Sets the colors used for debug rendering joints. pub fn with_joint_colors(anchor_color: Option, separation_color: Option) -> Self { Self { diff --git a/src/plugins/debug/mod.rs b/src/plugins/debug/mod.rs index f8f1aca4..dc385003 100644 --- a/src/plugins/debug/mod.rs +++ b/src/plugins/debug/mod.rs @@ -281,6 +281,7 @@ fn debug_render_contacts( mut collisions: EventReader, mut debug_renderer: PhysicsDebugRenderer, config: Res, + time: Res>, ) { if config.contact_point_color.is_none() && config.contact_normal_color.is_none() { return; @@ -322,15 +323,30 @@ fn debug_render_contacts( // Draw contact normals if let Some(color) = config.contact_normal_color { + // Use dimmer color for second normal + let mut color_dim = color.as_hsla(); + color_dim.set_l(color_dim.l() * 0.5); + + // The length of the normal arrows + let length = match config.contact_normal_scale { + ContactGizmoScale::Constant(length) => length, + ContactGizmoScale::Scaled(scale) => { + scale * contacts.total_normal_impulse + / time.delta_seconds_f64().adjust_precision() + } + }; + #[cfg(feature = "2d")] { - debug_renderer.draw_arrow(p1, p1 + normal1 * 30.0, 8.0, 8.0, color); - debug_renderer.draw_arrow(p2, p2 + normal2 * 30.0, 8.0, 8.0, color); + debug_renderer.draw_arrow(p1, p1 + normal1 * length, 8.0, 8.0, color); + debug_renderer.draw_arrow(p2, p2 + normal2 * length, 8.0, 8.0, color); } + #[cfg(feature = "3d")] { - debug_renderer.draw_arrow(p1, p1 + normal1 * 0.5, 0.1, 0.1, color); - debug_renderer.draw_arrow(p2, p2 + normal2 * 0.5, 0.1, 0.1, color); + debug_renderer.draw_arrow(p1, p1 + normal1 * length, 0.1, 0.1, color); + + debug_renderer.draw_arrow(p2, p2 + normal2 * length, 0.1, 0.1, color_dim); } } } diff --git a/src/plugins/setup/mod.rs b/src/plugins/setup/mod.rs index a382639a..2c6c7db9 100644 --- a/src/plugins/setup/mod.rs +++ b/src/plugins/setup/mod.rs @@ -173,6 +173,7 @@ impl Plugin for PhysicsSetupPlugin { SubstepSet::SolveUserConstraints, SubstepSet::UpdateVelocities, SubstepSet::SolveVelocities, + SubstepSet::StoreImpulses, SubstepSet::ApplyTranslation, ) .chain(), diff --git a/src/plugins/solver.rs b/src/plugins/solver.rs index bb4cdd47..89352753 100644 --- a/src/plugins/solver.rs +++ b/src/plugins/solver.rs @@ -70,6 +70,8 @@ impl Plugin for SolverPlugin { .in_set(SubstepSet::SolveVelocities), ); + substeps.add_systems(store_contact_impulses.in_set(SubstepSet::StoreImpulses)); + substeps.add_systems(apply_translation.in_set(SubstepSet::ApplyTranslation)); } } @@ -167,15 +169,14 @@ fn penetration_constraints( .friction .unwrap_or(body1.friction) .combine(*collider2.friction.unwrap_or(body2.friction)); - let restitution_coefficient = collider1 + let restitution = collider1 .restitution .unwrap_or(body1.restitution) - .combine(*collider2.restitution.unwrap_or(body2.restitution)) - .coefficient; + .combine(*collider2.restitution.unwrap_or(body2.restitution)); // Create and solve penetration constraints for each contact. - for contact_manifold in contacts.manifolds.iter() { - for contact in contact_manifold.contacts.iter() { + for (manifold_index, manifold) in contacts.manifolds.iter().enumerate() { + for contact in manifold.contacts.iter() { // Add collider transforms to local contact points let contact = ContactData { point1: collider1.transform.map_or(contact.point1, |t| { @@ -194,10 +195,16 @@ fn penetration_constraints( }; let mut constraint = PenetrationConstraint { - dynamic_friction_coefficient: friction.dynamic_coefficient, - static_friction_coefficient: friction.static_coefficient, - restitution_coefficient, - ..PenetrationConstraint::new(&body1, &body2, contact) + friction, + restitution, + ..PenetrationConstraint::new( + &body1, + &body2, + *collider_entity1, + *collider_entity2, + contact, + manifold_index, + ) }; constraint.solve([&mut body1, &mut body2], delta_secs); penetration_constraints.0.push(constraint); @@ -422,13 +429,13 @@ fn update_ang_vel( #[allow(clippy::type_complexity)] fn solve_vel( mut bodies: Query>, - penetration_constraints: Res, + mut penetration_constraints: ResMut, gravity: Res, time: Res