Fix Line3 distance for parallel and non-unit directions

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

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):