From 47f6f2af3d9a7f4cd6cdbb2357264524d8ce6d64 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E6=9D=8E=E6=B0=B8=E7=A5=BA?= <doribelove@gmail.com>
Date: Wed, 16 Sep 2026 16:08:08 +0800
Subject: [PATCH] Fix Line3 distance for parallel and non-unit directions

Project principal-point displacement onto the common normal of unit
directions, with a scalar perpendicular-distance fallback for parallel
lines. Cover intersecting, coincident and skew lines, direction rescaling,
operand reversal and the parallel tolerance.

AI assistance: prepared with Codex; validation executed locally.
---
 spatialmath/geom3d.py | 26 +++++++++++---------------
 tests/test_geom3d.py  | 30 ++++++++++++++++++++++++++++++
 2 files changed, 41 insertions(+), 15 deletions(-)

diff --git a/spatialmath/geom3d.py b/spatialmath/geom3d.py
index edc4dc08..17f54a76 100755
--- a/spatialmath/geom3d.py
+++ b/spatialmath/geom3d.py
@@ -836,7 +836,7 @@ def distance(
 
         :param l2: Second line
         :type l2: ``Line3``
-        :param tol: Tolerance in multiples of eps, defaults to 20
+        :param tol: Parallel-direction tolerance in multiples of eps, defaults to 20
         :type tol: float, optional
         :return: Closest distance between lines
         :rtype: float
@@ -847,20 +847,16 @@ def distance(
 
         :seealso: :meth:`closest_to_line`
         """
-        if l1 | l2:
-            # lines are parallel
-            l = np.cross(
-                l1.w, l1.v - l2.v * np.dot(l1.w, l2.w) / dot(l2.w, l2.w)
-            ) / np.linalg.norm(l1.w)
-        else:
-            # lines are not parallel
-            if abs(l1 * l2) < tol * _eps:
-                # lines intersect at a point
-                l = 0
-            else:
-                # lines don't intersect, find closest distance
-                l = abs(l1 * l2) / np.linalg.norm(np.cross(l1.w, l2.w)) ** 2
-        return l
+        w1, w2 = l1.uw, l2.uw
+        normal = np.cross(w1, w2)
+        normal_length = np.linalg.norm(normal)
+        delta = l2.pp - l1.pp
+        if normal_length <= tol * _eps:
+            # Parallel lines: remove the component along the common direction.
+            return float(np.linalg.norm(np.cross(delta, w1)))
+        # The displacement along the common normal is the shortest distance.
+        # Unit directions make the result independent of Plucker scaling.
+        return float(abs(np.dot(delta, normal)) / normal_length)
 
     def closest_to_line(
         l1, l2: Line3  # type:ignore
diff --git a/tests/test_geom3d.py b/tests/test_geom3d.py
index 81a50fdb..bc6f2e63 100755
--- a/tests/test_geom3d.py
+++ b/tests/test_geom3d.py
@@ -17,6 +17,36 @@
 import matplotlib.pyplot as plt
 
 
+@pytest.mark.parametrize(
+    "p1, w1, p2, w2, expected",
+    [
+        ([0, 0, 0], [1, 0, 0], [0, 3, 4], [1, 0, 0], 5),
+        ([1, 2, 3], [1, 0, 0], [4, 2, 3], [-1, 0, 0], 0),
+        ([1, 2, 3], [2, 0, 0], [1, 2, 3], [0, 5, 0], 0),
+        ([0, 0, 0], [1, 0, 0], [0, 0, 2], [0.6, 0.8, 0], 2),
+        ([1, 2, 3], [2, 0, 0], [1, 2, 5], [3, 4, 0], 2),
+    ],
+    ids=["parallel", "coincident", "intersecting", "skew-unit", "skew-scaled"],
+)
+@pytest.mark.parametrize("scale1, scale2", [(1, 1), (2, 5), (-2, 0.25), (1e-8, 1e-8)])
+def test_line_distance(p1, w1, p2, w2, expected, scale1, scale2):
+    # Scaling either set of Plucker coordinates does not change the line.
+    line1 = Line3.PointDir(p1, np.array(w1) * scale1)
+    line2 = Line3.PointDir(p2, np.array(w2) * scale2)
+    for first, second in [(line1, line2), (line2, line1)]:
+        distance = first.distance(second)
+        assert np.isscalar(distance)
+        assert distance == pytest.approx(expected, abs=1e-12)
+
+
+def test_line_distance_parallel_tolerance():
+    line1 = Line3.PointDir([0, 0, 0], [1, 0, 0])
+    line2 = Line3.PointDir([0, 1, 2], [1, 1e-8, 0])
+    assert line1.distance(line2) == pytest.approx(2)
+    # A larger angular tolerance treats the directions as parallel.
+    assert line1.distance(line2, tol=1e8) == pytest.approx(np.sqrt(5))
+
+
 class Line3Test(unittest.TestCase):
     # Primitives
     def test_constructor1(self):