TheCloud/panda3d
1
0
Fork 0
mirror of https://github.com/panda3d/panda3d.git synced 2025-09-29 08:15:18 -04:00
Code Issues Packages Projects Releases Wiki Activity

collide: Improve box-into-polygon test

Browse Source 
* Use nearly-zero test to check which edges to snap the interior/surface points to, if any, to be less sensitive to fp precision
* The respect-prev-transform test now tests for a sphere that fits inside the box, to prevent a false positive test when the box has been rotated since the last test
* More appropriate surface point for respect-prev-transform test
This commit is contained in:
rdb 2021-01-12 14:53:42 +01:00
parent 1b7f3e4bbd
commit 3f08781eac
1 changed files with 97 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

171
panda/src/collide/collisionPolygon.cxx
View File

@ -1035,88 +1035,108 @@ test_intersection_from_box(const CollisionEntry &entry) const {
LVecBase3 box_y = plane_mat.get_row3(1) * from_extents[1];
LVecBase3 box_z = plane_mat.get_row3(2) * from_extents[2];
PT(CollisionEntry) new_entry;
// Is there a separating axis between the plane and the box?
if (cabs(from_center[1]) > cabs(box_x[1]) + cabs(box_y[1]) + cabs(box_z[1])) {
// There is one. No collision.
if (cabs(from_center[1]) <= cabs(box_x[1]) + cabs(box_y[1]) + cabs(box_z[1])) {
// No, there isn't. Now do the same test 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 nullptr;
}
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 nullptr;
}
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 nullptr;
}
// 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;
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)) + cabs(box_y.dot(axis)) + cabs(box_z.dot(axis));
project(axis, center, r2);
if (cabs(from_center.dot(axis) - center) > r1 + r2) {
return nullptr;
}
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)) + cabs(box_y.dot(axis)) + cabs(box_z.dot(axis));
project(axis, center, r2);
if (cabs(from_center.dot(axis) - center) > r1 + r2) {
return nullptr;
}
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)) + cabs(box_y.dot(axis)) + cabs(box_z.dot(axis));
project(axis, center, r2);
if (cabs(from_center.dot(axis) - center) > r1 + r2) {
return nullptr;
}
}
new_entry = new CollisionEntry(entry);
}
else {
// There is a separating axis. No collision, but the box may have moved
// through the polygon since the last frame.
if (from_center[1] < 0.0f || !entry.get_respect_prev_transform()) {
return nullptr;
}
CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
if (wrt_prev_space != wrt_space) {
// Did the center travel into the plane of the polygon?
LPoint3 prev_center = box->get_center() * (wrt_prev_space->get_mat() * _to_2d_mat);
if (prev_center[1] > 0.0f) {
// Nope, it did not.
return nullptr;
}
// Do the rest of the test with the box placed between the previous and
// current positions, such that the box center is exactly at the plane.
// Note that this ensures there is no separating axis between the plane
// and the box, so we don't need to do that test again.
PN_stdfloat t = from_center[1] / (from_center[1] - prev_center[1]);
from_center.set(
prev_center[0] * t + from_center[0] * (1.0f - t),
0.0f,
prev_center[2] * t + from_center[2] * (1.0f - t));
}
}
// 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 nullptr;
}
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 nullptr;
}
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 nullptr;
}
// 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;
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)) + cabs(box_y.dot(axis)) + cabs(box_z.dot(axis));
project(axis, center, r2);
if (cabs(from_center.dot(axis) - center) > r1 + r2) {
if (wrt_prev_space == wrt_space) {
return nullptr;
}
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)) + cabs(box_y.dot(axis)) + cabs(box_z.dot(axis));
project(axis, center, r2);
if (cabs(from_center.dot(axis) - center) > r1 + r2) {
// Did the center travel into the plane of the polygon?
LPoint3 prev_center = box->get_center() * (wrt_prev_space->get_mat() * _to_2d_mat);
if (prev_center[1] > 0.0f) {
// Nope, it did not.
return nullptr;
}
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)) + cabs(box_y.dot(axis)) + cabs(box_z.dot(axis));
project(axis, center, r2);
if (cabs(from_center.dot(axis) - center) > r1 + r2) {
// We don't know how much the box has been rotated in the meantime, so we
// test a circle that we know fits inside the box, positioned at the box
// center as it was going through the plane.
PN_stdfloat t = from_center[1] / (from_center[1] - prev_center[1]);
LPoint2 p(
prev_center[0] * t + from_center[0] * (1.0f - t),
prev_center[2] * t + from_center[2] * (1.0f - t));
LPoint2 edge_p(p);
PN_stdfloat edge_dist = dist_to_polygon(p, edge_p, _points);
if (edge_dist > from_extents[0] ||
edge_dist > from_extents[1] ||
edge_dist > from_extents[2]) {
return nullptr;
}
new_entry = new CollisionEntry(entry);
// This is not quite correct but close enough.
LMatrix4 to_3d_mat;
rederive_to_3d_mat(to_3d_mat);
new_entry->set_surface_point(to_3d(edge_p, to_3d_mat));
}
if (collide_cat.is_debug()) {
@ -1124,7 +1144,6 @@ test_intersection_from_box(const CollisionEntry &entry) const {
<< "intersection detected from " << entry.get_from_node_path()
<< " into " << entry.get_into_node_path() << "\n";
}
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);
@ -1132,14 +1151,18 @@ test_intersection_from_box(const CollisionEntry &entry) const {
// 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.
const PN_stdfloat nearly_zero = get_nearly_zero_value((PN_stdfloat)0);
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));
wrt_mat.get_row3(0) * from_extents[0] * ((box_x[1] > nearly_zero) - (box_x[1] < -nearly_zero)) +
wrt_mat.get_row3(1) * from_extents[1] * ((box_y[1] > nearly_zero) - (box_y[1] < -nearly_zero)) +
wrt_mat.get_row3(2) * from_extents[2] * ((box_z[1] > nearly_zero) - (box_z[1] < -nearly_zero));
new_entry->set_interior_point(interior_point);
// 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);
if (!new_entry->has_surface_point()) {
new_entry->set_surface_point(get_plane().project(interior_point));
}
return new_entry;
}