Fix DistanceJoint distance limits (#286)

# Objective

Currently, the distance limits in `DistanceJoint` don't actually work, and it always just uses the rest length instead.

## Solution

If distance limits are specified, use them, otherwise use the rest length. Eventually, we should probably just remove the rest length in favor of only the min/max distance limits.

I also managed to clean up *and* optimize the limit handling a bit by making `DistanceLimit::compute_correction` return the correction direction and magnitude separately.

crates/bevy_xpbd_3d/Cargo.toml

@@ -82,6 +82,10 @@ required-features = ["3d"]
 name = "custom_constraint"
 required-features = ["3d"]
 
+[[example]]
+name = "distance_joint_3d"
+required-features = ["3d", "debug-plugin"]
+
 [[example]]
 name = "fixed_joint_3d"
 required-features = ["3d"]

crates/bevy_xpbd_3d/examples/distance_joint_3d.rs

@@ -3,7 +3,11 @@ use bevy_xpbd_3d::{math::*, prelude::*};
 
 fn main() {
     App::new()
-        .add_plugins((DefaultPlugins, PhysicsPlugins::default()))
+        .add_plugins((
+            DefaultPlugins,
+            PhysicsPlugins::default(),
+            PhysicsDebugPlugin::default(),
+        ))
         .add_systems(Startup, setup)
         .run();
 }
@@ -16,7 +20,7 @@ fn setup(
     let cube_mesh = meshes.add(Mesh::from(shape::Cube { size: 1.0 }));
     let cube_material = materials.add(Color::rgb(0.8, 0.7, 0.6).into());
 
-    // Spawn a static cube and a dynamic cube that is outside of the rest length.
+    // Spawn a static cube and a dynamic cube that is connected to it by a distance joint.
     let static_cube = commands
         .spawn((
             PbrBundle {
@@ -33,7 +37,7 @@ fn setup(
             PbrBundle {
                 mesh: cube_mesh,
                 material: cube_material,
-                transform: Transform::from_xyz(0.0, -2.5, 0.0),
+                transform: Transform::from_xyz(-2.0, -0.5, 0.0),
                 ..default()
             },
             RigidBody::Dynamic,
@@ -43,16 +47,11 @@ fn setup(
         .id();
 
     // Add a distance joint to keep the cubes at a certain distance from each other.
-    // The dynamic cube should bounce like it's on a spring.
     commands.spawn(
         DistanceJoint::new(static_cube, dynamic_cube)
-            .with_local_anchor_1(0.5 * Vector::NEG_Y)
-            .with_local_anchor_2(0.5 * Vector::new(1.0, 1.0, 1.0))
+            .with_local_anchor_2(0.5 * Vector::ONE)
             .with_rest_length(1.5)
-            .with_limits(0.75, 2.5)
-            // .with_linear_velocity_damping(0.1)
-            // .with_angular_velocity_damping(1.0)
-            .with_compliance(1.0 / 100.0),
+            .with_compliance(1.0 / 400.0),
     );
 
     // Light

src/constraints/joints/distance.rs

@@ -129,56 +129,49 @@ impl DistanceJoint {
         let world_r1 = body1.rotation.rotate(self.local_anchor1);
         let world_r2 = body2.rotation.rotate(self.local_anchor2);
 
-        // // Compute the positional difference
-        let mut delta_x =
-            (body1.current_position() + world_r1) - (body2.current_position() + world_r2);
-
-        // The current separation distance
-        let mut length = delta_x.length();
-
-        if let Some(limits) = self.length_limits {
-            if length < Scalar::EPSILON {
-                return Vector::ZERO;
-            }
-            delta_x += limits.compute_correction(
-                body1.current_position() + world_r1,
-                body2.current_position() + world_r2,
-            );
-            length = delta_x.length();
-        }
-
-        // The value of the constraint function. When this is zero, the
-        // constraint is satisfied, and the distance between the bodies is the
-        // rest length.
-        let c = length - self.rest_length;
-
-        // Avoid division by zero and unnecessary computation.
-        if length < Scalar::EPSILON || c.abs() < Scalar::EPSILON {
+        // If min and max limits aren't specified, use rest length
+        // TODO: Remove rest length, just use min/max limits.
+        let limits = self
+            .length_limits
+            .unwrap_or(DistanceLimit::new(self.rest_length, self.rest_length));
+
+        // Compute the direction and magnitude of the positional correction required
+        // to keep the bodies within a certain distance from each other.
+        let (dir, distance) = limits.compute_correction(
+            body1.current_position() + world_r1,
+            body2.current_position() + world_r2,
+        );
+
+        // Avoid division by zero and unnecessary computation
+        if distance.abs() < Scalar::EPSILON {
             return Vector::ZERO;
         }
 
-        // Normalized delta_x
-        let n = delta_x / length;
-
         // Compute generalized inverse masses (method from PositionConstraint)
-        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 w1 = PositionConstraint::compute_generalized_inverse_mass(self, body1, world_r1, dir);
+        let w2 = PositionConstraint::compute_generalized_inverse_mass(self, body2, world_r2, dir);
         let w = [w1, w2];
 
         // Constraint gradients, i.e. how the bodies should be moved
         // relative to each other in order to satisfy the constraint
-        let gradients = [n, -n];
+        let gradients = [dir, -dir];
 
         // Compute Lagrange multiplier update, essentially the signed magnitude of the correction
-        let delta_lagrange =
-            self.compute_lagrange_update(self.lagrange, c, &gradients, &w, self.compliance, dt);
+        let delta_lagrange = self.compute_lagrange_update(
+            self.lagrange,
+            distance,
+            &gradients,
+            &w,
+            self.compliance,
+            dt,
+        );
         self.lagrange += delta_lagrange;
 
         // Apply positional correction (method from PositionConstraint)
-        self.apply_positional_correction(body1, body2, delta_lagrange, n, world_r1, world_r2);
+        self.apply_positional_correction(body1, body2, delta_lagrange, dir, world_r1, world_r2);
 
         // Return constraint force
-        self.compute_force(self.lagrange, n, dt)
+        self.compute_force(self.lagrange, dir, dt)
     }
 
     /// Sets the minimum and maximum distances between the attached bodies.

src/constraints/joints/mod.rs

@@ -143,18 +143,15 @@ pub trait Joint: Component + PositionConstraint + AngularConstraint {
         let world_r1 = body1.rotation.rotate(r1);
         let world_r2 = body2.rotation.rotate(r2);
 
-        let delta_x = DistanceLimit::new(0.0, 0.0).compute_correction(
+        let (dir, magnitude) = DistanceLimit::new(0.0, 0.0).compute_correction(
             body1.current_position() + world_r1,
             body2.current_position() + world_r2,
         );
-        let magnitude = delta_x.length();
 
         if magnitude <= Scalar::EPSILON {
             return Vector::ZERO;
         }
 
-        let dir = delta_x / magnitude;
-
         // Compute generalized inverse masses
         let w1 = PositionConstraint::compute_generalized_inverse_mass(self, body1, world_r1, dir);
         let w2 = PositionConstraint::compute_generalized_inverse_mass(self, body2, world_r2, dir);
@@ -247,30 +244,30 @@ impl DistanceLimit {
         Self { min, max }
     }
 
-    /// Returns the positional correction required to limit the distance between `p1` and `p2` to be
-    /// to be inside the distance limit.
-    pub fn compute_correction(&self, p1: Vector, p2: Vector) -> Vector {
+    /// Returns the direction and magnitude of the positional correction required
+    /// to limit the distance between `p1` and `p2` to be within the distance limit.
+    pub fn compute_correction(&self, p1: Vector, p2: Vector) -> (Vector, Scalar) {
         let pos_offset = p2 - p1;
         let distance = pos_offset.length();
 
         if distance <= Scalar::EPSILON {
-            return Vector::ZERO;
+            return (Vector::ZERO, 0.0);
         }
 
         // Equation 25
         if distance < self.min {
             // Separation distance lower limit
-            -pos_offset / distance * (distance - self.min)
+            (-pos_offset / distance, (distance - self.min))
         } else if distance > self.max {
             // Separation distance upper limit
-            -pos_offset / distance * (distance - self.max)
+            (-pos_offset / distance, (distance - self.max))
         } else {
-            Vector::ZERO
+            (Vector::ZERO, 0.0)
         }
     }
 
     /// Returns the positional correction required to limit the distance between `p1` and `p2`
-    /// to be inside the distance limit along a given `axis`.
+    /// to be within the distance limit along a given `axis`.
     fn compute_correction_along_axis(&self, p1: Vector, p2: Vector, axis: Vector) -> Vector {
         let pos_offset = p2 - p1;
         let a = pos_offset.dot(axis);
@@ -311,7 +308,7 @@ impl AngleLimit {
     }
 
     /// Returns the angular correction required to limit the angle between the axes `n1` and `n2`
-    /// to be inside the angle limits.
+    /// to be within the angle limits.
     fn compute_correction(
         &self,
         n: Vector3,