Fix Line3 distance for parallel and non-unit directions (#224)

`Line3.distance()` raises `NameError` for parallel lines and can return
incorrect distances for skew or intersecting lines. For example:

```python
from spatialmath import Line3

a = Line3.PointDir([0, 0, 0], [1, 0, 0])
b = Line3.PointDir([0, 0, 2], [0.6, 0.8, 0])
print(a.distance(b))  # before: 2.5; expected: 2.0
```

The old expression divides by the squared cross-product norm and uses a
reciprocal product that mixes unit directions with unnormalized moments.
Scaling directions can therefore change the result; a translated
intersecting pair can also return a nonzero distance.

Compute the displacement between principal points and project it onto
the common normal of the unit directions. For parallel directions, use
the perpendicular component of that displacement and return a scalar.
Apply the existing `tol` parameter to the parallel-direction test.

Initial validation on base `f6a572c6e75e2a7e3c06e632bc34153941e00404`:

- Added regressions with the original implementation: **21 failed, 18
passed** in `tests/test_geom3d.py`; with the fix: **39 passed**.
- Full `tests` suite: baseline **346 passed, 3 skipped**; fixed **367
passed, 3 skipped**, on Python 3.10/NumPy 2.2.6 and Python 3.12/NumPy
2.5.3. Existing plotting/deprecation warnings remain.
- Separate seeded validation: **4,800 scalar comparisons per
environment** against closest points computed by `numpy.linalg.lstsq`,
including parallel/intersecting/skew lines, sign/scale changes, rigid
transformations and operand reversal.
- Pinned pre-commit hooks on both changed files, `git diff --check`,
wheel and sdist builds passed.

The branch is now rebased onto upstream `2de7801` (current head
`9600087`). On this updated tree, the complete `tests` suite passes with
**368 passed, 3 skipped** on Python 3.10.12, and the four pre-commit
hooks on both changed files and `git diff --check` pass. The newer
upstream commit adds a separate spatial-vector regression, accounting
for one more passing test than the initial run. GitHub Actions has since
run: all **10 platform/Python unittest jobs**, the aggregate test check,
and Codecov passed. The Sphinx documentation build succeeded with 22
warnings, but its final `gh-pages` push failed with HTTP 403 because
`github-actions[bot]` lacks write permission for this fork PR. This
patch changes only `spatialmath/geom3d.py` and `tests/test_geom3d.py`.

AI assistance: the implementation, tests and this description were
prepared with Codex; the local validation commands were executed
locally, and the multi-platform results above come from GitHub Actions.

Author: Doribelove

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