diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint.rs b/src/dynamics/solver/contact_constraint/one_body_constraint.rs index 5e6e86b3..951f09d6 100644 --- a/src/dynamics/solver/contact_constraint/one_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/one_body_constraint.rs @@ -411,6 +411,8 @@ impl OneBodyConstraint { active_contact.data.warmstart_tangent_impulse = self.elements[k].tangent_part.impulse; active_contact.data.impulse = self.elements[k].normal_part.total_impulse(); active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse(); + #[cfg(feature = "dim3")] + active_contact.data.set_tangent1(self.tangent1); } } diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs b/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs index 077dd750..47e9c863 100644 --- a/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs +++ b/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs @@ -412,6 +412,8 @@ impl OneBodyConstraintSimd { warmstart_tangent_impulses.extract(ii); active_contact.data.impulse = impulses[ii]; active_contact.data.tangent_impulse = tangent_impulses.extract(ii); + #[cfg(feature = "dim3")] + active_contact.data.set_tangent1(self.tangent1.extract(ii)); } } } diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint.rs b/src/dynamics/solver/contact_constraint/two_body_constraint.rs index d2a2c494..e4a501e2 100644 --- a/src/dynamics/solver/contact_constraint/two_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/two_body_constraint.rs @@ -481,6 +481,8 @@ impl TwoBodyConstraint { active_contact.data.warmstart_tangent_impulse = self.elements[k].tangent_part.impulse; active_contact.data.impulse = self.elements[k].normal_part.total_impulse(); active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse(); + #[cfg(feature = "dim3")] + active_contact.data.set_tangent1(self.tangent1); } } diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs b/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs index 2e2c68a5..3e26fed1 100644 --- a/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs +++ b/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs @@ -419,6 +419,8 @@ impl TwoBodyConstraintSimd { warmstart_tangent_impulses.extract(ii); active_contact.data.impulse = impulses[ii]; active_contact.data.tangent_impulse = tangent_impulses.extract(ii); + #[cfg(feature = "dim3")] + active_contact.data.set_tangent1(self.tangent1.extract(ii)); } } } diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index 3f569b20..249ce44d 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -38,12 +38,23 @@ pub struct ContactData { /// The friction impulse along the vector orthonormal to the contact normal, applied to the first /// collider's rigid-body. pub tangent_impulse: TangentImpulse, + #[cfg(feature = "dim3")] + /// One of the friction force directions. + pub tangent1: Vector, /// The impulse retained for warmstarting the next simulation step. pub warmstart_impulse: Real, /// The friction impulse retained for warmstarting the next simulation step. pub warmstart_tangent_impulse: TangentImpulse, } +impl ContactData { + #[cfg(feature = "dim3")] + #[inline] + pub(crate) fn set_tangent1(&mut self, tangent1: Vector) { + self.tangent1 = tangent1; + } +} + impl Default for ContactData { fn default() -> Self { Self { @@ -51,6 +62,8 @@ impl Default for ContactData { tangent_impulse: na::zero(), warmstart_impulse: 0.0, warmstart_tangent_impulse: na::zero(), + #[cfg(feature = "dim3")] + tangent1: na::zero(), } } } diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index ea0c7fbd..5530340c 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -20,6 +20,10 @@ pub use parry::query::{PointQuery, PointQueryWithLocation, RayCast, TrackedConta pub use parry::shape::SharedShape; use crate::math::{Real, Vector}; +#[cfg(feature = "dim3")] +use std::ops::MulAssign; +#[cfg(feature = "dim3")] +use std::ops::AddAssign; /// A contact between two colliders. pub type Contact = parry::query::TrackedContact; @@ -134,6 +138,9 @@ pub struct ContactForceEvent { pub collider2: ColliderHandle, /// The sum of all the forces between the two colliders. pub total_force: Vector, + #[cfg(feature = "dim3")] + /// The sum of all the tangent forces between the two colliders. + pub total_tangent_force: Vector, /// The sum of the magnitudes of each force between the two colliders. /// /// Note that this is **not** the same as the magnitude of `self.total_force`. @@ -158,8 +165,17 @@ impl ContactForceEvent { for m in &pair.manifolds { let mut total_manifold_impulse = 0.0; + #[cfg(feature = "dim3")] + let mut total_manifold_tangent_impulse = Vector::zeros(); for pt in m.contacts() { total_manifold_impulse += pt.data.impulse; + #[cfg(feature = "dim3")] + let tangents1 = [&pt.data.tangent1, &m.data.normal.cross(&pt.data.tangent1)]; + #[cfg(feature = "dim3")] + let tangent_impulse = tangents1[0] * pt.data.tangent_impulse[0] + + tangents1[1] * pt.data.tangent_impulse[1]; + #[cfg(feature = "dim3")] + total_manifold_tangent_impulse.add_assign(tangent_impulse); if pt.data.impulse > result.max_force_magnitude { result.max_force_magnitude = pt.data.impulse; @@ -168,6 +184,10 @@ impl ContactForceEvent { } result.total_force += m.data.normal * total_manifold_impulse; + #[cfg(feature = "dim3")] + result + .total_tangent_force + .add_assign(total_manifold_tangent_impulse); } let inv_dt = crate::utils::inv(dt); @@ -176,6 +196,8 @@ impl ContactForceEvent { // because it’s an input of this function already // assumed to be a force instead of an impulse. result.total_force *= inv_dt; + #[cfg(feature = "dim3")] + result.total_tangent_force.mul_assign(inv_dt); result.max_force_magnitude *= inv_dt; result }