Make Line3 relation tests invariant to Plücker scale

spatialmath/geom3d.py

@@ -693,7 +693,14 @@ def isparallel(
 
         :seealso: :meth:`__or__` :meth:`intersects`
         """
-        return bool(np.linalg.norm(np.cross(l1.w, l2.w)) < tol * _eps)
+        n1 = np.linalg.norm(l1.w)
+        n2 = np.linalg.norm(l2.w)
+        if n1 == 0 or n2 == 0:
+            return True
+
+        # Plucker coordinates are homogeneous: compare the angle, not the
+        # magnitude of the direction vectors.
+        return bool(np.linalg.norm(np.cross(l1.w / n1, l2.w / n2)) < tol * _eps)
 
     def isintersecting(
         l1, l2: Line3, tol: float = 20  # type: ignore
@@ -715,7 +722,15 @@ def isintersecting(
 
         :seealso: :meth:`__xor__` :meth:`intersects` :meth:`isparallel`
         """
-        return not l1.isparallel(l2, tol=tol) and bool(abs(l1 * l2) < tol * _eps)
+        if l1.isparallel(l2, tol=tol):
+            return False
+
+        n1 = np.linalg.norm(l1.w)
+        n2 = np.linalg.norm(l2.w)
+        # Normalize both moments as well as directions before applying the
+        # intersection tolerance; each pair (v, w) may be rescaled freely.
+        reciprocal = np.dot(l1.w / n1, l2.v / n2) + np.dot(l2.w / n2, l1.v / n1)
+        return bool(abs(reciprocal) < tol * _eps)
 
     def __eq__(l1, l2: Line3) -> bool:  # type: ignore pylint: disable=no-self-argument
         """
@@ -1009,16 +1024,12 @@ def commonperp(
         if l1 | l2:
             # no common perpendicular if lines are parallel
             return None
-        else:
-            # lines are skew or intersecting
-            w = np.cross(l1.w, l2.w)
-            v = (
-                np.cross(l1.v, l2.w)
-                - np.cross(l2.v, l1.w)
-                + (l1 * l2) * np.dot(l1.w, l2.w) * base.unitvec(np.cross(l1.w, l2.w))
-            )
 
-        return l1.__class__(v, w)
+        # The nearest point lies on both the common perpendicular and l1.
+        # Unit directions keep its direction independent of Plucker scaling.
+        point, _ = l1.closest_to_line(l2)
+        w = np.cross(l1.w / np.linalg.norm(l1.w), l2.w / np.linalg.norm(l2.w))
+        return l1.__class__.PointDir(point, w)
 
     def __mul__(
         left, right: Line3

tests/test_geom3d.py

@@ -207,6 +207,26 @@ def test_parallel(self):
         self.assertFalse(L1.isparallel(L3))
         self.assertFalse(L1 | L3)
 
+    def test_parallel_plucker_scale(self):
+        for scale1, scale2 in ((1, 1), (1e-8, 1e-8), (-1e-8, 1e-8)):
+            l1 = Line3.PointDir([0, 0, 0], [scale1, 0, 0])
+            perpendicular = Line3.PointDir([0, 0, 1], [0, scale2, 0])
+            parallel = Line3.PointDir([0, 0, 1], [scale2, 0, 0])
+
+            self.assertFalse(l1.isparallel(perpendicular))
+            self.assertFalse(l1 | perpendicular)
+            self.assertTrue(l1.isparallel(parallel))
+            self.assertTrue(l1 | parallel)
+
+        # A loose angular tolerance must not shrink when Plucker coordinates grow.
+        l1 = Line3.PointDir([0, 0, 0], [1, 0, 0])
+        # Only the directions matter here. A zero moment also avoids
+        # round-off in the constructor's Plucker orthogonality check.
+        almost_parallel = Line3.PointDir([0, 0, 0], [1, 1e-9, 0])
+        scaled = Line3.PointDir([0, 0, 0], [1e8, 0.1, 0])
+        self.assertTrue(l1.isparallel(almost_parallel, tol=1e8))
+        self.assertTrue(l1.isparallel(scaled, tol=1e8))
+
     def test_intersect(self):
         L1 = Line3.PointDir([4, 5, 6], [1, 2, 3])
         L2 = Line3.PointDir([5, 5, 6], [1, 2, 3])
@@ -223,6 +243,22 @@ def test_intersect(self):
             L3 ^ L4,
         )
 
+    def test_intersect_plucker_scale(self):
+        for scale in (1, 1e-8, 1e8):
+            l1 = Line3.PointDir([0, 0, 0], [scale, 0, 0])
+            crossing = Line3.PointDir([0, 0, 0], [0, scale, 0])
+            skew = Line3.PointDir([0, 0, 1e-7], [0, scale, 0])
+
+            self.assertTrue(l1.isintersecting(crossing))
+            self.assertTrue(l1 ^ crossing)
+            self.assertFalse(l1.isintersecting(skew))
+            self.assertFalse(l1 ^ skew)
+
+        # Tolerance on the miss distance is independent of direction magnitude.
+        l1 = Line3.PointDir([0, 0, 0], [1e8, 0, 0])
+        near = Line3.PointDir([0, 0, 1e-8], [0, 1e8, 0])
+        self.assertTrue(l1.isintersecting(near, tol=1e9))
+
     def test_commonperp(self):
         L1 = Line3.PointDir([4, 5, 6], [0, 0, 1])
         L2 = Line3.PointDir([6, 5, 6], [0, 1, 0])
@@ -237,6 +273,22 @@ def test_commonperp(self):
         self.assertTrue(L ^ L1)
         self.assertTrue(L ^ L2)
 
+    def test_commonperp_plucker_scale(self):
+        for scale1, scale2 in ((1, 1), (1e-8, 1e-8), (-1e-8, 1e8)):
+            l1 = Line3.PointDir([1, 2, 3], [scale1, 0, 0])
+            l2 = Line3.PointDir([0, 2, 4], [scale2, scale2, 0])
+
+            common = l1.commonperp(l2)
+            self.assertIsNotNone(common)
+            nt.assert_allclose(common.pp, [0, 2, 0], atol=1e-12)
+            self.assertAlmostEqual(abs(common.uw[2]), 1)
+            self.assertTrue(common.isintersecting(l1))
+            self.assertTrue(common.isintersecting(l2))
+
+            nearest, distance = l1.closest_to_line(l2)
+            nt.assert_allclose(nearest, [0, 2, 3], atol=1e-12)
+            self.assertAlmostEqual(distance, 1)
+
     def test_line(self):
         # mindist
         # intersect