From ca3777224755383dcb471517f436a816ae9bf956 Mon Sep 17 00:00:00 2001 From: Jondolf Date: Mon, 17 Jul 2023 22:04:12 +0300 Subject: [PATCH] Use world-space center of mass in penetration constraint Found by LeshaInc --- src/constraints/penetration.rs | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/constraints/penetration.rs b/src/constraints/penetration.rs index 6b08c94a..7a7ac3eb 100644 --- a/src/constraints/penetration.rs +++ b/src/constraints/penetration.rs @@ -55,8 +55,10 @@ impl XpbdConstraint<2> for PenetrationConstraint { impl PenetrationConstraint { /// Creates a new [`PenetrationConstraint`] with the given bodies and contact data. pub fn new(body1: &RigidBodyQueryItem, body2: &RigidBodyQueryItem, contact: Contact) -> Self { - let world_r1 = contact.point1 - body1.position.0 + body1.center_of_mass.0; - let world_r2 = contact.point2 - body2.position.0 + body2.center_of_mass.0; + let world_r1 = + contact.point1 - body1.position.0 + body1.rotation.rotate(body1.center_of_mass.0); + let world_r2 = + contact.point2 - body2.position.0 + body2.rotation.rotate(body2.center_of_mass.0); let local_r1 = body1.rotation.inverse().rotate(world_r1); let local_r2 = body2.rotation.inverse().rotate(world_r2);