From fa014b40354a89775dcce8781229e10ebb67b2a0 Mon Sep 17 00:00:00 2001 From: ZHANG YU <182837830+xixihaha-ha@users.noreply.github.com> Date: Tue, 3 Nov 2026 10:22:01 +0800 Subject: [PATCH 1/2] Solve the plane equations for a three-plane intersection --- spatialmath/geom3d.py | 2 +- tests/test_geom3d.py | 28 ++++++++++++++++++++++++++++ 2 files changed, 29 insertions(+), 1 deletion(-) mode change 100755 => 100644 spatialmath/geom3d.py mode change 100755 => 100644 tests/test_geom3d.py diff --git a/spatialmath/geom3d.py b/spatialmath/geom3d.py old mode 100755 new mode 100644 index 84813a41..5808055c --- a/spatialmath/geom3d.py +++ b/spatialmath/geom3d.py @@ -145,7 +145,7 @@ def intersection(pi1: Plane3, pi2: Plane3, pi3: Plane3) -> R3: """ A = np.vstack([pi1.n, pi2.n, pi3.n]) b = np.array([pi1.d, pi2.d, pi3.d]) - return np.linalg.det(A) @ b + return np.linalg.solve(A, -b) @property def n(self) -> R3: diff --git a/tests/test_geom3d.py b/tests/test_geom3d.py old mode 100755 new mode 100644 index 1fca4402..0cf87437 --- a/tests/test_geom3d.py +++ b/tests/test_geom3d.py @@ -47,6 +47,34 @@ def test_line_distance_parallel_tolerance(): assert line1.distance(line2, tol=1e8) == pytest.approx(np.sqrt(5)) +@pytest.mark.parametrize("point", [[0, 0, 0], [1, 2, 3], [-1.5, 2, 3.25]]) +@pytest.mark.parametrize( + "normals", [np.eye(3), np.array([[1, 2, 0], [0, 1, 3], [2, 0, 1]])] +) +@pytest.mark.parametrize("scales", [[1, 1, 1], [-2, 0.5, 3]]) +def test_three_plane_intersection(point, normals, scales): + normals = normals * np.array(scales)[:, None] + planes = [Plane3.PointNormal(point, normal) for normal in normals] + intersection = Plane3.intersection(*planes) + + assert intersection.shape == (3,) + nt.assert_allclose(intersection, point, atol=1e-12) + # Each plane is defined by n @ x + d = 0, regardless of coefficient scale. + nt.assert_allclose( + normals @ intersection + np.array([plane.d for plane in planes]), + 0, + atol=1e-12, + ) + nt.assert_allclose(Plane3.intersection(*reversed(planes)), point, atol=1e-12) + + +@pytest.mark.parametrize("offset", [0, 1]) +def test_three_plane_intersection_singular(offset): + planes = [Plane3([1, 0, 0, 0]), Plane3([1, 0, 0, offset]), Plane3([0, 1, 0, 0])] + with pytest.raises(np.linalg.LinAlgError): + Plane3.intersection(*planes) + + class Line3Test(unittest.TestCase): # Primitives def test_constructor1(self): From 4ab65ff640de49f97f156c822750172f057f781e Mon Sep 17 00:00:00 2001 From: Copilot <198982749+Copilot@users.noreply.github.com> Date: Thu, 5 Nov 2026 09:24:43 -0500 Subject: [PATCH 2/2] Move three-plane tests to the end of the test file Avoids a textual conflict with other PRs that add tests after the line distance tests, and restores the executable bit on the test file, which runs as a script. --- tests/test_geom3d.py | 56 ++++++++++++++++++++++---------------------- 1 file changed, 28 insertions(+), 28 deletions(-) mode change 100644 => 100755 tests/test_geom3d.py diff --git a/tests/test_geom3d.py b/tests/test_geom3d.py old mode 100644 new mode 100755 index 0cf87437..ea2d6623 --- a/tests/test_geom3d.py +++ b/tests/test_geom3d.py @@ -47,34 +47,6 @@ def test_line_distance_parallel_tolerance(): assert line1.distance(line2, tol=1e8) == pytest.approx(np.sqrt(5)) -@pytest.mark.parametrize("point", [[0, 0, 0], [1, 2, 3], [-1.5, 2, 3.25]]) -@pytest.mark.parametrize( - "normals", [np.eye(3), np.array([[1, 2, 0], [0, 1, 3], [2, 0, 1]])] -) -@pytest.mark.parametrize("scales", [[1, 1, 1], [-2, 0.5, 3]]) -def test_three_plane_intersection(point, normals, scales): - normals = normals * np.array(scales)[:, None] - planes = [Plane3.PointNormal(point, normal) for normal in normals] - intersection = Plane3.intersection(*planes) - - assert intersection.shape == (3,) - nt.assert_allclose(intersection, point, atol=1e-12) - # Each plane is defined by n @ x + d = 0, regardless of coefficient scale. - nt.assert_allclose( - normals @ intersection + np.array([plane.d for plane in planes]), - 0, - atol=1e-12, - ) - nt.assert_allclose(Plane3.intersection(*reversed(planes)), point, atol=1e-12) - - -@pytest.mark.parametrize("offset", [0, 1]) -def test_three_plane_intersection_singular(offset): - planes = [Plane3([1, 0, 0, 0]), Plane3([1, 0, 0, offset]), Plane3([0, 1, 0, 0])] - with pytest.raises(np.linalg.LinAlgError): - Plane3.intersection(*planes) - - class Line3Test(unittest.TestCase): # Primitives def test_constructor1(self): @@ -433,5 +405,33 @@ def test_methods(self): # py.intersect_plane(plane) +@pytest.mark.parametrize("point", [[0, 0, 0], [1, 2, 3], [-1.5, 2, 3.25]]) +@pytest.mark.parametrize( + "normals", [np.eye(3), np.array([[1, 2, 0], [0, 1, 3], [2, 0, 1]])] +) +@pytest.mark.parametrize("scales", [[1, 1, 1], [-2, 0.5, 3]]) +def test_three_plane_intersection(point, normals, scales): + normals = normals * np.array(scales)[:, None] + planes = [Plane3.PointNormal(point, normal) for normal in normals] + intersection = Plane3.intersection(*planes) + + assert intersection.shape == (3,) + nt.assert_allclose(intersection, point, atol=1e-12) + # Each plane is defined by n @ x + d = 0, regardless of coefficient scale. + nt.assert_allclose( + normals @ intersection + np.array([plane.d for plane in planes]), + 0, + atol=1e-12, + ) + nt.assert_allclose(Plane3.intersection(*reversed(planes)), point, atol=1e-12) + + +@pytest.mark.parametrize("offset", [0, 1]) +def test_three_plane_intersection_singular(offset): + planes = [Plane3([1, 0, 0, 0]), Plane3([1, 0, 0, offset]), Plane3([0, 1, 0, 0])] + with pytest.raises(np.linalg.LinAlgError): + Plane3.intersection(*planes) + + if __name__ == "__main__": unittest.main()