From 3deefc8d9c0007f54372788a2617e1b71033c345 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=9D=8E=E6=B0=B8=E7=A5=BA?= Date: Sun, 27 Sep 2026 21:01:46 +0800 Subject: [PATCH 1/2] Make Line3 relation tests invariant to Plucker scale MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Normalize line directions and moments for parallel and intersection checks. Construct common perpendiculars from the nearest point and unit direction cross product, with regression coverage across Plucker scalings. Assisted-by: OpenAI Codex (GPT-6 Sol) Signed-off-by: 李永祺 --- spatialmath/geom3d.py | 33 ++++++++++++++++++---------- tests/test_geom3d.py | 50 +++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 72 insertions(+), 11 deletions(-) diff --git a/spatialmath/geom3d.py b/spatialmath/geom3d.py index edc4dc08..241c779d 100755 --- a/spatialmath/geom3d.py +++ b/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 diff --git a/tests/test_geom3d.py b/tests/test_geom3d.py index 81a50fdb..4e4a874c 100755 --- a/tests/test_geom3d.py +++ b/tests/test_geom3d.py @@ -207,6 +207,24 @@ 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]) + almost_parallel = Line3.PointDir([0, 0, 1], [1, 1e-9, 0]) + scaled = Line3.PointDir([0, 0, 1], [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 +241,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 +271,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 From 149653a3ccad5499a8047260198613ba3d620a89 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=9D=8E=E6=B0=B8=E7=A5=BA?= Date: Mon, 28 Sep 2026 15:27:48 +0800 Subject: [PATCH 2/2] test: avoid round-off in scaled Line3 fixture MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Keep the near-parallel tolerance test focused on direction vectors by constructing both lines through the origin. This avoids an unrelated Plucker orthogonality check failure on NumPy 2. Assisted-by: OpenAI Codex (GPT-6 Sol) Signed-off-by: 李永祺 --- tests/test_geom3d.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/tests/test_geom3d.py b/tests/test_geom3d.py index 4e4a874c..d8b6a4d0 100755 --- a/tests/test_geom3d.py +++ b/tests/test_geom3d.py @@ -220,8 +220,10 @@ def test_parallel_plucker_scale(self): # A loose angular tolerance must not shrink when Plucker coordinates grow. l1 = Line3.PointDir([0, 0, 0], [1, 0, 0]) - almost_parallel = Line3.PointDir([0, 0, 1], [1, 1e-9, 0]) - scaled = Line3.PointDir([0, 0, 1], [1e8, 0.1, 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))