diff --git a/spatialmath/geom3d.py b/spatialmath/geom3d.py index c1efbbb2..a1921abf 100644 --- a/spatialmath/geom3d.py +++ b/spatialmath/geom3d.py @@ -638,14 +638,16 @@ def contains( the Line3 object self. If ``X`` is an array with 3 rows, the test is performed on every column and - an array of booleans is returned. + an array of booleans is returned. The tolerance applies to perpendicular + distance, independent of the scale of the Plucker coordinates. """ + direction = self.w / np.linalg.norm(self.w) if base.isvector(x, 3): x = cast(R3, base.getvector(x)) - return bool(np.linalg.norm(np.cross(x - self.pp, self.w)) < tol * _eps) + return bool(np.linalg.norm(np.cross(x - self.pp, direction)) < tol * _eps) elif base.ismatrix(x, (3, None)): return [ - bool(np.linalg.norm(np.cross(p - self.pp, self.w)) < tol * _eps) + bool(np.linalg.norm(np.cross(p - self.pp, direction)) < tol * _eps) for p in x.T ] else: diff --git a/tests/test_line_contains_scale.py b/tests/test_line_contains_scale.py new file mode 100644 index 00000000..5effdb62 --- /dev/null +++ b/tests/test_line_contains_scale.py @@ -0,0 +1,38 @@ +"""Point containment must not depend on homogeneous Plucker scale.""" + +import numpy as np +import numpy.testing as nt +import pytest +from spatialmath import Line3 + + +@pytest.mark.parametrize("direction", [[1, 0, 0], [0, 1, 0], [1, 2, -1]]) +@pytest.mark.parametrize("scale", [1.0, -1.0, 1e-8, -1e-8, 1e8]) +def test_contains_scalar_and_batch_are_scale_invariant(direction, scale): + direction = np.asarray(direction, dtype=float) + normal = np.cross(direction, [0, 0, 1]) + normal /= np.linalg.norm(normal) + origin = np.array([1.0, 2.0, 3.0]) + line = Line3.PointDir(origin, scale * direction) + direction /= np.linalg.norm(direction) + # Far outside tolerance even for the translated/oblique lines. + on_line = origin + 2 * direction + off_line = on_line + 1e-8 * normal + assert line.contains(on_line, tol=100) + assert not line.contains(off_line, tol=100) + nt.assert_array_equal( + line.contains(np.column_stack([on_line, off_line]), tol=100), [True, False] + ) + + +@pytest.mark.parametrize("scale", [1.0, -1.0, 1e-8, 1e8]) +def test_contains_tolerance_is_a_geometric_distance(scale): + line = Line3.PointDir([0, 0, 0], [scale, 0, 0]) + eps = np.finfo(float).eps + assert line.contains([1, 10 * eps, 0], tol=20) + assert not line.contains([1, 30 * eps, 0], tol=20) + assert line.contains([1, 30 * eps, 0], tol=40) + nt.assert_array_equal( + line.contains(np.array([[1, 1], [10 * eps, 30 * eps], [0, 0]]), tol=20), + [True, False], + )