197compute_internal_bounds()
const {
208 DCAST_INTO_R(sphere, entry.
get_from(),
nullptr);
213 const LMatrix4 &wrt_mat = wrt_space->get_mat();
215 LPoint3 orig_center = sphere->get_center() * wrt_mat;
216 LPoint3 from_center = orig_center;
217 bool moved_from_center =
false;
218 PN_stdfloat t = 1.0f;
219 LPoint3 contact_point(from_center);
220 PN_stdfloat actual_t = 1.0f;
222 LVector3 from_radius_v =
223 LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
224 PN_stdfloat from_radius_2 = from_radius_v.length_squared();
225 PN_stdfloat from_radius = csqrt(from_radius_2);
228 PN_stdfloat max_dist = 0.0;
229 PN_stdfloat dist = 0.0;
233 bool fully_inside =
true;
235 for(ip = 0, intersect =
false; ip < 6 && !intersect; ip++) {
237 if (_points[ip].size() < 3) {
238 fully_inside =
false;
241 if (wrt_prev_space != wrt_space) {
245 LPoint3 b = from_center;
246 LPoint3 a = sphere->get_center() * wrt_prev_space->get_mat();
247 LVector3 delta = b - a;
251 PN_stdfloat dot = delta.dot(plane.get_normal());
253 fully_inside =
false;
257 if (IS_NEARLY_ZERO(dot)) {
270 PN_stdfloat dist_to_p = plane.dist_to_plane(a);
271 t = (dist_to_p / -dot);
275 actual_t = ((dist_to_p - from_radius) / -dot);
276 actual_t = min((PN_stdfloat)1.0, max((PN_stdfloat)0.0, actual_t));
277 contact_point = a + (actual_t * delta);
282 }
else if (t < 0.0f) {
284 moved_from_center =
true;
286 from_center = a + t * delta;
287 moved_from_center =
true;
307 if (!plane.intersects_line(dist, from_center, -(plane.get_normal()))) {
310 fully_inside =
false;
314 if (dist > from_radius) {
318 if (dist < -from_radius) {
322 fully_inside =
false;
324 LPoint2 p =
to_2d(from_center - dist * plane.get_normal(), ip);
325 PN_stdfloat edge_dist = 0.0f;
328 if (cpa !=
nullptr) {
334 }
else if (new_points.empty()) {
346 max_dist = from_radius;
356 if((edge_dist > 0) &&
357 ((edge_dist * edge_dist + dist * dist) > from_radius_2)) {
367 if (edge_dist >= 0.0f) {
368 PN_stdfloat max_dist_2 = max(from_radius_2 - edge_dist * edge_dist, (PN_stdfloat)0.0);
369 max_dist = csqrt(max_dist_2);
372 if (dist > max_dist) {
378 if (!fully_inside && !intersect) {
382 if (collide_cat.is_debug()) {
390 PN_stdfloat into_depth = max_dist - dist;
391 if (moved_from_center) {
394 PN_stdfloat orig_dist;
395 plane.intersects_line(orig_dist, orig_center, -normal);
396 into_depth = max_dist - orig_dist;
400 LPoint3 surface = from_center - normal * dist;
401 surface = surface.fmax(_min);
402 surface = surface.fmin(_max);
404 new_entry->set_surface_normal(normal);
405 new_entry->set_surface_point(surface);
406 new_entry->set_interior_point(surface - normal * into_depth);
407 new_entry->set_contact_pos(contact_point);
408 new_entry->set_contact_normal(plane.get_normal());
409 new_entry->set_t(actual_t);
420 DCAST_INTO_R(line, entry.
get_from(),
nullptr);
422 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
424 LPoint3 from_origin = line->get_origin() * wrt_mat;
425 LVector3 from_direction = line->get_direction() * wrt_mat;
428 if (!intersects_line(t1, t2, from_origin, from_direction)) {
433 if (collide_cat.is_debug()) {
440 LPoint3 point = from_origin + t1 * from_direction;
441 new_entry->set_surface_point(point);
447 IS_NEARLY_EQUAL(point[0], _max[0]) - IS_NEARLY_EQUAL(point[0], _min[0]),
448 IS_NEARLY_EQUAL(point[1], _max[1]) - IS_NEARLY_EQUAL(point[1], _min[1]),
449 IS_NEARLY_EQUAL(point[2], _max[2]) - IS_NEARLY_EQUAL(point[2], _min[2])
452 new_entry->set_surface_normal(normal);
464 DCAST_INTO_R(ray, entry.
get_from(),
nullptr);
465 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
467 LPoint3 from_origin = ray->get_origin() * wrt_mat;
468 LVector3 from_direction = ray->get_direction() * wrt_mat;
471 if (!intersects_line(t1, t2, from_origin, from_direction) || (t1 < 0.0 && t2 < 0.0)) {
476 if (collide_cat.is_debug()) {
485 new_entry->set_interior_point(from_origin);
489 LPoint3 point = from_origin + t1 * from_direction;
490 new_entry->set_surface_point(point);
496 IS_NEARLY_EQUAL(point[0], _max[0]) - IS_NEARLY_EQUAL(point[0], _min[0]),
497 IS_NEARLY_EQUAL(point[1], _max[1]) - IS_NEARLY_EQUAL(point[1], _min[1]),
498 IS_NEARLY_EQUAL(point[2], _max[2]) - IS_NEARLY_EQUAL(point[2], _min[2])
501 new_entry->set_surface_normal(normal);
511test_intersection_from_segment(
const CollisionEntry &entry)
const {
513 DCAST_INTO_R(seg, entry.
get_from(),
nullptr);
514 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
516 LPoint3 from_origin = seg->get_point_a() * wrt_mat;
517 LPoint3 from_extent = seg->get_point_b() * wrt_mat;
518 LVector3 from_direction = from_extent - from_origin;
521 if (!intersects_line(t1, t2, from_origin, from_direction) ||
522 (t1 < 0.0 && t2 < 0.0) || (t1 > 1.0 && t2 > 1.0)) {
527 if (collide_cat.is_debug()) {
536 if (t1 < (1.0 - t2)) {
541 new_entry->set_interior_point(from_origin + std::min(std::max(t2, 0.0), 1.0) * from_direction);
543 LPoint3 point = from_origin + t1 * from_direction;
544 new_entry->set_surface_point(point);
550 IS_NEARLY_EQUAL(point[0], _max[0]) - IS_NEARLY_EQUAL(point[0], _min[0]),
551 IS_NEARLY_EQUAL(point[1], _max[1]) - IS_NEARLY_EQUAL(point[1], _min[1]),
552 IS_NEARLY_EQUAL(point[2], _max[2]) - IS_NEARLY_EQUAL(point[2], _min[2])
555 new_entry->set_surface_normal(normal);
565test_intersection_from_capsule(
const CollisionEntry &entry)
const {
567 DCAST_INTO_R(capsule, entry.
get_from(),
nullptr);
569 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
571 LPoint3 from_a = capsule->get_point_a() * wrt_mat;
572 LPoint3 from_b = capsule->get_point_b() * wrt_mat;
573 LVector3 from_direction = from_b - from_a;
574 PN_stdfloat radius_sq = wrt_mat.xform_vec(LVector3(0, 0, capsule->get_radius())).length_squared();
575 PN_stdfloat radius = csqrt(radius_sq);
577 LPoint3 box_min = get_min();
578 LPoint3 box_max = get_max();
579 LVector3 dimensions = box_max - box_min;
588 if (!intersects_line(t1, t2, from_a, from_direction, radius)) {
592 if (t2 < 0.0 || t1 > 1.0) {
596 t1 = std::min(1.0, std::max(0.0, (t1 + t2) * 0.5));
597 LPoint3 point = from_a + from_direction * t1;
604 if ((point[0] < box_min[0] || point[0] > box_max[0]) +
605 (point[1] < box_min[1] || point[1] > box_max[1]) +
606 (point[2] < box_min[2] || point[2] > box_max[2]) > 1) {
608 static const struct {
626 PN_stdfloat best_dist_sq = FLT_MAX;
628 for (
int i = 0; i < 12; ++i) {
629 LPoint3 vertex = edges[i].point;
630 vertex.componentwise_mult(dimensions);
633 delta[edges[i].axis] = dimensions[edges[i].axis];
635 CollisionCapsule::calc_closest_segment_points(u1, u2, from_a, from_direction, vertex, delta);
636 PN_stdfloat dist_sq = ((from_a + from_direction * u1) - (vertex + delta * u2)).length_squared();
637 if (dist_sq < best_dist_sq) {
638 best_dist_sq = dist_sq;
642 if (best_dist_sq > radius_sq) {
648 if (collide_cat.is_debug()) {
656 LVector3 diff = point - _center;
657 diff[0] /= dimensions[0];
658 diff[1] /= dimensions[1];
659 diff[2] /= dimensions[2];
661 if (cabs(diff[0]) > cabs(diff[1])) {
662 if (cabs(diff[0]) > cabs(diff[2])) {
668 if (cabs(diff[1]) > cabs(diff[2])) {
675 normal[axis] = std::copysign(1, diff[axis]);
677 LPoint3 clamped = point.fmax(box_min).fmin(box_max);
678 LPoint3 surface_point = clamped;
679 surface_point[axis] = (diff[axis] >= 0.0f) ? box_max[axis] : box_min[axis];
682 LVector3 interior_vec;
683 if (clamped != point) {
686 interior_vec = point - surface_point;
687 if (!interior_vec.normalize()) {
688 interior_vec = normal;
692 interior_vec = normal;
694 new_entry->set_interior_point(point - interior_vec * radius);
695 new_entry->set_surface_point(surface_point);
700 new_entry->set_surface_normal(normal);
712 DCAST_INTO_R(box, entry.
get_from(),
nullptr);
714 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
716 LPoint3 diff = wrt_mat.xform_point_general(box->get_center()) - _center;
717 LVector3 from_extents = box->get_dimensions() * 0.5f;
718 LVector3 into_extents = get_dimensions() * 0.5f;
720 LVecBase3 box_x = wrt_mat.get_row3(0);
721 LVecBase3 box_y = wrt_mat.get_row3(1);
722 LVecBase3 box_z = wrt_mat.get_row3(2);
730 from_extents[0] *= l;
733 from_extents[1] *= l;
736 from_extents[2] *= l;
740 PN_stdfloat min_pen = 0;
745 r1 = into_extents[0];
746 r2 = cabs(box_x[0] * from_extents[0]) +
747 cabs(box_y[0] * from_extents[1]) +
748 cabs(box_z[0] * from_extents[2]);
749 pen = r1 + r2 - cabs(diff[0]);
755 r1 = into_extents[1];
756 r2 = cabs(box_x[1] * from_extents[0]) +
757 cabs(box_y[1] * from_extents[1]) +
758 cabs(box_z[1] * from_extents[2]);
759 pen = r1 + r2 - cabs(diff[1]);
768 r1 = into_extents[2];
769 r2 = cabs(box_x[2] * from_extents[0]) +
770 cabs(box_y[2] * from_extents[1]) +
771 cabs(box_z[2] * from_extents[2]);
772 pen = r1 + r2 - cabs(diff[2]);
782 r1 = cabs(box_x[0] * into_extents[0]) +
783 cabs(box_x[1] * into_extents[1]) +
784 cabs(box_x[2] * into_extents[2]);
785 r2 = from_extents[0];
786 pen = r1 + r2 - cabs(diff.dot(box_x));
794 r1 = cabs(box_y[0] * into_extents[0]) +
795 cabs(box_y[1] * into_extents[1]) +
796 cabs(box_y[2] * into_extents[2]);
797 r2 = from_extents[1];
798 pen = r1 + r2 - cabs(diff.dot(box_y));
806 r1 = cabs(box_z[0] * into_extents[0]) +
807 cabs(box_z[1] * into_extents[1]) +
808 cabs(box_z[2] * into_extents[2]);
809 r2 = from_extents[2];
810 pen = r1 + r2 - cabs(diff.dot(box_z));
819 r1 = into_extents[1] * cabs(box_x[2]) + into_extents[2] * cabs(box_x[1]);
820 r2 = from_extents[1] * cabs(box_z[0]) + from_extents[2] * cabs(box_y[0]);
821 if (cabs(diff[2] * box_x[1] - diff[1] * box_x[2]) > r1 + r2) {
825 r1 = into_extents[1] * cabs(box_y[2]) + into_extents[2] * cabs(box_y[1]);
826 r2 = from_extents[0] * cabs(box_z[0]) + from_extents[2] * cabs(box_x[0]);
827 if (cabs(diff[2] * box_y[1] - diff[1] * box_y[2]) > r1 + r2) {
831 r1 = into_extents[1] * cabs(box_z[2]) + into_extents[2] * cabs(box_z[1]);
832 r2 = from_extents[0] * cabs(box_y[0]) + from_extents[1] * cabs(box_x[0]);
833 if (cabs(diff[2] * box_z[1] - diff[1] * box_z[2]) > r1 + r2) {
837 r1 = into_extents[0] * cabs(box_x[2]) + into_extents[2] * cabs(box_x[0]);
838 r2 = from_extents[1] * cabs(box_z[1]) + from_extents[2] * cabs(box_y[1]);
839 if (cabs(diff[0] * box_x[2] - diff[2] * box_x[0]) > r1 + r2) {
843 r1 = into_extents[0] * cabs(box_y[2]) + into_extents[2] * cabs(box_y[0]);
844 r2 = from_extents[0] * cabs(box_z[1]) + from_extents[2] * cabs(box_x[1]);
845 if (cabs(diff[0] * box_y[2] - diff[2] * box_y[0]) > r1 + r2) {
849 r1 = into_extents[0] * cabs(box_z[2]) + into_extents[2] * cabs(box_z[0]);
850 r2 = from_extents[0] * cabs(box_y[1]) + from_extents[1] * cabs(box_x[1]);
851 if (cabs(diff[0] * box_z[2] - diff[2] * box_z[0]) > r1 + r2) {
855 r1 = into_extents[0] * cabs(box_x[1]) + into_extents[1] * cabs(box_x[0]);
856 r2 = from_extents[1] * cabs(box_z[2]) + from_extents[2] * cabs(box_y[2]);
857 if (cabs(diff[1] * box_x[0] - diff[0] * box_x[1]) > r1 + r2) {
861 r1 = into_extents[0] * cabs(box_y[1]) + into_extents[1] * cabs(box_y[0]);
862 r2 = from_extents[0] * cabs(box_z[2]) + from_extents[2] * cabs(box_x[2]);
863 if (cabs(diff[1] * box_y[0] - diff[0] * box_y[1]) > r1 + r2) {
867 r1 = into_extents[0] * cabs(box_z[1]) + into_extents[1] * cabs(box_z[0]);
868 r2 = from_extents[0] * cabs(box_y[2]) + from_extents[1] * cabs(box_x[2]);
869 if (cabs(diff[1] * box_z[0] - diff[0] * box_z[1]) > r1 + r2) {
873 if (collide_cat.is_debug()) {
883 min(max(diff[0], -into_extents[0]), into_extents[0]),
884 min(max(diff[1], -into_extents[1]), into_extents[1]),
885 min(max(diff[2], -into_extents[2]), into_extents[2]));
889 PN_stdfloat diff_axis = diff[axis];
890 int sign = (diff_axis >= 0) ? 1 : -1;
892 surface[axis] = into_extents[axis] * sign;
894 new_entry->set_surface_point(surface + _center);
897 new_entry->set_interior_point(surface + _center + normal * -min_pen);
902 new_entry->set_surface_normal(normal);
914 if (collide_cat.is_debug()) {
916 <<
"Recomputing viz for " << *
this <<
"\n";
923 vdata->unclean_set_num_rows(8);
927 vertex.set_data3(_min[0], _min[1], _min[2]);
928 vertex.set_data3(_min[0], _max[1], _min[2]);
929 vertex.set_data3(_max[0], _max[1], _min[2]);
930 vertex.set_data3(_max[0], _min[1], _min[2]);
932 vertex.set_data3(_min[0], _min[1], _max[2]);
933 vertex.set_data3(_min[0], _max[1], _max[2]);
934 vertex.set_data3(_max[0], _max[1], _max[2]);
935 vertex.set_data3(_max[0], _min[1], _max[2]);
941 tris->add_vertices(0, 1, 2);
942 tris->add_vertices(2, 3, 0);
945 tris->add_vertices(4, 7, 6);
946 tris->add_vertices(6, 5, 4);
949 tris->add_vertices(0, 4, 1);
950 tris->add_vertices(1, 4, 5);
952 tris->add_vertices(1, 5, 2);
953 tris->add_vertices(2, 5, 6);
955 tris->add_vertices(2, 6, 3);
956 tris->add_vertices(3, 6, 7);
958 tris->add_vertices(3, 7, 0);
959 tris->add_vertices(0, 7, 4);
964 lines->add_vertices(0, 1);
965 lines->add_vertices(1, 2);
966 lines->add_vertices(0, 3);
967 lines->add_vertices(2, 3);
970 lines->add_vertices(4, 5);
971 lines->add_vertices(5, 6);
972 lines->add_vertices(4, 7);
973 lines->add_vertices(6, 7);
976 lines->add_vertices(0, 4);
977 lines->add_vertices(1, 5);
978 lines->add_vertices(2, 6);
979 lines->add_vertices(3, 7);
982 geom1->add_primitive(tris);
985 geom2->add_primitive(lines);
987 _viz_geom->add_geom(geom1, get_solid_viz_state());
988 _viz_geom->add_geom(geom2, get_wireframe_viz_state());
990 _bounds_viz_geom->add_geom(geom1, get_solid_bounds_viz_state());
991 _bounds_viz_geom->add_geom(geom2, get_wireframe_viz_state());