38 PStatCollector CollisionCapsule::_volume_pcollector(
"Collision Volumes:CollisionCapsule");
39 PStatCollector CollisionCapsule::_test_pcollector(
"Collision Tests:CollisionCapsule");
55 return entry.
get_into()->test_intersection_from_capsule(entry);
61 void CollisionCapsule::
62 xform(
const LMatrix4 &mat) {
68 LVector3 radius_v = LVector3(_radius, 0.0f, 0.0f) * mat;
69 _radius = length(radius_v);
72 CollisionSolid::xform(mat);
91 return _volume_pcollector;
100 return _test_pcollector;
106 void CollisionCapsule::
107 output(std::ostream &out)
const {
108 out <<
"capsule, a (" << _a <<
"), b (" << _b <<
"), r " << _radius;
115 compute_internal_bounds()
const {
116 PT(
BoundingVolume) bound = CollisionSolid::compute_internal_bounds();
118 if (bound->is_of_type(GeometricBoundingVolume::get_class_type())) {
120 DCAST_INTO_R(gbound, bound, bound);
122 LVector3 vec = (_b - _a);
123 if (vec.normalize()) {
127 points[0] = _a - vec * _radius;
128 points[1] = _b + vec * _radius;
130 gbound->
around(points, points + 2);
147 test_intersection_from_sphere(
const CollisionEntry &entry)
const {
149 DCAST_INTO_R(sphere, entry.
get_from(),
nullptr);
154 const LMatrix4 &wrt_mat = wrt_space->get_mat();
156 LPoint3 from_a = sphere->get_center() * wrt_mat;
157 LPoint3 from_b = from_a;
159 LPoint3 contact_point;
160 PN_stdfloat actual_t = 0.0f;
162 if (wrt_prev_space != wrt_space) {
164 from_a = sphere->get_center() * wrt_prev_space->get_mat();
167 LVector3 from_direction = from_b - from_a;
169 LVector3 from_radius_v =
170 LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
171 PN_stdfloat from_radius = length(from_radius_v);
174 if (!intersects_line(t1, t2, from_a, from_direction, from_radius)) {
179 if (t2 < 0.0 || t1 > 1.0) {
186 actual_t = std::min(1.0, std::max(0.0, t1));
187 contact_point = from_a + actual_t * (from_b - from_a);
189 if (collide_cat.is_debug()) {
196 LPoint3 into_intersection_point;
200 into_intersection_point = from_b;
204 into_intersection_point = from_a + t2 * from_direction;
206 set_intersection_point(new_entry, into_intersection_point, from_radius);
208 LPoint3 fake_contact_point;
209 LVector3 contact_normal;
210 calculate_surface_point_and_normal(contact_point,
214 new_entry->set_contact_pos(contact_point);
215 new_entry->set_contact_normal(contact_normal);
216 new_entry->set_t(actual_t);
227 DCAST_INTO_R(line, entry.
get_from(),
nullptr);
229 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
231 LPoint3 from_origin = line->get_origin() * wrt_mat;
232 LVector3 from_direction = line->get_direction() * wrt_mat;
235 if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
240 if (collide_cat.is_debug()) {
247 LPoint3 into_intersection_point = from_origin + t1 * from_direction;
248 set_intersection_point(new_entry, into_intersection_point, 0.0);
254 LVector3 normal = into_intersection_point * _inv_mat;
255 if (normal[1] > _length) {
257 normal[1] -= _length;
258 }
else if (normal[1] > 0.0f) {
262 normal = normalize(normal * _mat);
263 new_entry->set_surface_normal(normal);
275 DCAST_INTO_R(ray, entry.
get_from(),
nullptr);
277 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
279 LPoint3 from_origin = ray->get_origin() * wrt_mat;
280 LVector3 from_direction = ray->get_direction() * wrt_mat;
283 if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
293 if (collide_cat.is_debug()) {
300 LPoint3 into_intersection_point;
304 into_intersection_point = from_origin;
307 into_intersection_point = from_origin + t1 * from_direction;
309 set_intersection_point(new_entry, into_intersection_point, 0.0);
315 LVector3 normal = into_intersection_point * _inv_mat;
316 if (normal[1] > _length) {
318 normal[1] -= _length;
319 }
else if (normal[1] > 0.0f) {
323 normal = normalize(normal * _mat);
324 new_entry->set_surface_normal(normal);
334 test_intersection_from_segment(
const CollisionEntry &entry)
const {
336 DCAST_INTO_R(segment, entry.
get_from(),
nullptr);
338 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
340 LPoint3 from_a = segment->get_point_a() * wrt_mat;
341 LPoint3 from_b = segment->get_point_b() * wrt_mat;
342 LVector3 from_direction = from_b - from_a;
345 if (!intersects_line(t1, t2, from_a, from_direction, 0.0f)) {
350 if (t2 < 0.0 || t1 > 1.0) {
356 if (collide_cat.is_debug()) {
363 LPoint3 into_intersection_point;
367 into_intersection_point = from_a;
371 into_intersection_point = from_a + t1 * from_direction;
373 set_intersection_point(new_entry, into_intersection_point, 0.0);
379 LVector3 normal = into_intersection_point * _inv_mat;
380 if (normal[1] > _length) {
382 normal[1] -= _length;
383 }
else if (normal[1] > 0.0f) {
387 normal = normalize(normal * _mat);
388 new_entry->set_surface_normal(normal);
398 test_intersection_from_capsule(
const CollisionEntry &entry)
const {
400 DCAST_INTO_R(capsule, entry.
get_from(),
nullptr);
403 LVector3 into_direction = _b - into_a;
405 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
407 LPoint3 from_a = capsule->get_point_a() * wrt_mat;
408 LPoint3 from_b = capsule->get_point_b() * wrt_mat;
409 LVector3 from_direction = from_b - from_a;
411 LVector3 from_radius_v =
412 LVector3(capsule->get_radius(), 0.0f, 0.0f) * wrt_mat;
413 PN_stdfloat from_radius = length(from_radius_v);
416 double into_t, from_t;
417 calc_closest_segment_points(into_t, from_t,
418 into_a, into_direction,
419 from_a, from_direction);
420 LPoint3 into_closest = into_a + into_direction * into_t;
421 LPoint3 from_closest = from_a + from_direction * from_t;
424 LVector3 closest_vec = from_closest - into_closest;
425 PN_stdfloat distance = closest_vec.length();
426 if (distance > _radius + from_radius) {
430 if (collide_cat.is_debug()) {
440 LVector3 surface_normal = closest_vec * (1.0 / distance);
442 new_entry->set_surface_point(into_closest + surface_normal * _radius);
443 new_entry->set_interior_point(from_closest - surface_normal * from_radius);
447 }
else if (distance != 0) {
448 new_entry->set_surface_normal(surface_normal);
452 set_intersection_point(new_entry, into_closest, 0);
462 test_intersection_from_parabola(
const CollisionEntry &entry)
const {
464 DCAST_INTO_R(parabola, entry.
get_from(),
nullptr);
466 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
470 local_p.xform(wrt_mat);
473 if (!intersects_parabola(t, local_p, parabola->
get_t1(), parabola->
get_t2(),
474 local_p.calc_point(parabola->
get_t1()),
475 local_p.calc_point(parabola->
get_t2()))) {
480 if (collide_cat.is_debug()) {
487 LPoint3 into_intersection_point = local_p.calc_point(t);
488 set_intersection_point(new_entry, into_intersection_point, 0.0);
494 LVector3 normal = into_intersection_point * _inv_mat;
495 if (normal[1] > _length) {
497 normal[1] -= _length;
498 }
else if (normal[1] > 0.0f) {
502 normal = normalize(normal * _mat);
503 new_entry->set_surface_normal(normal);
513 void CollisionCapsule::
515 if (collide_cat.is_debug()) {
517 <<
"Recomputing viz for " << *
this <<
"\n";
523 LVector3 direction = (_b - _a);
524 PN_stdfloat length = direction.length();
533 static const int num_slices = 8;
534 static const int num_rings = 4;
536 for (ri = 0; ri < num_rings; ri++) {
537 for (si = 0; si <= num_slices; si++) {
538 vertex.add_data3(calc_sphere1_vertex(ri, si, num_rings, num_slices));
539 vertex.add_data3(calc_sphere1_vertex(ri + 1, si, num_rings, num_slices));
541 strip->add_next_vertices((num_slices + 1) * 2);
542 strip->close_primitive();
546 for (si = 0; si <= num_slices; si++) {
547 vertex.add_data3(calc_sphere1_vertex(num_rings, si, num_rings, num_slices));
548 vertex.add_data3(calc_sphere2_vertex(num_rings, si, num_rings, num_slices,
551 strip->add_next_vertices((num_slices + 1) * 2);
552 strip->close_primitive();
555 for (ri = num_rings - 1; ri >= 0; ri--) {
556 for (si = 0; si <= num_slices; si++) {
557 vertex.add_data3(calc_sphere2_vertex(ri + 1, si, num_rings, num_slices, length));
558 vertex.add_data3(calc_sphere2_vertex(ri, si, num_rings, num_slices, length));
560 strip->add_next_vertices((num_slices + 1) * 2);
561 strip->close_primitive();
565 geom->add_primitive(strip);
569 look_at(mat, direction, LVector3(0.0f, 0.0f, 1.0f), CS_zup_right);
571 geom->transform_vertices(mat);
573 _viz_geom->add_geom(geom, get_solid_viz_state());
574 _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state());
581 void CollisionCapsule::
583 LVector3 direction = (_b - _a);
584 _length = direction.length();
586 look_at(_mat, direction, LVector3(0.0f, 0.0f, 1.0f), CS_zup_right);
588 _inv_mat.invert_from(_mat);
591 mark_internal_bounds_stale();
598 LVertex CollisionCapsule::
599 calc_sphere1_vertex(
int ri,
int si,
int num_rings,
int num_slices) {
600 PN_stdfloat r = (PN_stdfloat)ri / (PN_stdfloat)num_rings;
601 PN_stdfloat s = (PN_stdfloat)si / (PN_stdfloat)num_slices;
604 PN_stdfloat theta = s * 2.0f * MathNumbers::pi;
605 PN_stdfloat x_rim = ccos(theta);
606 PN_stdfloat z_rim = csin(theta);
609 PN_stdfloat phi = r * 0.5f * MathNumbers::pi;
610 PN_stdfloat to_pole = csin(phi);
612 PN_stdfloat x = _radius * x_rim * to_pole;
613 PN_stdfloat y = -_radius * ccos(phi);
614 PN_stdfloat z = _radius * z_rim * to_pole;
616 return LVertex(x, y, z);
623 LVertex CollisionCapsule::
624 calc_sphere2_vertex(
int ri,
int si,
int num_rings,
int num_slices,
625 PN_stdfloat length) {
626 PN_stdfloat r = (PN_stdfloat)ri / (PN_stdfloat)num_rings;
627 PN_stdfloat s = (PN_stdfloat)si / (PN_stdfloat)num_slices;
630 PN_stdfloat theta = s * 2.0f * MathNumbers::pi;
631 PN_stdfloat x_rim = ccos(theta);
632 PN_stdfloat z_rim = csin(theta);
635 PN_stdfloat phi = r * 0.5f * MathNumbers::pi;
636 PN_stdfloat to_pole = csin(phi);
638 PN_stdfloat x = _radius * x_rim * to_pole;
639 PN_stdfloat y = length + _radius * ccos(phi);
640 PN_stdfloat z = _radius * z_rim * to_pole;
642 return LVertex(x, y, z);
649 void CollisionCapsule::
650 calc_closest_segment_points(
double &t1,
double &t2,
651 const LPoint3 &from1,
const LVector3 &delta1,
652 const LPoint3 &from2,
const LVector3 &delta2) {
659 LVector3 w = from1 - from2;
660 PN_stdfloat a = delta1.dot(delta1);
661 PN_stdfloat b = delta1.dot(delta2);
662 PN_stdfloat c = delta2.dot(delta2);
663 PN_stdfloat d = delta1.dot(w);
664 PN_stdfloat e = delta2.dot(w);
665 PN_stdfloat D = a * c - b * b;
666 PN_stdfloat sN, sD = D;
667 PN_stdfloat tN, tD = D;
670 if (IS_NEARLY_ZERO(D)) {
683 }
else if (sN > sD) {
701 }
else if (tN > tD) {
704 if ((-d + b) < 0.0) {
706 }
else if ((-d + b) > a) {
715 t1 = (IS_NEARLY_ZERO(sN) ? 0.0 : sN / sD);
716 t2 = (IS_NEARLY_ZERO(tN) ? 0.0 : tN / tD);
727 bool CollisionCapsule::
728 intersects_line(
double &t1,
double &t2,
729 const LPoint3 &from0,
const LVector3 &delta0,
730 PN_stdfloat inflate_radius)
const {
733 LPoint3 from = from0 * _inv_mat;
734 LVector3 delta = delta0 * _inv_mat;
736 PN_stdfloat radius = _radius + inflate_radius;
743 LVector2 from2(from[0], from[2]);
744 LVector2 delta2(delta[0], delta[2]);
746 double A = dot(delta2, delta2);
748 if (IS_NEARLY_ZERO(A)) {
752 if (from2.dot(from2) > radius * radius) {
757 if (IS_NEARLY_ZERO(delta[1])) {
762 if (from[1] < -radius || from[1] > _length + radius) {
766 if (from[1] < 0.0f) {
768 if (from.dot(from) > radius * radius) {
771 }
else if (from[1] > _length) {
774 if (from.dot(from) > radius * radius) {
786 t1 = (-radius - from[1]) / delta[1];
787 t2 = (_length + radius - from[1]) / delta[1];
794 double B = 2.0f * dot(delta2, from2);
795 double fc_d2 = dot(from2, from2);
796 double C = fc_d2 - radius * radius;
798 double radical = B*B - 4.0*A*C;
800 if (IS_NEARLY_ZERO(radical)) {
802 t1 = t2 = -B / (2.0*A);
804 }
else if (radical < 0.0) {
809 double reciprocal_2A = 1.0 / (2.0 * A);
810 double sqrt_radical = sqrtf(radical);
811 t1 = ( -B - sqrt_radical ) * reciprocal_2A;
812 t2 = ( -B + sqrt_radical ) * reciprocal_2A;
818 PN_stdfloat t1_y = from[1] + t1 * delta[1];
819 PN_stdfloat t2_y = from[1] + t2 * delta[1];
821 if (t1_y < -radius && t2_y < -radius) {
824 }
else if (t1_y > _length + radius && t2_y > _length + radius) {
833 if (!sphere_intersects_line(t1a, t2a, 0.0f, from, delta, radius)) {
840 }
else if (t1_y > _length) {
844 if (!sphere_intersects_line(t1b, t2b, _length, from, delta, radius)) {
856 if (!sphere_intersects_line(t1a, t2a, 0.0f, from, delta, radius)) {
863 }
else if (t2_y > _length) {
867 if (!sphere_intersects_line(t1b, t2b, _length, from, delta, radius)) {
883 bool CollisionCapsule::
884 sphere_intersects_line(
double &t1,
double &t2, PN_stdfloat center_y,
885 const LPoint3 &from,
const LVector3 &delta,
886 PN_stdfloat radius) {
889 double A = dot(delta, delta);
891 nassertr(A != 0.0,
false);
895 double B = 2.0f* dot(delta, fc);
896 double fc_d2 = dot(fc, fc);
897 double C = fc_d2 - radius * radius;
899 double radical = B*B - 4.0*A*C;
901 if (IS_NEARLY_ZERO(radical)) {
903 t1 = t2 = -B / (2.0 * A);
906 }
else if (radical < 0.0) {
911 double reciprocal_2A = 1.0 / (2.0 * A);
912 double sqrt_radical = sqrtf(radical);
913 t1 = ( -B - sqrt_radical ) * reciprocal_2A;
914 t2 = ( -B + sqrt_radical ) * reciprocal_2A;
927 bool CollisionCapsule::
928 intersects_parabola(
double &t,
const LParabola ¶bola,
929 double t1,
double t2,
930 const LPoint3 &p1,
const LPoint3 &p2)
const {
938 double tmid = (t1 + t2) * 0.5;
940 if (tmid != t1 && tmid != t2) {
941 LPoint3 pmid = parabola.calc_point(tmid);
942 LPoint3 pmid2 = (p1 + p2) * 0.5f;
944 if ((pmid - pmid2).length_squared() > 0.001f) {
946 if (intersects_parabola(t, parabola, t1, tmid, p1, pmid)) {
949 return intersects_parabola(t, parabola, tmid, t2, pmid, p2);
955 if (!intersects_line(t1a, t2a, p1, p2 - p1, 0.0f)) {
959 if (t2a < 0.0 || t1a > 1.0) {
963 t = std::max(t1a, 0.0);
972 void CollisionCapsule::
973 calculate_surface_point_and_normal(
const LPoint3 &surface_point,
975 LPoint3 &result_point,
976 LVector3 &result_normal)
const {
978 LPoint3 point = surface_point * _inv_mat;
981 if (point[1] <= 0.0) {
984 if (!normal.normalize()) {
985 normal.set(0.0, -1.0, 0.0);
987 point = normal * _radius;
989 }
else if (point[1] >= _length) {
991 normal.set(point[0], point[1] - _length, point[2]);
992 if (!normal.normalize()) {
993 normal.set(0.0, 1.0, 0.0);
995 point = normal * _radius;
1000 LVector2d normal2d(point[0], point[2]);
1001 if (!normal2d.normalize()) {
1002 normal2d.set(0.0, 1.0);
1004 normal.set(normal2d[0], 0.0, normal2d[1]);
1005 point.set(normal[0] * _radius, point[1], normal[2] * _radius);
1009 result_point = point * _mat;
1010 result_normal = normal * _mat;
1018 void CollisionCapsule::
1020 const LPoint3 &into_intersection_point,
1021 double extra_radius)
const {
1025 calculate_surface_point_and_normal(into_intersection_point,
1057 _a.write_datagram(dg);
1058 _b.write_datagram(dg);
1074 node->fillin(scan, manager);
1083 void CollisionCapsule::
1085 CollisionSolid::fillin(scan, manager);
1086 _a.read_datagram(scan);
1087 _b.read_datagram(scan);