From 1891f56224993588a0dd2ceaca0ed448b68c51c2 Mon Sep 17 00:00:00 2001 From: rdb Date: Sun, 6 Dec 2015 13:11:38 +0100 Subject: [PATCH] Improve box-into-plane and box-into-poly collisions, add interior/surface points --- panda/src/collide/collisionPlane.cxx | 32 +++++++++---- panda/src/collide/collisionPolygon.cxx | 64 ++++++++++++++++++-------- 2 files changed, 67 insertions(+), 29 deletions(-) diff --git a/panda/src/collide/collisionPlane.cxx b/panda/src/collide/collisionPlane.cxx index 3f3c81fa5b..c7707e1cb8 100644 --- a/panda/src/collide/collisionPlane.cxx +++ b/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); + // 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]; - 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])) { + // 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; } diff --git a/panda/src/collide/collisionPolygon.cxx b/panda/src/collide/collisionPolygon.cxx index f3a64ebb66..a373382ba8 100644 --- a/panda/src/collide/collisionPolygon.cxx +++ b/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; }