add plan tests

Author: cflinto

core_geometry/plane.py

@@ -36,12 +36,12 @@ def from_rays(cls, ray1, ray2, origin):
 
     def signed_distance_to(self, point):
         """Compute signed distance from a point to the plane."""
-        point = np.array(point)
+        point = np.array(point) # In case point is a list or tuple
         return np.dot(self.normal, point) + self.offset
 
     def project_point(self, point):
         """Project a point orthogonally onto the plane."""
-        point = np.array(point)
+        point = np.array(point) # In case point is a list or tuple
         d = self.signed_distance_to(point)
         return point - d * self.normal
 

tests/__init__.py

@@ -1,7 +1,8 @@
 # tests/dummy_test_2.py
 
 def test_always_fails():
-    assert 1 + 1 == 3, "This test is designed to fail."
+    # assert 1 + 1 == 3, "This test is designed to fail."
+    assert 1 + 1 == 2, "This test is designed to succeed."
 
 if __name__ == "__main__":
     test_always_fails()

tests/dummy_test_2.py

@@ -0,0 +1,122 @@
+from core_geometry.plane import Plane
+import numpy as np
+
+def test_plane_from_points_horizontal():
+    """
+    Create a horizontal plane using three points and verify its normal and offset.
+    """
+    p1 = [0, 0, 0]
+    p2 = [1, 0, 0]
+    p3 = [0, 0, 1]
+    plane = Plane.from_points(p1, p2, p3)
+    assert np.allclose(np.abs(plane.normal), [0, 1, 0]) or np.allclose(np.abs(plane.normal), [0, -1, 0])
+    assert np.isclose(plane.offset, 0)
+
+def test_plane_from_points_inclined():
+    """
+    Create an inclined plane using three points and verify that the signed distance to each point is zero.
+    """
+    p1 = [0, 0, 0]
+    p2 = [1, 1, 0]
+    p3 = [0, 1, 1]
+    plane = Plane.from_points(p1, p2, p3)
+    for p in (p1, p2, p3):
+        assert np.isclose(plane.signed_distance_to(p), 0)
+
+def test_plane_from_collinear_points_raises():
+    """
+    Attempt to create a plane from three collinear points and expect a ValueError.
+    """
+    p1 = [0, 0, 0]
+    p2 = [1, 0, 0]
+    p3 = [2, 0, 0]
+    try:
+        Plane.from_points(p1, p2, p3)
+        assert False, "Expected ValueError"
+    except ValueError:
+        pass
+
+def test_plane_signed_distance_positive():
+    """
+    Test signed distance to a horizontal plane with points above and below it.
+    """
+    plane = Plane([0, 1, 0], [0, 0, 0])
+    assert np.isclose(plane.signed_distance_to([0, 5, 0]), 5)
+    assert np.isclose(plane.signed_distance_to([0, -3, 0]), -3)
+
+def test_plane_project_points():
+    """
+    Project a point onto a horizontal plane and verify the result.
+    """
+    plane = Plane([0, 1, 0], [0, 0, 0])
+    points = [[1, 5, 1], [1, -3, 1], [1, 0, 1]]
+    for point in points:
+        projected = plane.project_point(point)
+        assert np.isclose(projected[1], 0)
+
+def test_plane_from_rays_basic():
+    """
+    Create a plane from two rays and verify that the normal is orthogonal to both rays.
+    """
+    ray1 = np.array([1, 0, 0], dtype=float)
+    ray2 = np.array([0, 0, 1], dtype=float)
+    origin = [0, 0, 0]
+    plane = Plane.from_rays(ray1, ray2, origin)
+    expected_normal = np.cross(ray1, ray2)
+    expected_normal = expected_normal / np.linalg.norm(expected_normal)
+    assert np.allclose(np.abs(plane.normal), expected_normal) or np.allclose(np.abs(plane.normal), -expected_normal)
+
+def test_plane_from_collinear_rays_raises():
+    """
+    Attempt to create a plane from two collinear rays and expect a ValueError.
+    """
+    ray1 = [1, 0, 0]
+    ray2 = [2, 0, 0]
+    origin = [0, 0, 0]
+    try:
+        Plane.from_rays(ray1, ray2, origin)
+        assert False, "Expected ValueError"
+    except ValueError:
+        pass
+
+def test_plane_normalization_enforced():
+    # plane = Plane([0, 10, 0], [0, 0, 0])
+    # assert np.allclose(plane.normal, [0, 1, 0])
+    # assert np.isclose(plane.offset, 0)
+    planes = [Plane([0, 10, -7], [0, 1, 0]), 
+              Plane([5, -10, 0], [2, 0, 0]),
+              Plane([0, 1, 10], [0, 0, -3]),
+              Plane([2, 0, -10], [1, 0, 5])]
+    for plane in planes:
+        # assert np.linalg.norm(plane.normal) == 1, f"Plane normal not normalized: {plane.normal}"
+        assert np.isclose(np.linalg.norm(plane.normal), 1), f"Plane normal not normalized: {plane.normal}"
+
+def test_plane_equation():
+    p1 = [3, 4, 5]
+    p2 = [1, 0, 0]
+    p3 = [8, -2, 1]
+    plane = Plane.from_points(p1, p2, p3)
+    A, B, C = plane.normal
+    D = plane.offset
+    # Equation is A*x + B*y + C*z + D = 0
+    # Launch random rays and verify that the intersection point satisfies the plane equation
+    # ray_positions, ray_dirs = [np.array(-5, -100, 0)], [np.array([1, 0, -15])], [np.array([0, 1, 0]), np.array([0, 0, 1])]
+    ray_positions = np.array([[0, 0, 10], [0, 10, 10], [10, 0, 10], [5, 5, -10], [-10, -10, 10]])
+    ray_dirs = np.array([[0, 0, -1], [0, -1, -1], [-1, 0, -1], [-0.5, -0.5, 2], [1, 1, -1]])
+    intersections = []
+    for pos, dir in zip(ray_positions, ray_dirs):
+        intersection = plane.project_point(pos + dir)
+        intersections.append(intersection)
+        assert np.isclose(A * intersection[0] + B * intersection[1] + C * intersection[2] + D, 0)
+
+if __name__ == "__main__":
+    test_plane_from_points_horizontal()
+    test_plane_from_points_inclined()
+    test_plane_from_collinear_points_raises()
+    test_plane_signed_distance_positive()
+    test_plane_project_points()
+    test_plane_from_rays_basic()
+    test_plane_from_collinear_rays_raises()
+    test_plane_normalization_enforced()
+    test_plane_equation()
+    print("All tests passed!")