100compute_internal_bounds()
const {
110 DCAST_INTO_R(sphere, entry.
get_from(),
nullptr);
112 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
114 LPoint3 from_center = sphere->get_center() * wrt_mat;
115 LVector3 from_radius_v =
116 LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
117 PN_stdfloat from_radius = length(from_radius_v);
119 PN_stdfloat dist = dist_to_plane(from_center);
120 if (dist > from_radius) {
125 if (collide_cat.is_debug()) {
136 new_entry->set_surface_normal(normal);
137 new_entry->set_surface_point(from_center - get_normal() * dist);
138 new_entry->set_interior_point(from_center - get_normal() * from_radius);
149 DCAST_INTO_R(line, entry.
get_from(),
nullptr);
151 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
153 LPoint3 from_origin = line->get_origin() * wrt_mat;
154 LVector3 from_direction = line->get_direction() * wrt_mat;
157 if (!_plane.intersects_line(t, from_origin, from_direction)) {
160 if (_plane.dist_to_plane(from_origin) > 0.0f) {
169 if (collide_cat.is_debug()) {
176 LPoint3 into_intersection_point = from_origin + t * from_direction;
182 new_entry->set_surface_normal(normal);
183 new_entry->set_surface_point(into_intersection_point);
194 DCAST_INTO_R(ray, entry.
get_from(),
nullptr);
196 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
198 LPoint3 from_origin = ray->get_origin() * wrt_mat;
199 LVector3 from_direction = ray->get_direction() * wrt_mat;
203 if (_plane.dist_to_plane(from_origin) < 0.0f) {
209 if (!_plane.intersects_line(t, from_origin, from_direction)) {
221 if (collide_cat.is_debug()) {
228 LPoint3 into_intersection_point = from_origin + t * from_direction;
234 new_entry->set_surface_normal(normal);
235 new_entry->set_surface_point(into_intersection_point);
244test_intersection_from_segment(
const CollisionEntry &entry)
const {
246 DCAST_INTO_R(segment, entry.
get_from(),
nullptr);
248 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
250 LPoint3 from_a = segment->get_point_a() * wrt_mat;
251 LPoint3 from_b = segment->get_point_b() * wrt_mat;
253 PN_stdfloat dist_a = _plane.dist_to_plane(from_a);
254 PN_stdfloat dist_b = _plane.dist_to_plane(from_b);
256 if (dist_a >= 0.0f && dist_b >= 0.0f) {
261 if (collide_cat.is_debug()) {
269 new_entry->set_surface_normal(normal);
272 LVector3 from_direction = from_b - from_a;
273 if (_plane.intersects_line(t, from_a, from_direction)) {
275 if (t >= 0.0f && t <= 1.0f) {
277 new_entry->set_surface_point(from_a + t * from_direction);
281 if (dist_a < dist_b) {
283 new_entry->set_interior_point(from_a);
285 }
else if (dist_b < dist_a) {
287 new_entry->set_interior_point(from_b);
291 new_entry->set_interior_point((from_a + from_b) * 0.5);
301test_intersection_from_capsule(
const CollisionEntry &entry)
const {
303 DCAST_INTO_R(capsule, entry.
get_from(),
nullptr);
305 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
307 LPoint3 from_a = capsule->get_point_a() * wrt_mat;
308 LPoint3 from_b = capsule->get_point_b() * wrt_mat;
309 LVector3 from_radius_v =
310 LVector3(capsule->get_radius(), 0.0f, 0.0f) * wrt_mat;
311 PN_stdfloat from_radius = length(from_radius_v);
313 PN_stdfloat dist_a = _plane.dist_to_plane(from_a);
314 PN_stdfloat dist_b = _plane.dist_to_plane(from_b);
316 if (dist_a >= from_radius && dist_b >= from_radius) {
321 if (collide_cat.is_debug()) {
329 new_entry->set_surface_normal(normal);
332 LVector3 from_direction = from_b - from_a;
333 if (_plane.intersects_line(t, from_a, from_direction)) {
336 new_entry->set_surface_point(from_b - get_normal() * dist_b);
338 }
else if (t <= 0.0f) {
339 new_entry->set_surface_point(from_a - get_normal() * dist_a);
343 new_entry->set_surface_point(from_a + t * from_direction);
348 new_entry->set_surface_point(from_a + 0.5f * from_direction - get_normal() * dist_a);
351 if (IS_NEARLY_EQUAL(dist_a, dist_b)) {
353 new_entry->set_interior_point(from_a + 0.5f * from_direction - get_normal() * from_radius);
355 }
else if (dist_a < dist_b) {
357 new_entry->set_interior_point(from_a - get_normal() * from_radius);
359 }
else if (dist_b < dist_a) {
361 new_entry->set_interior_point(from_b - get_normal() * from_radius);
372test_intersection_from_parabola(
const CollisionEntry &entry)
const {
374 DCAST_INTO_R(parabola, entry.
get_from(),
nullptr);
376 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
380 local_p.xform(wrt_mat);
383 if (_plane.dist_to_plane(local_p.calc_point(parabola->
get_t1())) < 0.0f) {
390 if (!get_plane().intersects_parabola(t1, t2, local_p)) {
396 if (t1 >= parabola->
get_t1() && t1 <= parabola->get_t2()) {
397 if (t2 >= parabola->
get_t1() && t2 <= parabola->get_t2()) {
400 t = std::min(t1, t2);
406 }
else if (t2 >= parabola->
get_t1() && t2 <= parabola->get_t2()) {
416 if (collide_cat.is_debug()) {
423 LPoint3 into_intersection_point = local_p.calc_point(t);
427 new_entry->set_surface_normal(normal);
428 new_entry->set_surface_point(into_intersection_point);
440 DCAST_INTO_R(box, entry.
get_from(),
nullptr);
442 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
444 LPoint3 from_center = box->get_center() * wrt_mat;
445 LVector3 from_extents = box->get_dimensions() * 0.5f;
446 PN_stdfloat dist = _plane.dist_to_plane(from_center);
449 LVecBase3 box_x = wrt_mat.get_row3(0) * from_extents[0];
450 LVecBase3 box_y = wrt_mat.get_row3(1) * from_extents[1];
451 LVecBase3 box_z = wrt_mat.get_row3(2) * from_extents[2];
455 PN_stdfloat dx = box_x.dot(_plane.get_normal());
456 PN_stdfloat dy = box_y.dot(_plane.get_normal());
457 PN_stdfloat dz = box_z.dot(_plane.get_normal());
458 PN_stdfloat depth = dist - (cabs(dx) + cabs(dy) + cabs(dz));
465 if (collide_cat.is_debug()) {
473 new_entry->set_surface_normal(normal);
477 LPoint3 interior_point = from_center +
478 box_x * ((dx < 0) - (dx > 0)) +
479 box_y * ((dy < 0) - (dy > 0)) +
480 box_z * ((dz < 0) - (dz > 0));
483 new_entry->set_surface_point(interior_point - get_normal() * depth);
484 new_entry->set_interior_point(interior_point);
495 if (collide_cat.is_debug()) {
497 <<
"Recomputing viz for " << *
this <<
"\n";
512 LVector3 p1, p2, p3, p4;
514 LVector3 normal = get_normal();
515 PN_stdfloat D = _plane[3];
517 if (fabs(normal[0]) > fabs(normal[1]) &&
518 fabs(normal[0]) > fabs(normal[2])) {
520 cp.set(-D / normal[0], 0.0f, 0.0f);
521 p1 = LPoint3(-(normal[1] + normal[2] + D)/normal[0], 1.0f, 1.0f) - cp;
523 }
else if (fabs(normal[1]) > fabs(normal[2])) {
525 cp.set(0.0f, -D / normal[1], 0.0f);
526 p1 = LPoint3(1.0f, -(normal[0] + normal[2] + D)/normal[1], 1.0f) - cp;
530 cp.set(0.0f, 0.0f, -D / normal[2]);
531 p1 = LPoint3(1.0f, 1.0f, -(normal[0] + normal[1] + D)/normal[2]) - cp;
535 p2 = cross(normal, p1);
536 p3 = cross(normal, p2);
537 p4 = cross(normal, p3);
539 static const double plane_scale = 10.0;
546 vertex.add_data3(cp + p1 * plane_scale);
547 vertex.add_data3(cp + p2 * plane_scale);
548 vertex.add_data3(cp + p3 * plane_scale);
549 vertex.add_data3(cp + p4 * plane_scale);
552 body->add_consecutive_vertices(0, 4);
553 body->close_primitive();
556 border->add_consecutive_vertices(0, 4);
557 border->add_vertex(0);
558 border->close_primitive();
561 geom1->add_primitive(body);
564 geom2->add_primitive(border);
566 _viz_geom->add_geom(geom1, get_solid_viz_state());
567 _viz_geom->add_geom(geom2, get_wireframe_viz_state());
569 _bounds_viz_geom->add_geom(geom1, get_solid_bounds_viz_state());
570 _bounds_viz_geom->add_geom(geom2, get_wireframe_bounds_viz_state());