From 185d854c4c9ea4d418a55d705b19c629a24c004c Mon Sep 17 00:00:00 2001 From: ghostminer <1696415745@qq.com> Date: Sun, 22 Dec 2024 00:46:21 +0800 Subject: [PATCH] tangent_force = ok --- .../contact_constraint/one_body_constraint.rs | 4 ++++ .../one_body_constraint_simd.rs | 2 ++ .../contact_constraint/two_body_constraint.rs | 4 ++++ .../two_body_constraint_simd.rs | 2 ++ src/geometry/contact_pair.rs | 5 +++++ src/geometry/mod.rs | 21 +++++++++++++++++++ 6 files changed, 38 insertions(+) diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint.rs b/src/dynamics/solver/contact_constraint/one_body_constraint.rs index 5e6e86b3c..26436f283 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 077dd750c..47e9c863d 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 d2a2c4940..3a1643d64 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 2e2c68a55..3e26fed1a 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 3f569b200..e2f3fc5c4 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 ea0c7fbdd..779f9334d 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), + ]; + + log::debug!("from_contact_pair: tangents1 = {tangents1:?}, pt.data.tangent_impulse = {}", pt.data.tangent_impulse); + + let tangent_impulse = tangents1[0] * pt.data.tangent_impulse[0] + + tangents1[1] * pt.data.tangent_impulse[1]; + + 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 }