Add current_position getter for RigidBodyQueryItem

crates/bevy_xpbd_3d/examples/custom_constraint.rs

@@ -46,7 +46,7 @@ impl XpbdConstraint<2> for CustomDistanceConstraint {
         let [r1, r2] = [Vector::ZERO, Vector::ZERO];
 
         // Compute the positional difference
-        let delta_x = body1.position.0 - body2.position.0;
+        let delta_x = body1.current_position() - body2.current_position();
 
         // The current separation distance
         let length = delta_x.length();

src/components/world_queries.rs

@@ -64,6 +64,12 @@ impl<'w> RigidBodyQueryItem<'w> {
 
         inv_inertia
     }
+
+    /// Returns the current position of the body. This is a sum of the [`Position`] and
+    /// [`AccumulatedTranslation`] components.
+    pub fn current_position(&self) -> Vector {
+        self.position.0 + self.accumulated_translation.0
+    }
 }
 
 #[derive(WorldQuery)]

src/constraints/joints/distance.rs

@@ -122,8 +122,8 @@ impl DistanceJoint {
         let world_r2 = body2.rotation.rotate(self.local_anchor2);
 
         // // Compute the positional difference
-        let mut delta_x = (body1.position.0 + body1.accumulated_translation.0 + world_r1)
-            - (body2.position.0 + body2.accumulated_translation.0 + world_r2);
+        let mut delta_x =
+            (body1.current_position() + world_r1) - (body2.current_position() + world_r2);
 
         // The current separation distance
         let mut length = delta_x.length();
@@ -133,8 +133,8 @@ impl DistanceJoint {
                 return Vector::ZERO;
             }
             delta_x += limits.compute_correction(
-                body1.position.0 + body1.accumulated_translation.0 + world_r1,
-                body2.position.0 + body2.accumulated_translation.0 + world_r2,
+                body1.current_position() + world_r1,
+                body2.current_position() + world_r2,
             );
             length = delta_x.length();
         }
@@ -153,12 +153,8 @@ impl DistanceJoint {
         let n = delta_x / length;
 
         // Compute generalized inverse masses (method from PositionConstraint)
-        let w1 = <Self as PositionConstraint>::compute_generalized_inverse_mass(
-            self, body1, world_r1, n,
-        );
-        let w2 = <Self as PositionConstraint>::compute_generalized_inverse_mass(
-            self, body2, world_r2, n,
-        );
+        let w1 = PositionConstraint::compute_generalized_inverse_mass(self, body1, world_r1, n);
+        let w2 = PositionConstraint::compute_generalized_inverse_mass(self, body2, world_r2, n);
         let w = [w1, w2];
 
         // Constraint gradients, i.e. how the bodies should be moved

src/constraints/joints/mod.rs

@@ -148,8 +148,8 @@ pub trait Joint: Component + PositionConstraint + AngularConstraint {
         let world_r2 = body2.rotation.rotate(r2);
 
         let delta_x = DistanceLimit::new(0.0, 0.0).compute_correction(
-            body1.position.0 + body1.accumulated_translation.0 + world_r1,
-            body2.position.0 + body2.accumulated_translation.0 + world_r2,
+            body1.current_position() + world_r1,
+            body2.current_position() + world_r2,
         );
         let magnitude = delta_x.length();
 

src/constraints/joints/prismatic.rs

@@ -150,8 +150,8 @@ impl PrismaticJoint {
         let axis1 = body1.rotation.rotate(self.free_axis);
         if let Some(limits) = self.free_axis_limits {
             delta_x += limits.compute_correction_along_axis(
-                body1.position.0 + body1.accumulated_translation.0 + world_r1,
-                body2.position.0 + body2.accumulated_translation.0 + world_r2,
+                body1.current_position() + world_r1,
+                body2.current_position() + world_r2,
                 axis1,
             );
         }
@@ -162,8 +162,8 @@ impl PrismaticJoint {
         {
             let axis2 = Vector::new(axis1.y, -axis1.x);
             delta_x += zero_distance_limit.compute_correction_along_axis(
-                body1.position.0 + body1.accumulated_translation.0 + world_r1,
-                body2.position.0 + body2.accumulated_translation.0 + world_r2,
+                body1.current_position() + world_r1,
+                body2.current_position() + world_r2,
                 axis2,
             );
         }
@@ -173,13 +173,13 @@ impl PrismaticJoint {
             let axis3 = axis1.cross(axis2);
 
             delta_x += zero_distance_limit.compute_correction_along_axis(
-                body1.position.0 + body1.accumulated_translation.0 + world_r1,
-                body2.position.0 + body2.accumulated_translation.0 + world_r2,
+                body1.current_position() + world_r1,
+                body2.current_position() + world_r2,
                 axis2,
             );
             delta_x += zero_distance_limit.compute_correction_along_axis(
-                body1.position.0 + body1.accumulated_translation.0 + world_r1,
-                body2.position.0 + body2.accumulated_translation.0 + world_r2,
+                body1.current_position() + world_r1,
+                body2.current_position() + world_r2,
                 axis3,
             );
         }

src/constraints/penetration.rs

@@ -53,10 +53,8 @@ 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.r1);
-            let p2 =
-                body2.position.0 + body2.accumulated_translation.0 + body2.rotation.rotate(self.r2);
+            let p1 = body1.current_position() + body1.rotation.rotate(self.r1);
+            let p2 = body2.current_position() + body2.rotation.rotate(self.r2);
             self.contact.penetration = (p1 - p2).dot(self.contact.global_normal(&body1.rotation));
         }
 
@@ -144,12 +142,10 @@ impl PenetrationConstraint {
         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.r1);
-        let delta_p2 =
-            body2.position.0 - body2.previous_position.0 + body2.accumulated_translation.0 + r2
-                - body2.previous_rotation.rotate(self.r2);
+        let delta_p1 = body1.current_position() - body1.previous_position.0 + r1
+            - body1.previous_rotation.rotate(self.r1);
+        let delta_p2 = body2.current_position() - body2.previous_position.0 + 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;