From adb2b07a868946033d32d8c86515141c96dc7f6c Mon Sep 17 00:00:00 2001 From: Jondolf Date: Wed, 26 Jul 2023 17:53:04 +0300 Subject: [PATCH] Use local normals transformed into world-space in penetration constraints The normals should be updated at every solve. This significantly reduces positional drift. The contact normals and contact points are now stored in local space, and `ContactData` has helper methods for conversion to global space. --- src/collision.rs | 48 +++++++++++++++--------- src/constraints/penetration.rs | 67 ++++++++++++++++++---------------- src/plugins/debug/mod.rs | 12 +++++- src/plugins/solver.rs | 6 +-- 4 files changed, 78 insertions(+), 55 deletions(-) diff --git a/src/collision.rs b/src/collision.rs index 2e6d11d3..15e14724 100644 --- a/src/collision.rs +++ b/src/collision.rs @@ -43,7 +43,8 @@ pub struct ContactManifold { pub entity2: Entity, /// The contacts in this manifold. pub contacts: Vec, - /// A world-space contact normal shared by all contacts in this manifold. + /// A contact normal shared by all contacts in this manifold, + /// expressed in the local space of the first entity. pub normal: Vector, } @@ -51,14 +52,10 @@ pub struct ContactManifold { #[derive(Clone, Copy, Debug, PartialEq)] pub struct ContactData { /// Contact point on the first entity in local coordinates. - pub local_point1: Vector, - /// Contact point on the second entity in local coordinates. - pub local_point2: Vector, - /// Contact point on the first entity in global coordinates. pub point1: Vector, - /// Contact point on the second entity in global coordinates. + /// Contact point on the second entity in local coordinates. pub point2: Vector, - /// Contact normal from contact point 1 to 2 in world coordinates. + /// A contact normal expressed in the local space of the first entity. pub normal: Vector, /// Penetration depth. pub penetration: Scalar, @@ -67,6 +64,25 @@ pub struct ContactData { pub(crate) convex: bool, } +impl ContactData { + /// Returns the global contact point on the first entity, + /// transforming the local point by the given entity position and rotation. + pub fn global_point1(&self, position: &Position, rotation: &Rotation) -> Vector { + position.0 + rotation.rotate(self.point1) + } + + /// Returns the global contact point on the second entity, + /// transforming the local point by the given entity position and rotation. + pub fn global_point2(&self, position: &Position, rotation: &Rotation) -> Vector { + position.0 + rotation.rotate(self.point2) + } + + /// Returns the world-space contact normal pointing towards the exterior of the first entity. + pub fn global_normal(&self, rotation: &Rotation) -> Vector { + rotation.rotate(self.normal) + } +} + /// Computes one pair of contact points between two shapes. #[allow(clippy::too_many_arguments)] pub(crate) fn compute_contacts( @@ -103,16 +119,14 @@ pub(crate) fn compute_contacts( .map(|manifold| ContactManifold { entity1, entity2, - normal: rotation1.rotate(manifold.local_n1.into()), + normal: manifold.local_n1.into(), contacts: manifold .contacts() .iter() .map(|contact| ContactData { - local_point1: contact.local_p1.into(), - local_point2: contact.local_p2.into(), - point1: position1 + rotation1.rotate(contact.local_p1.into()), - point2: position2 + rotation2.rotate(contact.local_p2.into()), - normal: rotation1.rotate(manifold.local_n1.into()), + point1: contact.local_p1.into(), + point2: contact.local_p2.into(), + normal: manifold.local_n1.into(), penetration: -contact.dist, convex, }) @@ -134,11 +148,9 @@ pub(crate) fn compute_contacts( ) .unwrap() .map(|contact| ContactData { - local_point1: contact.point1.into(), - local_point2: contact.point2.into(), - point1: position1 + rotation1.rotate(contact.point1.into()), - point2: position2 + rotation2.rotate(contact.point2.into()), - normal: rotation1.rotate(contact.normal1.into()), + point1: contact.point1.into(), + point2: contact.point2.into(), + normal: contact.normal1.into(), penetration: -contact.dist, convex, }); diff --git a/src/constraints/penetration.rs b/src/constraints/penetration.rs index e8883691..dba01b19 100644 --- a/src/constraints/penetration.rs +++ b/src/constraints/penetration.rs @@ -15,13 +15,9 @@ pub struct PenetrationConstraint { /// Data associated with the contact. pub contact: ContactData, /// Vector from the first entity's center of mass to the contact point in local coordinates. - pub local_r1: Vector, + pub r1: Vector, /// Vector from the second entity's center of mass to the contact point in local coordinates. - pub local_r2: Vector, - /// Vector from the first entity's center of mass to the contact position in world coordinates. - pub world_r1: Vector, - /// Vector from the second entity's center of mass to the contact position in world coordinates. - pub world_r2: Vector, + pub r2: Vector, /// Lagrange multiplier for the normal force. pub normal_lagrange: Scalar, /// Lagrange multiplier for the tangential force. @@ -57,13 +53,11 @@ impl XpbdConstraint<2> for PenetrationConstraint { // Todo: Figure out why this is and use the method below for all collider types in order to fix // explosions for all contacts. if self.contact.convex { - let p1 = body1.position.0 - + body1.accumulated_translation.0 - + body1.rotation.rotate(self.local_r1); - let p2 = body2.position.0 - + body2.accumulated_translation.0 - + body2.rotation.rotate(self.local_r2); - self.contact.penetration = (p1 - p2).dot(self.contact.normal); + let p1 = + body1.position.0 + body1.accumulated_translation.0 + body1.rotation.rotate(self.r1); + let p2 = + body2.position.0 + body2.accumulated_translation.0 + body2.rotation.rotate(self.r2); + self.contact.penetration = (p1 - p2).dot(self.contact.global_normal(&body1.rotation)); } // If penetration depth is under 0, skip the collision @@ -72,6 +66,20 @@ impl XpbdConstraint<2> for PenetrationConstraint { } self.solve_contact(body1, body2, dt); + + if self.contact.convex { + let p1 = + body1.position.0 + body1.accumulated_translation.0 + body1.rotation.rotate(self.r1); + let p2 = + body2.position.0 + body2.accumulated_translation.0 + body2.rotation.rotate(self.r2); + self.contact.penetration = (p1 - p2).dot(self.contact.global_normal(&body1.rotation)); + } + + // If penetration depth is under 0, skip the collision + if self.contact.penetration <= Scalar::EPSILON { + return; + } + self.solve_friction(body1, body2, dt); } } @@ -83,20 +91,15 @@ impl PenetrationConstraint { body2: &RigidBodyQueryItem, contact: ContactData, ) -> Self { - let local_r1 = contact.local_point1 - body1.center_of_mass.0; - let local_r2 = contact.local_point2 - body2.center_of_mass.0; - - let world_r1 = body1.rotation.rotate(local_r1); - let world_r2 = body2.rotation.rotate(local_r2); + let r1 = contact.point1 - body1.center_of_mass.0; + let r2 = contact.point2 - body2.center_of_mass.0; Self { entity1: body1.entity, entity2: body2.entity, contact, - local_r1, - local_r2, - world_r1, - world_r2, + r1, + r2, normal_lagrange: 0.0, tangent_lagrange: 0.0, compliance: 0.0, @@ -113,12 +116,12 @@ impl PenetrationConstraint { dt: Scalar, ) { // Shorter aliases - let penetration = self.contact.penetration; - let normal = self.contact.normal; let compliance = self.compliance; let lagrange = self.normal_lagrange; - let r1 = body1.rotation.rotate(self.local_r1); - let r2 = body2.rotation.rotate(self.local_r2); + let penetration = self.contact.penetration; + let normal = self.contact.global_normal(&body1.rotation); + let r1 = body1.rotation.rotate(self.r1); + let r2 = body2.rotation.rotate(self.r2); // Compute generalized inverse masses let w1 = self.compute_generalized_inverse_mass(body1, r1, normal); @@ -147,20 +150,20 @@ impl PenetrationConstraint { dt: Scalar, ) { // Shorter aliases - let penetration = self.contact.penetration; - let normal = self.contact.normal; let compliance = self.compliance; let lagrange = self.tangent_lagrange; - let r1 = body1.rotation.rotate(self.local_r1); - let r2 = body2.rotation.rotate(self.local_r2); + let penetration = self.contact.penetration; + let normal = self.contact.global_normal(&body1.rotation); + let r1 = body1.rotation.rotate(self.r1); + let r2 = body2.rotation.rotate(self.r2); // Compute relative motion of the contact points and get the tangential component let delta_p1 = body1.position.0 - body1.previous_position.0 + body1.accumulated_translation.0 + r1 - - body1.previous_rotation.rotate(self.local_r1); + - body1.previous_rotation.rotate(self.r1); let delta_p2 = body2.position.0 - body2.previous_position.0 + body2.accumulated_translation.0 + r2 - - body2.previous_rotation.rotate(self.local_r2); + - body2.previous_rotation.rotate(self.r2); let delta_p = delta_p1 - delta_p2; let delta_p_tangent = delta_p - delta_p.dot(normal) * normal; diff --git a/src/plugins/debug/mod.rs b/src/plugins/debug/mod.rs index 165e0457..e3dfefd7 100644 --- a/src/plugins/debug/mod.rs +++ b/src/plugins/debug/mod.rs @@ -139,6 +139,7 @@ fn debug_render_aabbs( } fn debug_render_contacts( + colliders: Query<(&Position, &Rotation), With>, mut collisions: EventReader, mut debug_renderer: PhysicsDebugRenderer, config: Res, @@ -147,10 +148,17 @@ fn debug_render_contacts( return; }; for Collision(contacts) in collisions.iter() { + let Ok((position1, rotation1)) = colliders.get(contacts.entity1) else { + continue; + }; + let Ok((position2, rotation2)) = colliders.get(contacts.entity2) else { + continue; + }; + for manifold in contacts.manifolds.iter() { for contact in manifold.contacts.iter() { - let p1 = contact.point1; - let p2 = contact.point2; + let p1 = contact.global_point1(position1, rotation1); + let p2 = contact.global_point2(position2, rotation2); #[cfg(feature = "2d")] let len = 5.0; #[cfg(feature = "3d")] diff --git a/src/plugins/solver.rs b/src/plugins/solver.rs index 523f1f19..6d3d0091 100644 --- a/src/plugins/solver.rs +++ b/src/plugins/solver.rs @@ -382,9 +382,9 @@ fn solve_vel( continue; } - let normal = constraint.contact.normal; - let r1 = body1.rotation.rotate(constraint.local_r1); - let r2 = body2.rotation.rotate(constraint.local_r2); + let normal = constraint.contact.global_normal(&body1.rotation); + let r1 = body1.rotation.rotate(constraint.r1); + let r2 = body2.rotation.rotate(constraint.r2); // Compute pre-solve relative normal velocities at the contact point (used for restitution) let pre_solve_contact_vel1 = compute_contact_vel(