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 index 1fca4402..ea2d6623 100755 --- a/tests/test_geom3d.py +++ b/tests/test_geom3d.py @@ -405,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()