Add pre_solve to DistanceJoint and fix Island solver

- Add pre_solve method to DistanceJoint for computing world anchors,
  direction, and Baumgarte bias before constraint solving
- Add hasattr checks in Island.solve to handle joints with optional
  solver methods
- Pass time_step to solve_velocity_constraints
- Remove redundant joint position correction loop from World.step
- Adjust contact separation threshold and reset normal_impulse when
  bodies are moving apart

Author: CipherJon

src/collision/contact.py

@@ -110,7 +110,7 @@ def resolve(self, dt):
         logger.info(f"Baumgarte bias: {bias:.4f}")
 
         # Calculate normal impulse scalar with warm starting
-        j = -(1 + e) * velocity_along_normal + bias
+        j = -(1 + e) * velocity_along_normal + bias  # direct + bias
         j += self.normal_impulse  # Warm starting
 
         # Remove or reduce min_impulse to prevent over-correction
@@ -129,8 +129,8 @@ def resolve(self, dt):
         j = max(j, 0.0)
 
         # Check if bodies are moving apart
-        if velocity_along_normal > 0.0:  # moving apart
-            logger.info("Bodies are moving apart, skipping impulse application")
+        if velocity_along_normal > 0.01:  # moving apart
+            self.normal_impulse = 0.0
             return 0.0
 
         # Apply normal impulse

src/constraints/distance.py

@@ -157,6 +157,27 @@ def solve_position_constraints(self):
             self.body2.inverse_inertia * self.anchor2.cross(impulse_vector)
         )
 
+    def pre_solve(self, time_step: float):
+        """
+        Prepare for solving (compute world anchors, bias, etc.).
+        """
+        # World anchors
+        self.world_anchor1 = self.body1.position + self.anchor1
+        self.world_anchor2 = self.body2.position + self.anchor2
+
+        # Direction and current length
+        self.direction = self.world_anchor2 - self.world_anchor1
+        self.current_length = (
+            self.direction.magnitude() if self.direction.magnitude() > 1e-6 else 1.0
+        )
+        self.direction = self.direction / self.current_length
+
+        # Bias for position error (Baumgarte)
+        position_error = self.current_length - self.length
+        self.bias = (
+            -(0.2 / time_step) * position_error * self.direction
+        )  # negative to correct
+
     def get_reaction_force(self, dt: float) -> Vec2:
         """
         Get the reaction force of the distance joint.

src/dynamics/island.py

@@ -66,18 +66,22 @@ def solve(self, time_step):
             time_step (float): The time step for the simulation.
         """
         for joint in self.joints:
-            joint.pre_solve(time_step)
+            if hasattr(joint, "pre_solve"):
+                joint.pre_solve(time_step)
 
         for _ in range(8):
             for contact in self.contacts:
                 contact.resolve()
 
             for joint in self.joints:
-                joint.solve_velocity_constraints()
+                if hasattr(joint, "solve_velocity_constraints"):
+                    joint.solve_velocity_constraints(time_step)
 
         for _ in range(3):
             for joint in self.joints:
-                joint.solve_position_constraints()
+                if hasattr(joint, "solve_position_constraints"):
+                    joint.solve_position_constraints()
 
         for joint in self.joints:
-            joint.post_solve()
+            if hasattr(joint, "post_solve"):
+                joint.post_solve()

src/dynamics/world.py

@@ -246,14 +246,6 @@ def step(
                 f"Total impulse applied this step: {total_impulse_magnitude:.4f}\n"
             )
 
-        # Correct positions (joints)
-        for _ in range(self.position_iterations):
-            for joint in self.joints:
-                if hasattr(joint, "pre_solve"):
-                    joint.pre_solve(self.time_step)
-                if hasattr(joint, "solve_position_constraints"):
-                    joint.solve_position_constraints()
-
     def _solve_contacts(self, collision_pairs, dt):
         """
         Solve contacts using the contact solver with persistence.