mirror/panda3d
1
0
Fork 0
mirror of https://github.com/panda3d/panda3d.git synced 2025-12-21 06:29:52 -06:00
Code Issues Packages Projects Releases Wiki Activity

Revert "collide: Rewrite sphere-into-box collision test"

Browse Source 
This reverts commit a1fd79c80b.
This commit is contained in:
rdb
2025-01-29 17:07:56 +01:00
parent d13622db9a
commit 31be0b4e7a
2 changed files with 169 additions and 219 deletions
Download Patch File Download Diff File Expand all files Collapse all files

385
panda/src/collide/collisionBox.cxx
View File

@@ -54,102 +54,6 @@ const int CollisionBox::plane_def[6][4] = {
{2, 6, 4, 0},
};
/**
* Helper function to calculate the intersection between a line segment and a
* sphere. t is filled with the first position along the line segment where
* the intersection hits.
*/
static bool
intersect_segment_sphere(double &t,
const LPoint3 &from, const LVector3 &delta,
const LPoint3 &center, double radius_sq) {
double A2 = dot(delta, delta) * 2;
LVector3 fc = from - center;
double fc_d2 = dot(fc, fc);
double C = fc_d2 - radius_sq;
if (UNLIKELY(A2 == 0.0)) {
// Degenerate case where delta is zero. This is effectively a test
// against a point (or sphere, for nonzero inflate_radius).
t = 0.0;
return C < 0.0;
}
double B = 2.0f * dot(delta, fc);
double radical = B*B - 2.0*A2*C;
if (radical < 0.0) {
// No real roots: no intersection with the line.
return false;
}
t = (-B - csqrt(radical)) / A2;
return true;
}
/**
* Helper function to calculate the intersection between a line segment and a
* capsule. t is filled with the first position along the line segment where
* the intersection hits.
*
* Code derived from a book by Christer Ericson.
*/
static bool
intersect_segment_capsule(double &t,
const LPoint3 &from_a, const LVector3 &delta_a,
const LPoint3 &from_b, const LPoint3 &to_b,
double radius_sq) {
LVector3 m = from_a - from_b;
LVector3 delta_b = to_b - from_b;
PN_stdfloat md = m.dot(delta_b);
PN_stdfloat nd = delta_a.dot(delta_b);
PN_stdfloat dd = delta_b.dot(delta_b);
if (md < 0 && md + nd < 0) {
return intersect_segment_sphere(t, from_a, delta_a, from_b, radius_sq);
}
if (md > dd && md + nd > dd) {
return intersect_segment_sphere(t, from_a, delta_a, to_b, radius_sq);
}
PN_stdfloat nn = delta_a.dot(delta_a);
PN_stdfloat mn = m.dot(delta_a);
PN_stdfloat a = dd * nn - nd * nd;
PN_stdfloat k = m.dot(m) - radius_sq;
PN_stdfloat c = dd * k - md * md;
if (IS_NEARLY_ZERO(a)) {
// Segments run parallel
if (c > 0.0f) {
return false;
}
if (md < 0.0f) {
return intersect_segment_sphere(t, from_a, delta_a, from_b, radius_sq);
}
else if (md > dd) {
return intersect_segment_sphere(t, from_a, delta_a, to_b, radius_sq);
}
else {
t = 0.0;
}
return true;
}
PN_stdfloat b = dd * mn - nd * md;
PN_stdfloat discr = b * b - a * c;
if (discr < 0.0f) {
return false;
}
t = (-b - csqrt(discr)) / a;
if (t < 0.0 || t > 1.0) {
return false;
}
if (md + t * nd < 0) {
return intersect_segment_sphere(t, from_a, delta_a, from_b, radius_sq);
}
else if (md + t * nd > dd) {
return intersect_segment_sphere(t, from_a, delta_a, to_b, radius_sq);
}
return true;
}
/**
*
*/
@@ -286,45 +190,168 @@ test_intersection_from_sphere(const CollisionEntry &entry) const {
const LMatrix4 &wrt_mat = wrt_space->get_mat();
LPoint3 center = wrt_mat.xform_point(sphere->get_center());
PN_stdfloat radius_sq = wrt_mat.xform_vec(LVector3(0, 0, sphere->get_radius())).length_squared();
LPoint3 orig_center = sphere->get_center() * wrt_mat;
LPoint3 from_center = orig_center;
bool moved_from_center = false;
PN_stdfloat t = 1.0f;
LPoint3 contact_point(from_center);
PN_stdfloat actual_t = 1.0f;
bool had_prev = false;
LPoint3 prev_center = center;
double t = 0;
LVector3 from_radius_v =
LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
PN_stdfloat from_radius_2 = from_radius_v.length_squared();
PN_stdfloat from_radius = csqrt(from_radius_2);
if (wrt_space != wrt_prev_space) {
prev_center = wrt_prev_space->get_mat().xform_point(sphere->get_center());
}
LPoint3 contact_center = prev_center;
int ip;
PN_stdfloat max_dist = 0.0;
PN_stdfloat dist = 0.0;
bool intersect;
LPlane plane;
LVector3 normal;
bool fully_inside = true;
// First, just test the starting point of the sphere.
LVector3 vec = (prev_center - _min).fmin(0) + (prev_center - _max).fmax(0);
PN_stdfloat vec_lsq = vec.length_squared();
if (vec_lsq > radius_sq) {
if (wrt_space == wrt_prev_space) {
return nullptr;
for(ip = 0, intersect = false; ip < 6 && !intersect; ip++) {
plane = get_plane(ip);
if (wrt_prev_space != wrt_space) {
// If we have a delta between the previous position and the current
// position, we use that to determine some more properties of the
// collision.
LPoint3 b = from_center;
LPoint3 a = sphere->get_center() * wrt_prev_space->get_mat();
LVector3 delta = b - a;
// First, there is no collision if the "from" object is definitely
// moving in the same direction as the plane's normal.
PN_stdfloat dot = delta.dot(plane.get_normal());
if (dot > 0.1f) {
fully_inside = false;
continue; // no intersection
}
if (IS_NEARLY_ZERO(dot)) {
// If we're moving parallel to the plane, the sphere is tested at its
// final point. Leave it as it is.
} else {
/*
* Otherwise, we're moving into the plane; the sphere is tested at the point
* along its path that is closest to intersecting the plane. This may be the
* actual intersection point, or it may be the starting point or the final
* point. dot is equal to the (negative) magnitude of 'delta' along the
* direction of the plane normal t = ratio of (distance from start pos to
* plane) to (distance from start pos to end pos), along axis of plane normal
*/
PN_stdfloat dist_to_p = plane.dist_to_plane(a);
t = (dist_to_p / -dot);
// also compute the actual contact point and time of contact for
// handlers that need it
actual_t = ((dist_to_p - from_radius) / -dot);
actual_t = min((PN_stdfloat)1.0, max((PN_stdfloat)0.0, actual_t));
contact_point = a + (actual_t * delta);
if (t >= 1.0f) {
// Leave it where it is.
} else if (t < 0.0f) {
from_center = a;
moved_from_center = true;
} else {
from_center = a + t * delta;
moved_from_center = true;
}
}
}
// We must effectively do a capsule-into-box test.
LVector3 delta = center - prev_center;
if (!intersects_capsule(t, prev_center, delta, radius_sq)) {
normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : plane.get_normal();
#ifndef NDEBUG
/*if (!IS_THRESHOLD_EQUAL(normal.length_squared(), 1.0f, 0.001), NULL) {
std::cout
<< "polygon within " << entry.get_into_node_path()
<< " has normal " << normal << " of length " << normal.length()
<< "\n";
normal.normalize();
}*/
#endif
// The nearest point within the plane to our center is the intersection of
// the line (center, center - normal) with the plane.
if (!plane.intersects_line(dist, from_center, -(plane.get_normal()))) {
// No intersection with plane? This means the plane's effective normal
// was within the plane itself. A useless polygon.
fully_inside = false;
continue;
}
if (dist > from_radius) {
// Fully outside this plane, there can not be an intersection.
return nullptr;
}
contact_center = prev_center + delta * t;
if (dist < -from_radius) {
// Fully inside this plane.
continue;
}
fully_inside = false;
// This is used to calculate the surface normal, which must always be
// opposed to the movement direction!
vec = (contact_center - _min).fmin(0) + (contact_center - _max).fmax(0);
if ((vec[0] > 0) == (delta[0] > 0)) vec[0] = 0;
if ((vec[1] > 0) == (delta[1] > 0)) vec[1] = 0;
if ((vec[2] > 0) == (delta[2] > 0)) vec[2] = 0;
LPoint2 p = to_2d(from_center - dist * plane.get_normal(), ip);
PN_stdfloat edge_dist = 0.0f;
had_prev = true;
const ClipPlaneAttrib *cpa = entry.get_into_clip_planes();
if (cpa != nullptr) {
// We have a clip plane; apply it.
Points new_points;
if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform(),ip)) {
// All points are behind the clip plane; just do the default test.
edge_dist = dist_to_polygon(p, _points[ip], 4);
} else if (new_points.empty()) {
// The polygon is completely clipped.
continue;
} else {
// Test against the clipped polygon.
edge_dist = dist_to_polygon(p, new_points.data(), new_points.size());
}
} else {
// No clip plane is in effect. Do the default test.
edge_dist = dist_to_polygon(p, _points[ip], 4);
}
max_dist = from_radius;
// Now we have edge_dist, which is the distance from the sphere center to
// the nearest edge of the polygon, within the polygon's plane.
// edge_dist<0 means the point is within the polygon.
if(edge_dist < 0) {
intersect = true;
continue;
}
if((edge_dist > 0) &&
((edge_dist * edge_dist + dist * dist) > from_radius_2)) {
// No intersection; the circle is outside the polygon.
continue;
}
// The sphere appears to intersect the polygon. If the edge is less than
// from_radius away, the sphere may be resting on an edge of the polygon.
// Determine how far the center of the sphere must remain from the plane,
// based on its distance from the nearest edge.
if (edge_dist >= 0.0f) {
PN_stdfloat max_dist_2 = max(from_radius_2 - edge_dist * edge_dist, (PN_stdfloat)0.0);
max_dist = csqrt(max_dist_2);
}
if (dist > max_dist) {
// There's no intersection: the sphere is hanging off the edge.
continue;
}
intersect = true;
}
else if (vec_lsq == 0.0f) {
// It's completely inside.
vec = prev_center - _center;
if (!fully_inside && !intersect) {
return nullptr;
}
if (collide_cat.is_debug()) {
@@ -335,44 +362,26 @@ test_intersection_from_sphere(const CollisionEntry &entry) const {
PT(CollisionEntry) new_entry = new CollisionEntry(entry);
int axis;
if (abs(vec[0]) > abs(vec[1])) {
if (abs(vec[0]) > abs(vec[2])) {
axis = 0;
} else {
axis = 2;
}
} else {
if (abs(vec[1]) > abs(vec[2])) {
axis = 1;
} else {
axis = 2;
}
PN_stdfloat into_depth = max_dist - dist;
if (moved_from_center) {
// We have to base the depth of intersection on the sphere's final resting
// point, not the point from which we tested the intersection.
PN_stdfloat orig_dist;
plane.intersects_line(orig_dist, orig_center, -normal);
into_depth = max_dist - orig_dist;
}
LPoint3 surface_point = contact_center.fmax(_min).fmin(_max);
surface_point[axis] = vec[axis] > 0 ? _max[axis] : _min[axis];
// Clamp the surface point to the box bounds.
LPoint3 surface = from_center - normal * dist;
surface = surface.fmax(_min);
surface = surface.fmin(_max);
LVector3 normal(0, 0, 0);
normal[axis] = (vec[axis] > 0) * 2 - 1;
LPoint3 interior_point = surface_point;
if (had_prev) {
interior_point += (center - contact_center);
} else {
LVector3 other = surface_point - contact_center;
other[axis] = 0.0f;
interior_point[axis] = center[axis] - std::copysign(std::max((PN_stdfloat)0, csqrt(radius_sq - other.length_squared())), vec[axis]);
}
new_entry->set_interior_point(interior_point);
new_entry->set_surface_point(surface_point);
new_entry->set_surface_normal(
(has_effective_normal() && sphere->get_respect_effective_normal())
? get_effective_normal() : normal);
new_entry->set_contact_pos(contact_center);
new_entry->set_contact_normal(normal);
new_entry->set_t(t);
new_entry->set_surface_normal(normal);
new_entry->set_surface_point(surface);
new_entry->set_interior_point(surface - normal * into_depth);
new_entry->set_contact_pos(contact_point);
new_entry->set_contact_normal(plane.get_normal());
new_entry->set_t(actual_t);
return new_entry;
}
@@ -1078,62 +1087,6 @@ intersects_line(double &t1, double &t2,
return true;
}
/**
* Determine the first point of intersection of the given capsule with the box.
*/
bool CollisionBox::
intersects_capsule(double &t, const LPoint3 &from, const LVector3 &delta,
PN_stdfloat radius_sq) const {
// First, we check whether the line segment intersects with the box
// expanded by the sphere's radius.
double t2;
if (!intersects_line(t, t2, from, delta, csqrt(radius_sq)) ||
t > 1.0 || t2 < 0.0) {
return false;
}
LPoint3 intersection = from + delta * t;
// The following technique is derived from a book by Christer Ericson.
int u = 0, v = 0;
if (intersection[0] < _min[0]) u |= 4;
if (intersection[1] < _min[1]) u |= 2;
if (intersection[2] < _min[2]) u |= 1;
if (intersection[0] > _max[0]) v |= 4;
if (intersection[1] > _max[1]) v |= 2;
if (intersection[2] > _max[2]) v |= 1;
int m = u | v;
if (m == 7) {
double tmin = DBL_MAX;
LPoint3 vertex = get_point_aabb(v);
if (intersect_segment_capsule(t, from, delta, vertex,
get_point_aabb(v ^ 4), radius_sq)) {
tmin = std::min(t, tmin);
}
if (intersect_segment_capsule(t, from, delta, vertex,
get_point_aabb(v ^ 2), radius_sq)) {
tmin = std::min(t, tmin);
}
if (intersect_segment_capsule(t, from, delta, vertex,
get_point_aabb(v ^ 1), radius_sq)) {
tmin = std::min(t, tmin);
}
if (tmin == DBL_MAX) {
return false;
}
t = tmin;
}
else if ((m & (m - 1)) != 0) {
// There's just one edge to test.
LPoint3 edge_v1 = get_point_aabb(u ^ 7);
LPoint3 edge_v2 = get_point_aabb(v);
return intersect_segment_capsule(t, from, delta, edge_v1, edge_v2, radius_sq);
}
return true;
}
/**
* Clips the polygon by all of the clip planes named in the clip plane
* attribute and fills new_points up with the resulting points.

3
panda/src/collide/collisionBox.h
View File

@@ -94,9 +94,6 @@ protected:
bool intersects_line(double &t1, double &t2,
const LPoint3 &from, const LVector3 &delta,
PN_stdfloat inflate_size=0) const;
bool intersects_capsule(double &t,
const LPoint3 &from, const LVector3 &delta,
PN_stdfloat radius_sq) const;
private:
LPoint3 _center;