collide: Add CollisionHeightfield solid (#691)

panda/src/collide/CMakeLists.txt

@@ -12,6 +12,7 @@ set(P3COLLIDE_HEADERS
   collisionHandlerPusher.I collisionHandlerPusher.h
   collisionHandlerFluidPusher.I collisionHandlerFluidPusher.h
   collisionHandlerQueue.h
+  collisionHeightfield.I collisionHeightfield.h
   collisionInvSphere.I collisionInvSphere.h
   collisionLine.I collisionLine.h
   collisionLevelStateBase.I collisionLevelStateBase.h
@@ -46,6 +47,7 @@ set(P3COLLIDE_SOURCES
   collisionHandlerPusher.cxx
   collisionHandlerFluidPusher.cxx
   collisionHandlerQueue.cxx
+  collisionHeightfield.cxx
   collisionLevelStateBase.cxx
   collisionLevelState.cxx
   collisionInvSphere.cxx

panda/src/collide/collisionHeightfield.I

@@ -0,0 +1,102 @@
+/**
+ * PANDA 3D SOFTWARE
+ * Copyright (c) Carnegie Mellon University.  All rights reserved.
+ *
+ * All use of this software is subject to the terms of the revised BSD
+ * license.  You should have received a copy of this license along
+ * with this source code in a file named "LICENSE."
+ *
+ * @file collisionHeightfield.I
+ * @author hecris
+ * @date 2019-07-01
+ */
+
+/**
+ *
+ */
+INLINE CollisionHeightfield::
+CollisionHeightfield() {
+}
+
+/**
+ *
+ */
+INLINE CollisionHeightfield::
+CollisionHeightfield(const CollisionHeightfield &copy) {
+}
+
+/**
+ *
+ */
+INLINE CollisionHeightfield::
+~CollisionHeightfield() {
+  delete[] _nodes;
+ }
+
+/**
+ *
+ */
+INLINE void CollisionHeightfield::
+flush_level() {
+  _volume_pcollector.flush_level();
+  _test_pcollector.flush_level();
+}
+
+/**
+ *
+ */
+INLINE PNMImage CollisionHeightfield::
+get_heightfield() {
+  return _heightfield;
+}
+
+/**
+ *
+ */
+INLINE void CollisionHeightfield::
+set_heightfield(PNMImage heightfield) {
+  int r = _heightfield.get_x_size();
+  int c = _heightfield.get_y_size();
+  _heightfield = heightfield;
+
+  if (_heightfield.get_x_size() == r &&
+      _heightfield.get_y_size() == c) {
+    fill_quadtree_heights();
+  } else {
+    fill_quadtree_areas();
+    fill_quadtree_heights();
+  }
+}
+
+/**
+ *
+ */
+INLINE PN_stdfloat CollisionHeightfield::
+get_max_height() {
+  return _max_height;
+}
+
+/**
+ *
+ */
+INLINE void CollisionHeightfield::
+set_max_height(PN_stdfloat max_height) {
+  _max_height = max_height;
+  fill_quadtree_heights();
+}
+
+/**
+ *
+ */
+INLINE int CollisionHeightfield::
+get_num_subdivisions() {
+  return _num_subdivisions;
+}
+
+/**
+ *
+ */
+INLINE PN_stdfloat CollisionHeightfield::
+get_height(int x, int y) const {
+  return _heightfield.get_gray(x, y) * _max_height;
+}

panda/src/collide/collisionHeightfield.cxx

@@ -0,0 +1,819 @@
+/**
+ * PANDA 3D SOFTWARE
+ * Copyright (c) Carnegie Mellon University.  All rights reserved.
+ *
+ * All use of this software is subject to the terms of the revised BSD
+ * license.  You should have received a copy of this license along
+ * with this source code in a file named "LICENSE."
+ *
+ * @file collisionHeightfield.cxx
+ * @author hecris
+ * @date 2019-07-01
+ */
+
+#include "collisionBox.h"
+#include "collisionRay.h"
+#include "collisionSphere.h"
+#include "collisionHeightfield.h"
+#include "collisionHandler.h"
+#include "collisionEntry.h"
+#include "cmath.h"
+#include "config_collide.h"
+#include "boundingBox.h"
+#include "datagram.h"
+#include "datagramIterator.h"
+#include "bamReader.h"
+#include "bamWriter.h"
+#include "geom.h"
+#include "geomTriangles.h"
+#include "geomVertexWriter.h"
+#include <queue>
+#include <algorithm>
+
+using std::min;
+using std::max;
+using std::queue;
+using std::vector;
+using std::sort;
+
+PStatCollector CollisionHeightfield::_volume_pcollector(
+      "Collision Volumes:CollisionHeightfield");
+PStatCollector CollisionHeightfield::_test_pcollector(
+      "Collision Tests:CollisionHeightfield");
+TypeHandle CollisionHeightfield::_type_handle;
+
+/**
+ *
+ */
+CollisionHeightfield::
+CollisionHeightfield(PNMImage heightfield,
+                     PN_stdfloat max_height, int num_subdivisions) {
+  _heightfield = heightfield;
+  _max_height = max_height;
+  _nodes_count = 0;
+
+  set_num_subdivisions(num_subdivisions);
+}
+
+/**
+ * Sets the number of quadtree subdivisions and modifies
+ * the quadtree accordingly. This should be called when a
+ * user wants to modify the number of quadtree subdivisions
+ * or from a constructor to initialize the quadtree.
+ *
+ * If the number of subdivisions is too high, it will
+ * automatically be decremented.
+ */
+void CollisionHeightfield::
+set_num_subdivisions(int num_subdivisions) {
+  // The number of subdivisions should not be negative
+  nassertv(num_subdivisions >= 0 && num_subdivisions <= 10);
+  // Determine the number of quadtree nodes needed
+  // for the corresponding number of subdivisions.
+  int nodes_count = 0;
+  for (int i = 0; i <= num_subdivisions; i++) {
+    nodes_count += 1 << (i * 2);
+  }
+  if (nodes_count == _nodes_count) {
+    // No changes to quadtree to be done
+    return;
+  }
+  // Calculate the index of the first quad tree leaf node
+  int leaf_first_index = nodes_count - pow(4, num_subdivisions);
+  // Calculate the area of a leaf node in the quadtree
+  PN_stdfloat heightfield_area = _heightfield.get_x_size() *
+                                 _heightfield.get_y_size();
+  nassertv(heightfield_area > 0);
+  PN_stdfloat 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.
+  if (heightfield_area / num_leafs < 1) {
+    set_num_subdivisions(num_subdivisions - 1);
+    return;
+  }
+  if (nodes_count < _nodes_count) {
+    // If the user is decreasing the number of subdivisions, then
+    // we need only to update the index where the quadtree leaves start.
+    _leaf_first_index = leaf_first_index;
+  } else {
+    // Otherwise we need to build a new quadtree
+    if (_nodes_count != 0) {
+      // There is an existing quadtree, delete it first
+      delete[] _nodes;
+      _nodes = nullptr;
+    }
+    QuadTreeNode *nodes = new QuadTreeNode[nodes_count];
+    _nodes = nodes;
+    _nodes_count = nodes_count;
+    _leaf_first_index = leaf_first_index;
+    fill_quadtree_areas();
+    fill_quadtree_heights();
+  }
+  _num_subdivisions = num_subdivisions;
+}
+
+/**
+ *
+ */
+void CollisionHeightfield::
+fill_quadtree_areas() {
+  nassertv(_heightfield.get_x_size() > 0 &&
+           _heightfield.get_y_size() > 0);
+
+  _nodes[0].area.min = {0, 0};
+  _nodes[0].area.max = {(float)_heightfield.get_x_size(),
+                        (float)_heightfield.get_y_size()};
+  _nodes[0].index = 0;
+  QuadTreeNode parent;
+  for (int i = 1; i < _nodes_count; i += 4) {
+    parent = _nodes[(i-1) / 4];
+    LVector2 sub_area = (parent.area.max - parent.area.min) / 2;
+    // SE Quadrant
+    _nodes[i].area.min = parent.area.min;
+    _nodes[i].area.max = parent.area.min + sub_area;
+    _nodes[i].index = i;
+    // SW Quadrant
+    _nodes[i + 1].area.min = {parent.area.min[0] + sub_area[0],
+                              parent.area.min[1]};
+    _nodes[i + 1].area.max = _nodes[i + 1].area.min + sub_area;
+    _nodes[i + 1].index = i + 1;
+    // NE Quadrant
+    _nodes[i + 2].area.min = {parent.area.min[0],
+                              parent.area.min[1] + sub_area[1]};
+    _nodes[i + 2].area.max = _nodes[i + 2].area.min + sub_area;
+    _nodes[i + 2].index = i + 2;
+    // NW Quadrant
+    _nodes[i + 3].area.min = parent.area.min + sub_area;
+    _nodes[i + 3].area.max = parent.area.max;
+    _nodes[i + 3].index = i + 3;
+  }
+}
+
+/**
+ * Processes the heightfield image, setting the height values
+ * of each quadtree node.
+ *
+ * @relates set_max_height
+ * @relates set_heightfield
+ */
+void CollisionHeightfield::
+fill_quadtree_heights() {
+  QuadTreeNode node;
+  QuadTreeNode child;
+  for (int i = _nodes_count - 1; i >= 0; i--) {
+    node = _nodes[i];
+    PN_stdfloat height_min = INT_MAX;
+    PN_stdfloat height_max = INT_MIN;
+    if (i >= _leaf_first_index) {
+      for (int x = node.area.min[0]; x < node.area.max[0]; x++) {
+        for (int y = node.area.min[1]; y < node.area.max[1]; y++) {
+          PN_stdfloat value = _heightfield.get_gray(x,
+            _heightfield.get_y_size() - 1 - y) * _max_height;
+
+          height_min = min(value, height_min);
+          height_max = max(value, height_max);
+        }
+      }
+    } else {
+      for (int c = (i * 4) + 1, cmax = c + 4; c < cmax; c++) {
+        child = _nodes[c];
+        height_min = min(child.height_min, height_min);
+        height_max = max(child.height_max, height_max);
+      }
+    }
+    _nodes[i].height_min = height_min;
+    _nodes[i].height_max = height_max;
+  }
+}
+
+/**
+ *
+ */
+PT(CollisionEntry) CollisionHeightfield::
+test_intersection_from_ray(const CollisionEntry &entry) const {
+  const CollisionRay *ray;
+  DCAST_INTO_R(ray, entry.get_from(), nullptr);
+  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
+
+  LPoint3 from_origin = ray->get_origin() * wrt_mat;
+  LPoint3 from_direction = ray->get_direction() * wrt_mat;
+
+  IntersectionParams params;
+  params.from_origin = from_origin;
+  params.from_direction = from_direction;
+  vector<QuadTreeIntersection> intersections;
+  intersections = find_intersections(line_intersects_box, params);
+
+  if (intersections.empty()) {
+    return nullptr;
+  }
+  // Sort intersections by their t1 values so we can return
+  // the first intersection found.
+  sort(intersections.begin(), intersections.end());
+
+  double t1, t2;
+  for (size_t i = 0; i < intersections.size(); i++) {
+    t1 = intersections[i].tmin;
+    t2 = intersections[i].tmax;
+
+    if (t1 < 0.0 && t2 < 0.0) continue;
+    t1 = max(t1, 0.0);
+
+    LPoint3 p1 = from_origin + from_direction * t1;
+    LPoint3 p2 = from_origin + from_direction * t2;
+    // We use Bresenhaum's Line Algorithm to directly get the heightfield
+    // coordinates that the line passes through.
+    int err = cabs(2 * (p2[1] - p1[1]));
+    int deltaerr = cabs(err - (p2[0] - p1[0]));
+
+    int x = p1[0], y = p1[1];
+    bool x_increasing = x < p2[0];
+    bool y_increasing = y < p2[1];
+    while ((x_increasing && x <= p2[0]) || (!x_increasing && x >= p2[0])) {
+      bool intersected = false;
+      Triangle int_tri;
+      vector<Triangle> triangles = get_triangles(x, y);
+      double min_t = DBL_MAX;
+      for (Triangle tri : triangles) {
+        double t;
+        if (line_intersects_triangle(t, from_origin, from_direction, tri)) {
+          if (t < 0) continue;
+          if (t < min_t) {
+            intersected = true;
+            min_t = t;
+            int_tri = tri;
+          }
+        }
+      }
+      // If the ray intersects this heightfield element, we can
+      // stop here and return the intersection.
+      if (intersected) {
+        PT(CollisionEntry) new_entry = new CollisionEntry(entry);
+        new_entry->set_surface_point(from_origin + from_direction * min_t);
+        LPlane p = LPlane(int_tri.p1, int_tri.p2, int_tri.p3);
+        LVector3 normal = p.get_normal();
+        if (p.dist_to_plane(from_origin) < 0.0f) normal *= -1;
+        new_entry->set_surface_normal(normal);
+        return new_entry;
+      }
+      // Otherwise, go to the next heightfield element
+      deltaerr += err;
+      if (deltaerr >= 0) {
+        if (y_increasing) y++;
+        else y--;
+        deltaerr -= 2 * (p2[0] - p1[0]);
+      }
+      if (x_increasing) x++;
+      else x--;
+    }
+  }
+
+  return nullptr;
+}
+
+/**
+ *
+ */
+PT(CollisionEntry) CollisionHeightfield::
+test_intersection_from_sphere(const CollisionEntry &entry) const {
+  const CollisionSphere *sphere;
+  DCAST_INTO_R(sphere, entry.get_from(), nullptr);
+
+  CPT(TransformState) wrt_space = entry.get_wrt_space();
+  const LMatrix4 &wrt_mat = wrt_space->get_mat();
+
+  LPoint3 center = sphere->get_center() * wrt_mat;
+  LVector3 radius_v = LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
+  PN_stdfloat radius_2 = radius_v.length_squared();
+  PN_stdfloat radius = csqrt(radius_2);
+
+  IntersectionParams params;
+  params.center = center;
+  params.radius = radius;
+
+  vector<QuadTreeIntersection> intersections;
+  intersections= find_intersections(sphere_intersects_box, params);
+
+  if (intersections.empty()) {
+    return nullptr;
+  }
+
+  LPoint3 point;
+  LPoint3 closest_point;
+  PN_stdfloat dist;
+  PN_stdfloat dist_min = FLT_MAX;
+
+  bool intersected = false;
+  for (size_t i = 0; i < intersections.size(); i++) {
+    QuadTreeNode node = _nodes[intersections[i].node_index];
+    // Iterate through the circle's area and find triangle intersections,
+    // find the one closest to the center of the sphere.
+    for (int dx = -radius; dx <= radius; dx++) {
+      for (int dy = -radius; dy <= radius; dy++) {
+        int x = dx + center[0];
+        int y = dy + center[1];
+        if (x < node.area.min[0] || y < node.area.min[1] ||
+            x > node.area.max[0] || y > node.area.max[1]) {
+          // point not contained in rectangle
+          continue;
+        }
+        if (dx * dx + dy * dy > radius_2) continue;
+
+        vector<Triangle> triangles = get_triangles(x, y);
+        for (Triangle tri : triangles) {
+          point = closest_point_on_triangle(center, tri);
+          dist = (point - center).length_squared();
+          if (dist > radius_2) continue;
+          if (dist < dist_min) {
+            intersected = true;
+            dist_min = dist;
+            closest_point = point;
+          }
+        }
+      }
+    }
+  }
+  if (intersected) {
+    PT(CollisionEntry) new_entry = new CollisionEntry(entry);
+    LVector3 v(center - closest_point);
+    PN_stdfloat v_length = v.length();
+    if (IS_NEARLY_ZERO(v_length)) {
+      v.set(1, 0, 0);
+    } else {
+      v /= v_length;
+    }
+    new_entry->set_surface_point(closest_point);
+    new_entry->set_surface_normal(v);
+    new_entry->set_interior_point(center - radius * v);
+    return new_entry;
+  } else {
+    return nullptr;
+  }
+}
+
+/**
+ *
+ */
+PT(CollisionEntry) CollisionHeightfield::
+test_intersection_from_box(const CollisionEntry &entry) const {
+  const CollisionBox *box;
+  DCAST_INTO_R(box, entry.get_from(), nullptr);
+
+  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
+  LPoint3 box_min = box->get_min() * wrt_mat;
+  LPoint3 box_max = box->get_max() * wrt_mat;
+
+  IntersectionParams params;
+  params.box_min = box_min;
+  params.box_max = box_max;
+
+  vector<QuadTreeIntersection> intersections;
+  intersections = find_intersections(box_intersects_box, params);
+
+  if (intersections.empty()) {
+    return nullptr;
+  }
+
+  bool intersected = false;
+  Triangle intersected_tri;
+  for (size_t i = 0; i < intersections.size(); i++) {
+    QuadTreeNode node = _nodes[intersections[i].node_index];
+    // Find the overlapping rectangle between the two boxes and
+    // test the heightfield elements in that area.
+    LVecBase2 overlap_min = LVecBase2(max(box_min[0], node.area.min[0]),
+                                      max(box_min[1], node.area.min[1]));
+    LVecBase2 overlap_max = LVecBase2(min(box_max[0], node.area.max[0]),
+                                      min(box_max[1], node.area.max[1]));
+    for (int x = overlap_min[0]; x < overlap_max[0]; x++) {
+      for (int y = overlap_min[1]; y < overlap_max[1]; y++) {
+        vector<Triangle> triangles = get_triangles(x, y);
+        for (Triangle tri : triangles) {
+          if (box_intersects_triangle(box_min, box_max, tri)) {
+            intersected = true;
+            intersected_tri = tri;
+          }
+        }
+      }
+    }
+  }
+
+  if (intersected) {
+    PT(CollisionEntry) new_entry = new CollisionEntry(entry);
+    LPlane p = LPlane(intersected_tri.p1, intersected_tri.p2, intersected_tri.p3);
+    LVector3 normal = p.get_normal();
+    if (p.dist_to_plane(box->get_center() * wrt_mat) < 0.0f) {
+      normal *= -1;
+      p.flip();
+    }
+    new_entry->set_surface_normal(normal);
+    PN_stdfloat min_dist = 0;
+    LPoint3 interior_point;
+    // Find the deepest vertex and set it as the interior point
+    for (int i = 0; i < 8; i++) {
+      LPoint3 vertex = box->get_point(i) * wrt_mat;
+      PN_stdfloat dist = p.dist_to_plane(vertex);
+      if (dist <= min_dist) {
+        min_dist = dist;
+        interior_point = vertex;
+      }
+    }
+    new_entry->set_interior_point(interior_point);
+    new_entry->set_surface_point(
+        closest_point_on_triangle(interior_point, intersected_tri));
+    return new_entry;
+  } else {
+    return nullptr;
+  }
+}
+
+/**
+ *
+ */
+bool CollisionHeightfield::
+line_intersects_box(const LPoint3 &box_min, const LPoint3 &box_max,
+                    IntersectionParams &params) {
+  LPoint3 from = params.from_origin;
+  LPoint3 delta = params.from_direction;
+
+  double tmin = -DBL_MAX;
+  double tmax = DBL_MAX;
+  for (int i = 0; i < 3; ++i) {
+    PN_stdfloat d = delta[i];
+    if (!IS_NEARLY_ZERO(d)) {
+      double tmin2 = (box_min[i] - from[i]) / d;
+      double tmax2 = (box_max[i] - from[i]) / d;
+      if (tmin2 > tmax2) {
+        std::swap(tmin2, tmax2);
+      }
+      tmin = max(tmin, tmin2);
+      tmax = min(tmax, tmax2);
+      if (tmin > tmax) {
+        return false;
+      }
+    } else if (from[i] < box_min[i] || from[i] > box_max[i]) {
+      // The line is parallel
+      return false;
+    }
+  }
+  params.t1 = tmin;
+  params.t2 = tmax;
+  return true;
+}
+
+/**
+ *
+ */
+bool CollisionHeightfield::
+line_intersects_triangle(double &t, const LPoint3 &from,
+                         const LPoint3 &delta,
+                         const Triangle &triangle) {
+  // Implementation of Möller-Trumbore algorithm
+  PN_stdfloat a,f,u,v;
+  LVector3 edge1, edge2, h, s, q;
+  edge1 = triangle.p2 - triangle.p1;
+  edge2 = triangle.p3 - triangle.p1;
+  h = delta.cross(edge2);
+  a = dot(edge1, h);
+  if (IS_NEARLY_ZERO(a)) {
+    // line parallel to triangle
+    return false;
+  }
+  f = 1.0 / a;
+  s = from - triangle.p1;
+  u = f * dot(s, h);
+  if (u < 0.0 || u > 1.0) {
+    return false;
+  }
+  q = s.cross(edge1);
+  v = f * dot(delta, q);
+  if (v < 0.0 || u + v > 1.0) {
+    return false;
+  }
+  t = f * dot(edge2, q);
+  return true;
+}
+
+/**
+ *
+ */
+bool CollisionHeightfield::
+sphere_intersects_box(const LPoint3 &box_min, const LPoint3 &box_max,
+                      IntersectionParams &params) {
+  LPoint3 center = params.center;
+  double radius = params.radius;
+  LPoint3 p = center.fmin(box_max).fmax(box_min);
+  return (center - p).length_squared() <= radius * radius;
+}
+
+/**
+ *
+ */
+bool CollisionHeightfield::
+box_intersects_triangle(const LPoint3 &box_min, const LPoint3 &box_max,
+                        const Triangle &triangle) {
+  // Using the Seperating Axis Theorem, we have a maximum of 13 SAT tests
+  // Refer to Christer Ericson's Collision Detection book for full explanation
+  LPoint3 v0 = triangle.p1, v1 = triangle.p2, v2 = triangle.p3;
+
+  PN_stdfloat p0, p1, p2, r;
+  LVector3 c = (box_min + box_max) * 0.5f;
+  PN_stdfloat e0 = (box_max[0] - box_min[0]) * 0.5f;
+  PN_stdfloat e1 = (box_max[1] - box_min[1]) * 0.5f;
+  PN_stdfloat e2 = (box_max[2] - box_min[2]) * 0.5f;
+
+  v0 = v0 - c;
+  v1 = v1 - c;
+  v2 = v2 - c;
+
+  LVector3 f0 = v1 - v0, f1 = v2 - v1, f2 = v0 - v2;
+
+  r = e1 * cabs(f0[2]) + e2 * cabs(f0[1]);
+  p0 =  v0[1]*(-v1[2] + v0[2]) + v0[2]*(v1[1] - v0[1]);
+  p2 =  v2[1]*(-v1[2] + v0[2]) + v2[2]*(v1[1] - v0[1]);
+  if (max(-1 * max(p0, p2), min(p0, p2)) > r) return false;
+
+  r = e1 * cabs(f1[2]) + e2 * cabs(f1[1]);
+  p0 =  v0[1]*(-v2[2] + v1[2]) + v0[2]*(v2[1] - v1[1]);
+  p2 =  v2[1]*(-v2[2] + v1[2]) + v2[2]*(v2[1] - v1[1]);
+  if (max(-1 * max(p0, p2), min(p0, p2)) > r) return false;
+
+  r = e1 * cabs(f2[2]) + e2 * cabs(f2[1]);
+  p0 =  v0[1]*(-v0[2] + v2[2]) + v0[2]*(v0[1] - v2[1]);
+  p1 =  v1[1]*(-v0[2] + v2[2]) + v1[2]*(v0[1] - v2[1]);
+  if (max(-1 * max(p0, p1), min(p0, p1)) > r) return false;
+
+  r = e0 * cabs(f0[2]) + e2 * cabs(f0[0]);
+  p0 =  v0[0]*(v1[2] - v0[2]) + v0[2]*(-v1[0] + v0[0]);
+  p2 =  v2[0]*(v1[2] - v0[2]) + v2[2]*(-v1[0] + v0[0]);
+  if (max(-1 * max(p0, p2), min(p0, p2)) > r) return false;
+
+  r = e0 * cabs(f1[2]) + e2 * cabs(f1[0]);
+  p0 =  v0[0]*(v2[2] - v1[2]) + v0[2]*(-v2[0] + v1[0]);
+  p2 =  v2[0]*(v2[2] - v1[2]) + v2[2]*(-v2[0] + v1[0]);
+  if (max(-1 * max(p0, p2), min(p0, p2)) > r) return false;
+
+  r = e0 * cabs(f2[2]) + e2 * cabs(f2[0]);
+  p0 =  v0[0]*(v0[2] - v2[2]) + v0[2]*(-v0[0] + v2[0]);
+  p1 =  v1[0]*(v0[2] - v2[2]) + v1[2]*(-v0[0] + v2[0]);
+  if (max(-1 * max(p0, p1), min(p0, p1)) > r) return false;
+
+  r = e0 * cabs(f0[1]) + e1 * cabs(f0[0]);
+  p0 =  v0[0]*(-v1[1] + v0[1]) + v0[1]*(v1[0] - v0[0]);
+  p2 =  v2[0]*(-v1[1] + v0[1]) + v2[1]*(v1[0] - v0[0]);
+  if (max(-1 * max(p0, p2), min(p0, p2)) > r) return false;
+
+  r = e0 * cabs(f1[1]) + e1 * cabs(f1[0]);
+  p0 =  v0[0]*(-v2[1] + v1[1]) + v0[1]*(v2[0] - v1[0]);
+  p2 =  v2[0]*(-v2[1] + v1[1]) + v2[1]*(v2[0] - v1[0]);
+  if (max(-1 * max(p0, p2), min(p0, p2)) > r) return false;
+
+  r = e0 * cabs(f2[1]) + e1 * cabs(f2[0]);
+  p0 =  v0[0]*(-v0[1] + v2[1]) + v0[1]*(v0[0] - v2[0]);
+  p1 =  v1[0]*(-v0[1] + v2[1]) + v1[1]*(v0[0] - v2[0]);
+  if (max(-1 * max(p0, p1), min(p0, p1)) > r) return false;
+
+  if (max(max(v0[0], v1[0]), v2[0]) < -e0 ||
+      min(min(v0[0], v1[0]), v2[0]) > e0) return false;
+  if (max(max(v0[1], v1[1]), v2[1]) < -e1 ||
+      min(min(v0[1], v1[1]), v2[1]) > e1) return false;
+  if (max(max(v0[2], v1[2]), v2[2]) < -e2 ||
+      min(min(v0[2], v1[2]), v2[2]) > e2) return false;
+
+  LVector3 n = f0.cross(f1);
+  PN_stdfloat d = dot(n, triangle.p1);
+
+  LPoint3 e = box_max - c;
+
+  PN_stdfloat r2 = e[0] * cabs(n[0]) + e[1] * cabs(n[1]) + e[2] * cabs(n[2]);
+  PN_stdfloat s = dot(n, c) - d;
+  return cabs(s) <= r2;
+}
+
+/**
+ *
+ */
+bool CollisionHeightfield::
+box_intersects_box(const LPoint3 &box_min, const LPoint3 &box_max,
+                   IntersectionParams &params) {
+
+  return (box_min[0] <= params.box_max[0] && box_max[0] >= params.box_min[0]) &&
+         (box_min[1] <= params.box_max[1] && box_max[1] >= params.box_min[1]) &&
+         (box_min[2] <= params.box_max[2] && box_max[2] >= params.box_min[2]);
+}
+
+/**
+ * Returns the closest point on a triangle to another given point.
+ */
+LPoint3 CollisionHeightfield::
+closest_point_on_triangle(const LPoint3 &p, const Triangle &triangle) {
+  LVector3 ab = triangle.p2 - triangle.p1;
+  LVector3 ac = triangle.p3 - triangle.p1;
+  LVector3 ap = p - triangle.p1;
+  PN_stdfloat d1 = dot(ab, ap);
+  PN_stdfloat d2 = dot(ac, ap);
+  if (d1 <= 0.0f && d2 <= 0.0f) return triangle.p1;
+
+  LVector3 bp = p - triangle.p2;
+  PN_stdfloat d3 = dot(ab, bp);
+  PN_stdfloat d4 = dot(ac, bp);
+  if (d3 >= 0.0f && d4 <= d3) return triangle.p2;
+
+  PN_stdfloat vc = d1 * d4 - d3 * d2;
+  if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) {
+    PN_stdfloat v = d1 / (d1 - d3);
+    return triangle.p1 + v * ab;
+  }
+
+  LVector3 cp = p - triangle.p3;
+  PN_stdfloat d5 = dot(ab, cp);
+  PN_stdfloat d6 = dot(ac, cp);
+  if (d6 >= 0.0f && d5 <= d6) return triangle.p3;
+
+  PN_stdfloat vb = d5 * d2 - d1 * d6;
+  if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) {
+    PN_stdfloat w = d2 / (d2 - d6);
+    return triangle.p1 + w * ac;
+  }
+
+  PN_stdfloat va = d3 * d6 - d5 * d4;
+  if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) {
+    PN_stdfloat w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
+    return triangle.p2 + w * (triangle.p3 - triangle.p2);
+  }
+
+  PN_stdfloat denom = 1.0f / (va + vb + vc);
+  PN_stdfloat v = vb * denom;
+  PN_stdfloat w = vc * denom;
+  return triangle.p1 + ab * v + ac * w;
+}
+
+/**
+ * Given a pointer to a function that tests for intersection between a solid
+ * and a box, return the quad tree nodes that are intersected by that solid.
+ */
+vector<CollisionHeightfield::QuadTreeIntersection> CollisionHeightfield::
+find_intersections(BoxIntersection intersects_box, IntersectionParams params) const {
+  queue<QuadTreeNode> q;
+  QuadTreeNode node = _nodes[0];
+  q.push(node);
+
+  LPoint3 box_min, box_max;
+  vector<QuadTreeIntersection> intersections;
+
+  while (!q.empty()) {
+    node = q.front();
+    q.pop();
+    box_min = {node.area.min[0], node.area.min[1], node.height_min};
+    box_max = {node.area.max[0], node.area.max[1], node.height_max};
+    if (!intersects_box(box_min, box_max, params)) {
+      continue;
+    }
+
+    if (node.index >= _leaf_first_index) {
+      QuadTreeIntersection intersection = {node.index, params.t1, params.t2};
+      intersections.push_back(intersection);
+    } else {
+      int child_first_index = 4 * node.index + 1;
+      q.push(_nodes[child_first_index]);
+      q.push(_nodes[child_first_index + 1]);
+      q.push(_nodes[child_first_index + 2]);
+      q.push(_nodes[child_first_index + 3]);
+    }
+  }
+
+  return intersections;
+}
+
+/**
+ * Return the triangles that are defined at the
+ * given 2D coordinate.
+ */
+vector<CollisionHeightfield::Triangle> CollisionHeightfield::
+get_triangles(int x, int y) const {
+  int rows = _heightfield.get_x_size();
+  int cols = _heightfield.get_y_size();
+  vector<Triangle> triangles;
+  if (x < 0 || y < 0 || x >= rows || y >= cols)
+    return triangles;
+
+  Triangle t;
+  int heightfield_y = cols - 1 - y;
+  bool odd = (x + heightfield_y) & 1;
+  t.p1 = LPoint3(x, y, get_height(x, heightfield_y));
+  for (int dx = -1; dx <= 1; dx += 2) {
+    for (int dy = -1; dy <= 1; dy += 2) {
+      int x2 = x + dx;
+      int y2 = y - dy;
+      int heightfield_y2 = heightfield_y + dy;
+      if (x2 < 0 || heightfield_y2 < 0 || x2 >= rows || heightfield_y2 >= cols) continue;
+      if (odd) {
+        t.p2 = LPoint3(x2, y, get_height(x2, heightfield_y));
+        t.p3 = LPoint3(x, y2, get_height(x, heightfield_y2));
+      } else {
+        t.p2 = LPoint3(x2, y2, get_height(x2, heightfield_y2));
+        t.p3 = LPoint3(x2, y, get_height(x2, heightfield_y));
+        triangles.push_back(t);
+        t.p3 = LPoint3(x, y2, get_height(x, heightfield_y2));
+        triangles.push_back(t);
+      }
+    }
+  }
+
+  return triangles;
+}
+
+/**
+ * Generic CollisionSolid member functions
+ */
+void CollisionHeightfield::
+fill_viz_geom() {
+  PT(GeomVertexData) vdata = new GeomVertexData
+    ("collision", GeomVertexFormat::get_v3(),
+     Geom::UH_static);
+
+  GeomVertexWriter vertex(vdata, InternalName::get_vertex());
+
+  PT(GeomTriangles) tris = new GeomTriangles(Geom::UH_static);
+
+  int cols = _heightfield.get_x_size();
+  int rows = _heightfield.get_y_size();
+
+  for (int y = 0; y < rows; y++) {
+    for (int x = 0; x < cols; x++) {
+      // a point (x, y) in the heightfield image
+      // maps to (x, y2, height_at(x, y)) in 3D space
+      int y2 = rows - y - 1;
+      vertex.add_data3(x, y2, get_height(x, y));
+    }
+  }
+
+  for (int i = 0; i < rows - 1; i++) {
+    for (int j = 0; j < cols - 1; j++) {
+      int pos = i * cols + j;
+      if (pos & 1) { // odd
+        tris->add_vertices(pos, pos + cols, pos + 1);
+        tris->add_vertices(pos + 1, pos + cols, pos + cols + 1);
+      }
+      else { // even
+        tris->add_vertices(pos, pos + cols, pos + cols + 1);
+        tris->add_vertices(pos, pos + cols + 1, pos + 1);
+      }
+    }
+  }
+
+  PT(Geom) geom = new Geom(vdata);
+  geom->add_primitive(tris);
+
+  _viz_geom->add_geom(geom, get_solid_viz_state());
+  _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state());
+}
+
+CollisionSolid *CollisionHeightfield::
+make_copy() {
+  return new CollisionHeightfield(*this);
+}
+
+LPoint3 CollisionHeightfield::
+get_collision_origin() const {
+  return LPoint3(0, 0, 0);
+}
+
+PT(BoundingVolume) CollisionHeightfield::
+compute_internal_bounds() const {
+  QuadTreeNode node = _nodes[0];
+  LPoint3 box_min = {node.area.min[0], node.area.min[1],
+                     node.height_min};
+  LPoint3 box_max = {node.area.max[0], node.area.max[1],
+                     node.height_max};
+  return new BoundingBox(box_min, box_max);
+}
+
+PStatCollector &CollisionHeightfield::
+get_volume_pcollector() {
+  return _volume_pcollector;
+}
+
+PStatCollector &CollisionHeightfield::
+get_test_pcollector() {
+  return _test_pcollector;
+}
+
+TypedWritable *CollisionHeightfield::
+make_CollisionHeightfield(const FactoryParams &params) {
+  CollisionHeightfield *me = new CollisionHeightfield;
+  DatagramIterator scan;
+  BamReader *manager;
+
+  parse_params(params, scan, manager);
+  me->fillin(scan, manager);
+  return me;
+}
+
+void CollisionHeightfield::
+fillin(DatagramIterator &scan, BamReader *manager) {
+}
+
+void CollisionHeightfield::
+register_with_read_factory() {
+  BamReader::get_factory()->register_factory(get_class_type(), make_CollisionHeightfield);
+}

