diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint.rs b/src/dynamics/solver/contact_constraint/one_body_constraint.rs index 5e6e86b3..26436f28 100644 --- a/src/dynamics/solver/contact_constraint/one_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/one_body_constraint.rs @@ -411,6 +411,10 @@ 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.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..3a1643d6 100644 --- a/src/dynamics/solver/contact_constraint/two_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/two_body_constraint.rs @@ -481,6 +481,10 @@ 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.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..e2f3fc5c 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -38,6 +38,9 @@ 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. @@ -51,6 +54,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..79d5b449 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -134,6 +134,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`. @@ -160,6 +163,20 @@ impl ContactForceEvent { let mut total_manifold_impulse = 0.0; 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), + ]; + + let tangent_impulse = tangents1[0] * pt.data.tangent_impulse[0] + + tangents1[1] * pt.data.tangent_impulse[1]; + + log::debug!("from_contact_pair: tangent_impulse = {tangent_impulse}"); + + result.total_tangent_force += tangent_impulse; + } if pt.data.impulse > result.max_force_magnitude { result.max_force_magnitude = pt.data.impulse; @@ -176,6 +193,10 @@ 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 *= inv_dt; + } result.max_force_magnitude *= inv_dt; result }