Improve box-into-plane and box-into-poly collisions, add interior/surface points

panda/src/collide/collisionPlane.cxx

@@ -408,14 +408,19 @@ test_intersection_from_box(const CollisionEntry &entry) const {
   LVector3 from_extents = box->get_dimensions() * 0.5f;
   PN_stdfloat dist = _plane.dist_to_plane(from_center);
 
-  LVecBase3 box_x = wrt_mat.get_row3(0);
-  LVecBase3 box_y = wrt_mat.get_row3(1);
-  LVecBase3 box_z = wrt_mat.get_row3(2);
-
-  if (cabs(dist) >
-      cabs(box_x.dot(_plane.get_normal()) * from_extents[0]) +
-      cabs(box_y.dot(_plane.get_normal()) * from_extents[1]) +
-      cabs(box_z.dot(_plane.get_normal()) * from_extents[2])) {
+  // Determine the basis vectors describing the box.
+  LVecBase3 box_x = wrt_mat.get_row3(0) * from_extents[0];
+  LVecBase3 box_y = wrt_mat.get_row3(1) * from_extents[1];
+  LVecBase3 box_z = wrt_mat.get_row3(2) * from_extents[2];
+
+  // Project the box onto the normal vector of the plane to determine
+  // whether there is a separating axis.
+  PN_stdfloat dx = box_x.dot(_plane.get_normal());
+  PN_stdfloat dy = box_y.dot(_plane.get_normal());
+  PN_stdfloat dz = box_z.dot(_plane.get_normal());
+  PN_stdfloat depth = dist - (cabs(dx) + cabs(dy) + cabs(dz));
+
+  if (depth > 0) {
     // No collision.
     return NULL;
   }
@@ -428,9 +433,18 @@ test_intersection_from_box(const CollisionEntry &entry) const {
   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
 
   LVector3 normal = (has_effective_normal() && box->get_respect_effective_normal()) ? get_effective_normal() : get_normal();
-
   new_entry->set_surface_normal(normal);
-  new_entry->set_surface_point(from_center - get_normal() * dist);
+
+  // Determine which point on the cube will be the interior point.  If
+  // the points are equally close, this chooses their center instead.
+  LPoint3 interior_point = from_center +
+    box_x * ((dx < 0) - (dx > 0)) +
+    box_y * ((dy < 0) - (dy > 0)) +
+    box_z * ((dz < 0) - (dz > 0));
+
+  // The surface point is the interior point projected onto the plane.
+  new_entry->set_surface_point(interior_point - get_normal() * depth);
+  new_entry->set_interior_point(interior_point);
 
   return new_entry;
 }

panda/src/collide/collisionPolygon.cxx

@@ -907,39 +907,55 @@ test_intersection_from_box(const CollisionEntry &entry) const {
 
   // To make things easier, transform the box into the coordinate
   // space of the plane.
-  LMatrix4 wrt_mat = entry.get_wrt_mat() * _to_2d_mat;
+  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
+  LMatrix4 plane_mat = wrt_mat * _to_2d_mat;
 
-  LPoint3 from_center = box->get_center() * wrt_mat;
+  LPoint3 from_center = box->get_center() * plane_mat;
   LVector3 from_extents = box->get_dimensions() * 0.5f;
 
-  LVecBase3 box_x = wrt_mat.get_row3(0);
-  LVecBase3 box_y = wrt_mat.get_row3(1);
-  LVecBase3 box_z = wrt_mat.get_row3(2);
+  // Determine the basis vectors describing the box.
+  LVecBase3 box_x = plane_mat.get_row3(0) * from_extents[0];
+  LVecBase3 box_y = plane_mat.get_row3(1) * from_extents[1];
+  LVecBase3 box_z = plane_mat.get_row3(2) * from_extents[2];
 
   // Is there a separating axis between the plane and the box?
-  if (cabs(from_center[1]) >
-      cabs(box_x[1] * from_extents[0]) +
-      cabs(box_y[1] * from_extents[1]) +
-      cabs(box_z[1] * from_extents[2])) {
+  if (cabs(from_center[1]) > cabs(box_x[1]) + cabs(box_y[1]) + cabs(box_z[1])) {
     // There is one.  No collision.
     return NULL;
   }
 
+  // Now do the same for each of the box' primary axes.
+  PN_stdfloat r1, center, r2;
+
+  r1 = cabs(box_x.dot(box_x)) + cabs(box_y.dot(box_x)) + cabs(box_z.dot(box_x));
+  project(box_x, center, r2);
+  if (cabs(from_center.dot(box_x) - center) > r1 + r2) {
+    return NULL;
+  }
+
+  r1 = cabs(box_x.dot(box_y)) + cabs(box_y.dot(box_y)) + cabs(box_z.dot(box_y));
+  project(box_y, center, r2);
+  if (cabs(from_center.dot(box_y) - center) > r1 + r2) {
+    return NULL;
+  }
+
+  r1 = cabs(box_x.dot(box_z)) + cabs(box_y.dot(box_z)) + cabs(box_z.dot(box_z));
+  project(box_z, center, r2);
+  if (cabs(from_center.dot(box_z) - center) > r1 + r2) {
+    return NULL;
+  }
+
   // Now do the same check for the cross products between the box axes
   // and the polygon edges.
   Points::const_iterator pi;
   for (pi = _points.begin(); pi != _points.end(); ++pi) {
     const PointDef &pd = *pi;
-
     LVector3 axis;
-    PN_stdfloat r1, center, r2;
 
     axis.set(-box_x[1] * pd._v[1],
               box_x[0] * pd._v[1] - box_x[2] * pd._v[0],
               box_x[1] * pd._v[0]);
-    r1 = cabs(box_x.dot(axis) * from_extents[0]) +
-         cabs(box_y.dot(axis) * from_extents[1]) +
-         cabs(box_z.dot(axis) * from_extents[2]);
+    r1 = cabs(box_x.dot(axis)) + cabs(box_y.dot(axis)) + cabs(box_z.dot(axis));
     project(axis, center, r2);
     if (cabs(from_center.dot(axis) - center) > r1 + r2) {
       return NULL;
@@ -948,9 +964,7 @@ test_intersection_from_box(const CollisionEntry &entry) const {
     axis.set(-box_y[1] * pd._v[1],
               box_y[0] * pd._v[1] - box_y[2] * pd._v[0],
               box_y[1] * pd._v[0]);
-    r1 = cabs(box_x.dot(axis) * from_extents[0]) +
-         cabs(box_y.dot(axis) * from_extents[1]) +
-         cabs(box_z.dot(axis) * from_extents[2]);
+    r1 = cabs(box_x.dot(axis)) + cabs(box_y.dot(axis)) + cabs(box_z.dot(axis));
     project(axis, center, r2);
     if (cabs(from_center.dot(axis) - center) > r1 + r2) {
       return NULL;
@@ -959,9 +973,7 @@ test_intersection_from_box(const CollisionEntry &entry) const {
     axis.set(-box_z[1] * pd._v[1],
               box_z[0] * pd._v[1] - box_z[2] * pd._v[0],
               box_z[1] * pd._v[0]);
-    r1 = cabs(box_x.dot(axis) * from_extents[0]) +
-         cabs(box_y.dot(axis) * from_extents[1]) +
-         cabs(box_z.dot(axis) * from_extents[2]);
+    r1 = cabs(box_x.dot(axis)) + cabs(box_y.dot(axis)) + cabs(box_z.dot(axis));
     project(axis, center, r2);
     if (cabs(from_center.dot(axis) - center) > r1 + r2) {
       return NULL;
@@ -978,6 +990,18 @@ test_intersection_from_box(const CollisionEntry &entry) const {
   LVector3 normal = (has_effective_normal() && box->get_respect_effective_normal()) ? get_effective_normal() : get_normal();
   new_entry->set_surface_normal(normal);
 
+  // Determine which point on the cube will be the interior point.  This
+  // is the calculation that is also used for the plane, which is not
+  // perfectly applicable, but I suppose it's better than nothing.
+  LPoint3 interior_point = box->get_center() * wrt_mat +
+    wrt_mat.get_row3(0) * from_extents[0] * ((box_x[1] > 0) - (box_x[1] < 0)) +
+    wrt_mat.get_row3(1) * from_extents[1] * ((box_y[1] > 0) - (box_y[1] < 0)) +
+    wrt_mat.get_row3(2) * from_extents[2] * ((box_z[1] > 0) - (box_z[1] < 0));
+
+  // The surface point is the interior point projected onto the plane.
+  new_entry->set_surface_point(get_plane().project(interior_point));
+  new_entry->set_interior_point(interior_point);
+
   return new_entry;
 }