panda/src/collide/collisionHeightfield.h

@@ -0,0 +1,180 @@
+/**
+ * PANDA 3D SOFTWARE
+ * Copyright (c) Carnegie Mellon University.  All rights reserved.
+ *
+ * All use of this software is subject to the terms of the revised BSD
+ * license.  You should have received a copy of this license along
+ * with this source code in a file named "LICENSE."
+ *
+ * @file collisionHeightfield.h
+ * @author hecris
+ * @date 2019-07-01
+ */
+
+#ifndef COLLISIONHEIGHTFIELD_H
+#define COLLISIONHEIGHTFIELD_H
+
+#include "pandabase.h"
+#include "collisionSolid.h"
+#include "pnmImage.h"
+
+/*
+ * CollisionHeightfield efficiently deals with collisions on uneven
+ * terrain given a heightfield image. A quad tree is implemented to
+ * significantly reduce the amount of triangles tested. Each quad
+ * tree node represents a sub-rectangle of the heightfield image
+ * and thus a box in 3D space.
+ * */
+class EXPCL_PANDA_COLLIDE CollisionHeightfield : public CollisionSolid {
+PUBLISHED:
+  CollisionHeightfield(PNMImage heightfield,
+                       PN_stdfloat max_height, int num_subdivisions);
+  virtual LPoint3 get_collision_origin() const;
+
+  INLINE PNMImage get_heightfield();
+  INLINE void set_heightfield(PNMImage heightfield);
+
+  INLINE PN_stdfloat get_max_height();
+  INLINE void set_max_height(PN_stdfloat max_height);
+
+  INLINE int get_num_subdivisions();
+  void set_num_subdivisions(int num_subdivisions);
+
+  INLINE PN_stdfloat get_height(int x, int y) const;
+
+protected:
+  struct Rect {
+    LVector2 min;
+    LVector2 max;
+  };
+
+  struct QuadTreeNode {
+    int index;
+    Rect area;
+    PN_stdfloat height_min;
+    PN_stdfloat height_max;
+  };
+
+  struct QuadTreeIntersection {
+    int node_index;
+    double tmin;
+    double tmax;
+    bool operator < (const QuadTreeIntersection& intersection) const {
+      return tmin < intersection.tmin;
+    }
+  };
+
+  struct IntersectionParams {
+    // From Line
+    double t1;
+    double t2;
+    LPoint3 from_origin;
+    LVector3 from_direction;
+    // From Sphere
+    LPoint3 center;
+    double radius;
+    // From Box
+    LPoint3 box_min;
+    LPoint3 box_max;
+  };
+
+  struct Triangle {
+    LPoint3 p1;
+    LPoint3 p2;
+    LPoint3 p3;
+  };
+
+private:
+  PNMImage _heightfield;
+  PN_stdfloat _max_height;
+  int _num_subdivisions;
+  QuadTreeNode *_nodes;
+  int _nodes_count;
+  int _leaf_first_index;
+  void fill_quadtree_areas();
+  void fill_quadtree_heights();
+  std::vector<Triangle> get_triangles(int x, int y) const;
+
+  // A pointer to a function that tests for intersection between a box and a
+  // solid defined by the given IntersectionParams.
+  typedef bool (*BoxIntersection)(const LPoint3 &box_min, const LPoint3 &box_max,
+                                  IntersectionParams &params);
+
+  std::vector<QuadTreeIntersection> find_intersections(BoxIntersection intersects_box,
+                                                  IntersectionParams params) const;
+
+protected:
+  static LPoint3 closest_point_on_triangle(const LPoint3 &p, const Triangle &triangle);
+
+  static bool line_intersects_box(const LPoint3 &box_min, const LPoint3 &box_max,
+                                  IntersectionParams &params);
+
+  static bool line_intersects_triangle(double &t, const LPoint3 &from,
+                                const LPoint3 &delta,
+                                const Triangle &triangle);
+
+  static bool sphere_intersects_box(const LPoint3 &box_min, const LPoint3 &box_max,
+                                    IntersectionParams &params);
+
+  static bool box_intersects_box(const LPoint3 &box_min, const LPoint3 &box_max,
+                                 IntersectionParams &params);
+
+  static bool box_intersects_triangle(const LPoint3 &box_min, const LPoint3 &box_max,
+                               const Triangle &triangle);
+
+protected:
+  virtual PT(CollisionEntry)
+  test_intersection_from_ray(const CollisionEntry &entry) const;
+  virtual PT(CollisionEntry)
+  test_intersection_from_sphere(const CollisionEntry &entry) const;
+  virtual PT(CollisionEntry)
+  test_intersection_from_box(const CollisionEntry &entry) const;
+
+  virtual void fill_viz_geom();
+
+public:
+  INLINE CollisionHeightfield(const CollisionHeightfield &copy);
+  virtual CollisionSolid *make_copy();
+
+  virtual PStatCollector &get_volume_pcollector();
+  virtual PStatCollector &get_test_pcollector();
+
+  INLINE static void flush_level();
+  INLINE ~CollisionHeightfield();
+
+protected:
+  virtual PT(BoundingVolume) compute_internal_bounds() const;
+
+private:
+  INLINE CollisionHeightfield();
+  static PStatCollector _volume_pcollector;
+  static PStatCollector _test_pcollector;
+
+protected:
+  static TypedWritable *make_CollisionHeightfield(const FactoryParams &params);
+  void fillin(DatagramIterator &scan, BamReader *manager);
+
+public:
+  static void register_with_read_factory();
+
+public:
+  static TypeHandle get_class_type() {
+    return _type_handle;
+  }
+  static void init_type() {
+    CollisionSolid::init_type();
+    register_type(_type_handle, "CollisionHeightfield",
+                  CollisionSolid::get_class_type());
+  }
+  virtual TypeHandle get_type() const {
+    return get_class_type();
+  }
+  virtual TypeHandle force_init_type() {init_type(); return get_class_type();}
+
+private:
+  static TypeHandle _type_handle;
+};
+
+#include "collisionHeightfield.I"
+
+#endif

