From fa95c2753d5ea2d55c8c1c77b1b06f9fbc10ae1e Mon Sep 17 00:00:00 2001
From: ZHANG YU <182837830+xixihaha-ha@users.noreply.github.com>
Date: Tue, 3 Nov 2026 11:08:55 +0800
Subject: [PATCH] Fix the constant coefficient sign in Plane3.contains

---
 spatialmath/geom3d.py        |  2 +-
 tests/test_plane_contains.py | 39 ++++++++++++++++++++++++++++++++++++
 2 files changed, 40 insertions(+), 1 deletion(-)
 mode change 100755 => 100644 spatialmath/geom3d.py
 create mode 100644 tests/test_plane_contains.py

diff --git a/spatialmath/geom3d.py b/spatialmath/geom3d.py
old mode 100755
new mode 100644
index 84813a41..1c3390c2
--- a/spatialmath/geom3d.py
+++ b/spatialmath/geom3d.py
@@ -190,7 +190,7 @@ def contains(self, p: ArrayLike3, tol: float = 20) -> bool:
         :return: if the point is in the plane
         :rtype: bool
         """
-        return abs(np.dot(self.n, p) - self.d) < tol * _eps
+        return abs(np.dot(self.n, p) + self.d) < tol * _eps
 
     def plot(
         self,
diff --git a/tests/test_plane_contains.py b/tests/test_plane_contains.py
new file mode 100644
index 00000000..df2a6604
--- /dev/null
+++ b/tests/test_plane_contains.py
@@ -0,0 +1,39 @@
+import numpy as np
+import pytest
+
+from spatialmath import Plane3
+
+
+@pytest.mark.parametrize("scale", [1, -1, 2.5])
+@pytest.mark.parametrize("factory", ["coefficients", "point-normal", "three-points"])
+def test_translated_plane_contains_points(factory, scale):
+    point = np.array([2.0, 3.0, 4.0])
+    tangent1 = np.array([1.0, -1.0, 0.0])
+    tangent2 = np.array([0.0, 1.0, -1.0])
+    normal = np.cross(tangent1, tangent2)
+    if factory == "coefficients":
+        plane = Plane3(scale * np.r_[normal, -normal @ point])
+    elif factory == "point-normal":
+        plane = Plane3.PointNormal(point, scale * normal)
+    else:
+        plane = Plane3.ThreePoints(
+            np.column_stack((point, point + scale * tangent1, point + tangent2))
+        )
+
+    for candidate in (point, point + tangent1, point + tangent2):
+        assert plane.contains(candidate)
+    assert not plane.contains(point + normal)
+    # The old sign incorrectly accepted the reflected plane.
+    assert not plane.contains(-point)
+
+
+def test_plane_contains_tolerance():
+    plane = Plane3([1, 0, 0, -2])
+    assert plane.contains([2 + 1e-10, 0, 0], tol=1e6)
+    assert not plane.contains([2 + 1e-10, 0, 0])
+
+
+def test_plane_through_origin():
+    plane = Plane3([0, 0, 1, 0])
+    assert plane.contains([2, 3, 0])
+    assert not plane.contains([2, 3, 1])