Skip to content

Commit

Permalink
tangent_force = ok
Browse files Browse the repository at this point in the history
  • Loading branch information
GhostMinerPlus committed Dec 21, 2024
1 parent 21ab120 commit 14cc870
Show file tree
Hide file tree
Showing 6 changed files with 36 additions and 0 deletions.
4 changes: 4 additions & 0 deletions src/dynamics/solver/contact_constraint/one_body_constraint.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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));
}
}
}
Expand Down
4 changes: 4 additions & 0 deletions src/dynamics/solver/contact_constraint/two_body_constraint.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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));
}
}
}
Expand Down
5 changes: 5 additions & 0 deletions src/geometry/contact_pair.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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<Real>,
#[cfg(feature = "dim3")]
/// One of the friction force directions.
pub tangent1: Vector<Real>,
/// The impulse retained for warmstarting the next simulation step.
pub warmstart_impulse: Real,
/// The friction impulse retained for warmstarting the next simulation step.
Expand All @@ -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(),
}
}
}
Expand Down
19 changes: 19 additions & 0 deletions src/geometry/mod.rs
Original file line number Diff line number Diff line change
Expand Up @@ -134,6 +134,9 @@ pub struct ContactForceEvent {
pub collider2: ColliderHandle,
/// The sum of all the forces between the two colliders.
pub total_force: Vector<Real>,
#[cfg(feature = "dim3")]
/// The sum of all the tangent forces between the two colliders.
pub total_tangent_force: Vector<Real>,
/// 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`.
Expand All @@ -160,6 +163,18 @@ 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.warmstart_tangent_impulse[0]
+ tangents1[1] * pt.data.warmstart_tangent_impulse[1];

result.total_tangent_force += tangent_impulse;
}

if pt.data.impulse > result.max_force_magnitude {
result.max_force_magnitude = pt.data.impulse;
Expand All @@ -176,6 +191,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
}
Expand Down

0 comments on commit 14cc870

Please sign in to comment.