From 694b993ced2c7deb514417c4f41f4dd30c41ba89 Mon Sep 17 00:00:00 2001
From: xixihaha-ha <182837830+xixihaha-ha@users.noreply.github.com>
Date: Fri, 6 Nov 2026 11:40:32 +0800
Subject: [PATCH] Make line-plane intersection tolerance independent of
 coordinate scale

---
 spatialmath/geom3d.py |  2 +-
 tests/test_geom3d.py  | 34 ++++++++++++++++++++++++++++++++++
 2 files changed, 35 insertions(+), 1 deletion(-)

diff --git a/spatialmath/geom3d.py b/spatialmath/geom3d.py
index c1efbbb2..894813dc 100644
--- a/spatialmath/geom3d.py
+++ b/spatialmath/geom3d.py
@@ -1122,7 +1122,7 @@ def intersect_plane(
 
         den = np.dot(self.w, plane.n)
 
-        if abs(den) > (tol * _eps):
+        if abs(den) > tol * _eps * np.linalg.norm(self.w) * np.linalg.norm(plane.n):
             # P = -(np.cross(line.v, plane.n) + plane.d * line.w) / den
             p = (np.cross(self.v, plane.n) - plane.d * self.w) / den
 
diff --git a/tests/test_geom3d.py b/tests/test_geom3d.py
index ca1675dd..1c7754c7 100755
--- a/tests/test_geom3d.py
+++ b/tests/test_geom3d.py
@@ -85,6 +85,40 @@ def test_line_intersection_none(point, direction):
 
 
 class Line3Test(unittest.TestCase):
+    def test_intersect_plane_coordinate_scale(self):
+        for line_scale, plane_scale in (
+            (1, 1),
+            (1e-8, 1e-8),
+            (-1e-8, 1e-8),
+            (1e-8, -1e-8),
+            (1e8, 1e8),
+        ):
+            with self.subTest(line_scale=line_scale, plane_scale=plane_scale):
+                line = Line3.PointDir([1, 2, 3], [0, 0, line_scale])
+                plane = Plane3(plane_scale * np.array([0, 0, 1, -5]))
+                result = line.intersect_plane(plane)
+                self.assertIsNotNone(result)
+                nt.assert_allclose(result.p, [1, 2, 5])
+                nt.assert_allclose(line.point(result.lam).ravel(), result.p)
+
+    def test_intersect_plane_angular_tolerance(self):
+        for line_scale, plane_scale in ((1, 1), (1e-8, 1e-8), (1e8, 1e8)):
+            with self.subTest(line_scale=line_scale, plane_scale=plane_scale):
+                line = Line3.PointDir([0, 0, 0], line_scale * np.array([1, 0, 1e-9]))
+                plane = Plane3(plane_scale * np.array([0, 0, 1, -1]))
+                self.assertIsNone(line.intersect_plane(plane, tol=1e8))
+                result = line.intersect_plane(plane, tol=1)
+                self.assertIsNotNone(result)
+                nt.assert_allclose(result.p, [1e9, 0, 1])
+
+    def test_intersect_plane_parallel_scale(self):
+        for scale in (1e-8, 1, 1e8):
+            with self.subTest(scale=scale):
+                line = Line3.PointDir([1, 2, 3], [scale, 0, 0])
+                self.assertIsNone(
+                    line.intersect_plane(Plane3([0, 0, scale, -5 * scale]))
+                )
+
     # Primitives
     def test_constructor1(self):
         # construct from 6-vector