panda/src/collide/collisionSolid.h

@@ -181,6 +181,7 @@ private:
   friend class CollisionParabola;
   friend class CollisionHandlerFluidPusher;
   friend class CollisionBox;
+  friend class CollisionHeightfield;
 };
 
 INLINE std::ostream &operator << (std::ostream &out, const CollisionSolid &cs) {

panda/src/collide/collisionTraverser.cxx

@@ -19,6 +19,7 @@
 #include "collisionRecorder.h"
 #include "collisionVisualizer.h"
 #include "collisionSphere.h"
+#include "collisionHeightfield.h"
 #include "collisionBox.h"
 #include "collisionCapsule.h"
 #include "collisionPolygon.h"
@@ -354,6 +355,7 @@ traverse(const NodePath &root) {
   CollisionPolygon::flush_level();
   CollisionPlane::flush_level();
   CollisionBox::flush_level();
+  CollisionHeightfield::flush_level();
 }
 
 #if defined(DO_COLLISION_RECORDING) || !defined(CPPPARSER)

panda/src/collide/config_collide.cxx

@@ -25,6 +25,7 @@
 #include "collisionHandlerFluidPusher.h"
 #include "collisionHandlerQueue.h"
 #include "collisionInvSphere.h"
+#include "collisionHeightfield.h"
 #include "collisionLine.h"
 #include "collisionLevelStateBase.h"
 #include "collisionGeom.h"
@@ -138,6 +139,7 @@ init_libcollide() {
   CollisionHandlerFluidPusher::init_type();
   CollisionHandlerQueue::init_type();
   CollisionInvSphere::init_type();
+  CollisionHeightfield::init_type();
   CollisionLine::init_type();
   CollisionLevelStateBase::init_type();
   CollisionGeom::init_type();
@@ -164,6 +166,7 @@ init_libcollide() {
   CollisionBox::register_with_read_factory();
   CollisionCapsule::register_with_read_factory();
   CollisionInvSphere::register_with_read_factory();
+  CollisionHeightfield::register_with_read_factory();
   CollisionLine::register_with_read_factory();
   CollisionNode::register_with_read_factory();
   CollisionParabola::register_with_read_factory();

panda/src/collide/p3collide_composite1.cxx

@@ -13,3 +13,4 @@
 #include "collisionHandlerFluidPusher.cxx"
 #include "collisionHandlerQueue.cxx"
 #include "collisionInvSphere.cxx"
+#include "collisionHeightfield.cxx"

tests/collide/collisions.py

@@ -2,7 +2,7 @@ from panda3d.core import CollisionNode, NodePath
 from panda3d.core import CollisionTraverser, CollisionHandlerQueue
 from panda3d.core import CollisionSphere, CollisionInvSphere, CollisionBox, CollisionPolygon, CollisionCapsule
 from panda3d.core import CollisionLine, CollisionRay, CollisionSegment, CollisionParabola
-from panda3d.core import CollisionPlane
+from panda3d.core import CollisionPlane, CollisionHeightfield
 from panda3d.core import Point3, Vec3, Plane, LParabola
 
 

tests/collide/test_into_heightfield.py

@@ -0,0 +1,92 @@
+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
+    num_subdivisions = 1
+    heightfield = CollisionHeightfield(img, max_height, num_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_num_subdivisions(-1) == err
+        assert heightfield.set_num_subdivisions(11) == err
+
+    # Use a greater number of subdivisions, should still work
+    num_subdivisions = 10
+    heightfield.set_num_subdivisions(num_subdivisions)
+    entry, np_from, np_into = make_collision(sphere, heightfield)
+    assert entry.get_surface_point(np_from) == (1, 510, 10.1)
+    # Using 10 subdivisions is overkill for such a small heightfield,
+    # CollisionHeightfield should've automatically decreased it
+    assert heightfield.get_num_subdivisions() < num_subdivisions
+    # Zero subdivisions should work too
+    num_subdivisions = 0
+    heightfield.set_num_subdivisions(num_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():
+    # Setup heightfield
+    img = PNMImage(127, 127, 1)
+    img.fill_val(0)
+    max_height = 10
+    num_subdivisions = 1
+    heightfield = CollisionHeightfield(img, max_height, num_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
+    num_subdivisions = 5
+    heightfield = CollisionHeightfield(img, max_height, num_subdivisions)
+    # Make box
+    box = CollisionBox((1, 5128, 10), 1, 1, 1)
+    entry = make_collision(box, heightfield)
+    assert entry is not None