collide: implement box into capsule

Closes #675

panda/src/collide/collisionCapsule.cxx

@@ -12,6 +12,7 @@
  */
 
 #include "collisionCapsule.h"
+#include "collisionBox.h"
 #include "collisionSphere.h"
 #include "collisionLine.h"
 #include "collisionRay.h"
@@ -140,6 +141,87 @@ compute_internal_bounds() const {
   return bound;
 }
 
+/**
+ *
+ */
+PT(CollisionEntry) CollisionCapsule::
+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();
+  const LMatrix4 inv_wrt_mat = entry.get_inv_wrt_mat();
+
+  // We find the closest point on the box to the line
+  // segment defined by the endpoints of the capsule. To do
+  // this, convert the capsule into the box's coordinate space.
+  LPoint3 box_min = box->get_min();
+  LPoint3 box_max = box->get_max();
+  LPoint3 a = _a * inv_wrt_mat;
+  LPoint3 b = _b * inv_wrt_mat;
+  LVector3 delta = b - a;
+
+  double min_dist = DBL_MAX;
+  PN_stdfloat t;
+  LPoint3 closest_point;
+  // Iterate through the 6 planes of the box
+  for (int i = 0; i < 6; i++) {
+    if (!box->get_plane(i).intersects_line(t, a, delta)) {
+      continue;
+    }
+    if (t > 1.0) t = 1.0;
+    if (t < 0.0) t = 0.0;
+    if (t == min_dist) continue;
+    LPoint3 intersection_point = a + delta * t;
+    // Find the closest point on the box to the intersection point
+    LPoint3 p = intersection_point.fmax(box_min).fmin(box_max);
+    double dist = (p - intersection_point).length_squared();
+    if (dist < min_dist) {
+      min_dist = dist;
+      closest_point = p;
+    }
+  }
+  // Convert the closest point to the capsule's coordinate
+  // space where the capsule is aligned with the y axis.
+  closest_point = closest_point * wrt_mat * _inv_mat;
+  a = _a * _inv_mat;
+  b = _b * _inv_mat;
+
+  // Find the closest point on the line segment
+  LPoint3 point_on_segment;
+  point_on_segment[0] = a[0];
+  point_on_segment[2] = a[2];
+  if (closest_point[1] < std::min(a[1], b[1])) {
+    point_on_segment[1] = std::min(a[1], b[1]);
+  } else if (closest_point[1] > std::max(a[1], b[1])) {
+    point_on_segment[1] = std::max(a[1], b[1]);
+  } else {
+    point_on_segment[1] = closest_point[1];
+  }
+
+  if ((closest_point - point_on_segment).length_squared() > _radius * _radius) {
+    // No intersection.
+    return nullptr;
+  }
+
+  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
+  LVector3 normal;
+  if (point_on_segment == closest_point) {
+    // If the box directly intersects the line segment, use
+    // the box's center to determine the surface normal.
+    normal = (box->get_center() * wrt_mat * _inv_mat) - point_on_segment;
+    if (closest_point != a && closest_point != b) normal[1] = 0;
+  } else {
+    normal = (closest_point - point_on_segment);
+  }
+  normal.normalize();
+  LPoint3 surface_point = point_on_segment + normal * _radius;
+  new_entry->set_interior_point(_mat.xform_point(closest_point));
+  new_entry->set_surface_point(_mat.xform_point(surface_point));
+  new_entry->set_surface_normal(_mat.xform_vec(normal));
+  return new_entry;
+}
+
 /**
  *
  */

panda/src/collide/collisionCapsule.h

@@ -74,6 +74,8 @@ protected:
   virtual PT(BoundingVolume) compute_internal_bounds() const;
 
 protected:
+  virtual PT(CollisionEntry)
+  test_intersection_from_box(const CollisionEntry &entry) const;
   virtual PT(CollisionEntry)
   test_intersection_from_sphere(const CollisionEntry &entry) const;
   virtual PT(CollisionEntry)

tests/collide/test_into_capsule.py

@@ -0,0 +1,39 @@
+from collisions import *
+
+def test_box_into_capsule():
+    capsule = CollisionCapsule((1, 0, 0), (-1, 0, 0), .5)
+
+    # colliding just on the capsule's cylinder
+    box = CollisionBox((0, 1, 0), .5, .5, .5)
+    entry = make_collision(box, capsule)[0]
+    assert entry is not None
+
+    # no longer colliding
+    box = CollisionBox((0, 1.1, 0), .5, .5, .5)
+    entry = make_collision(box, capsule)[0]
+    assert entry is None
+
+    # box is inside the capsule's cylinder
+    box = CollisionBox((0, .8, 0), .5, .5, .5)
+    entry = make_collision(box, capsule)[0]
+    assert entry is not None
+
+    # colliding with the first endcap
+    box = CollisionBox((2, 0, 0), .5, .5, .5)
+    entry = make_collision(box, capsule)[0]
+    assert entry is not None
+
+    # almost colliding with first endcap
+    box = CollisionBox((2.01, 0, 0), .5, .5, .5)
+    entry = make_collision(box, capsule)[0]
+    assert entry is None
+
+    # colliding with the second endcap
+    box = CollisionBox((-2, 0, 0), .5, .5, .5)
+    entry = make_collision(box, capsule)[0]
+    assert entry is not None
+
+    # almost colliding with second endcap
+    box = CollisionBox((-2.01, 0, 0), .5, .5, .5)
+    entry = make_collision(box, capsule)[0]
+    assert entry is None