add unit tests

panda/src/collide/collisionHeightfield.cxx

@@ -76,6 +76,7 @@ set_subdivisions(int subdivisions) {
   // Calculate the area of a leaf node in the quadtree
   int heightfield_area = _heightfield.get_read_x_size() *
                          _heightfield.get_read_y_size();
+  nassertv(heightfield_area > 0);
   int num_leafs = nodes_count - leaf_first_index;
   // If the area is too small (less than 1), then we
   // retry by decrementing the number of subdivisions.

tests/collide/test_into_heightfield.py

@@ -1,5 +1,84 @@
+import pytest
 from collisions import *
 from panda3d.core import PNMImage
 
+def test_sphere_into_heightfield():
+    # Setup PNMImage
+    img = PNMImage(512, 512, 1)
+    img.set_gray_val(1, 1, 255)
+    # Make CollisionHeightfield
+    max_height = 10
+    subdivisions = 1
+    heightfield = CollisionHeightfield(img, max_height, subdivisions)
+    # The coordinate (1, 1) on our heightfield image
+    # maps to the coordinate (1, 510, Z) in 3D space
+    sphere = CollisionSphere((1, 510, 11), 1)
+    entry, np_from, np_into = make_collision(sphere, heightfield)
+    assert entry.get_surface_point(np_from) == (1, 510, 10)
+    assert entry.get_surface_normal(np_from) == (0, 0, 1)
+    # Set the sphere higher so it is not colliding anymore
+    sphere.set_center((1, 510, 11.1))
+    entry = make_collision(sphere, heightfield)[0]
+    assert entry is None
+    # Set the max_height to re-collide with the sphere
+    max_height = 10.1
+    heightfield.set_max_height(max_height)
+    entry, np_from, np_into = make_collision(sphere, heightfield)
+    assert entry.get_surface_point(np_from) == (1, 510, 10.1)
+
+    with pytest.raises(AssertionError) as err:
+        assert heightfield.set_subdivisions(-1) == err
+        assert heightfield.set_subdivisions(100) == err
+
+    # Use a larger number of subdivisions, should still work
+    subdivisions = 16
+    heightfield.set_subdivisions(subdivisions)
+    entry, np_from, np_into = make_collision(sphere, heightfield)
+    assert entry.get_surface_point(np_from) == (1, 510, 10.1)
+    # Modify the heightfield, no longer colliding
+    img.set_gray_val(1, 1, 254)
+    heightfield.set_heightfield(img)
+    entry = make_collision(sphere, heightfield)[0]
+    assert entry is None
+
+
 def test_ray_into_heightfield():
-    pass
+    # Setup heightfield
+    img = PNMImage(127, 127, 1)
+    img.fill_val(0)
+    max_height = 10
+    subdivisions = 1
+    heightfield = CollisionHeightfield(img, max_height, subdivisions)
+    # Make ray
+    ray = CollisionRay((100, 100, 100), (-1, -1, -1))
+    entry = make_collision(ray, heightfield)[0]
+    assert entry is not None
+
+    # Ray with only a z component in its direction
+    ray.set_direction((0, 0, -5))
+    entry, np_from, np_into = make_collision(ray, heightfield)
+    assert entry.get_surface_point(np_from) == (100, 100, 0)
+
+    # Set coordinate (54, 38) on heightfield to gray value of 255
+    # Note that coordinate (54, 38) on the heightfield
+    # maps to (54, 88, Z) in 3D space
+    img.set_gray_val(54, 38, 255)
+    heightfield.set_heightfield(img)
+
+    ray.set_origin((54, 88, 10))
+    entry, np_from, np_into = make_collision(ray, heightfield)
+    assert entry.get_surface_point(np_from) == (54, 88, 10)
+
+
+def test_box_into_heightfield():
+    # Try using a large non-square heightfield image
+    img = PNMImage(5023, 5130, 1)
+    img.set_gray_val(1, 1, 255)
+    # Make CollisionHeightfield
+    max_height = 10
+    subdivisions = 1
+    heightfield = CollisionHeightfield(img, max_height, subdivisions)
+    # Make box
+    box = CollisionBox((1, 5128, 10), 1, 1, 1)
+    entry = make_collision(box, heightfield)
+    assert entry is not None