116compute_internal_bounds()
const {
126 DCAST_INTO_R(sphere, entry.
get_from(),
nullptr);
130 const LMatrix4 &wrt_mat = wrt_space->get_mat();
132 LPoint3 from_b = sphere->get_center() * wrt_mat;
134 LPoint3 into_center(get_center());
135 PN_stdfloat into_radius(get_radius());
137 LVector3 from_radius_v =
138 LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
139 PN_stdfloat from_radius = length(from_radius_v);
141 LPoint3 into_intersection_point(from_b);
143 LPoint3 contact_point(into_intersection_point);
144 PN_stdfloat actual_t = 0.0f;
146 LVector3 vec = from_b - into_center;
147 PN_stdfloat dist2 = dot(vec, vec);
148 if (dist2 > (into_radius + from_radius) * (into_radius + from_radius)) {
152 LPoint3 from_a = sphere->get_center() * wrt_prev_space->get_mat();
154 if (!from_a.almost_equal(from_b)) {
155 LVector3 from_direction = from_b - from_a;
156 if (!intersects_line(t1, t2, from_a, from_direction, from_radius)) {
161 if (t2 < 0.0 || t1 > 1.0) {
168 actual_t = min(1.0, max(0.0, t1));
169 contact_point = from_a + actual_t * (from_b - from_a);
174 into_intersection_point = from_a;
178 into_intersection_point = from_a + t1 * from_direction;
186 if (collide_cat.is_debug()) {
193 LPoint3 from_center = sphere->get_center() * wrt_mat;
195 LVector3 surface_normal;
196 LVector3 v(into_intersection_point - into_center);
197 PN_stdfloat vec_length = v.length();
198 if (IS_NEARLY_ZERO(vec_length)) {
202 surface_normal.set(1.0, 0.0, 0.0);
204 surface_normal = v / vec_length;
209 LVector3 contact_normal;
210 LVector3 v2 = contact_point - into_center;
211 PN_stdfloat v2_len = v2.length();
212 if (IS_NEARLY_ZERO(v2_len)) {
216 contact_normal.set(1.0, 0.0, 0.0);
218 contact_normal = v2 / v2_len;
221 new_entry->set_surface_normal(eff_normal);
222 new_entry->set_surface_point(into_center + surface_normal * into_radius);
223 new_entry->set_interior_point(from_center - surface_normal * from_radius);
224 new_entry->set_contact_pos(contact_point);
225 new_entry->set_contact_normal(contact_normal);
226 new_entry->set_t(actual_t);
237 DCAST_INTO_R(line, entry.
get_from(),
nullptr);
239 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
241 LPoint3 from_origin = line->get_origin() * wrt_mat;
242 LVector3 from_direction = line->get_direction() * wrt_mat;
245 if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
250 if (collide_cat.is_debug()) {
257 LPoint3 into_intersection_point = from_origin + t1 * from_direction;
258 new_entry->set_surface_point(into_intersection_point);
263 LVector3 normal = into_intersection_point - get_center();
265 new_entry->set_surface_normal(normal);
277 DCAST_INTO_R(box, entry.
get_from(),
nullptr);
281 const LMatrix4 &wrt_mat = entry.get_inv_wrt_mat();
283 LPoint3 center = wrt_mat.xform_point(_center);
284 PN_stdfloat radius_sq = wrt_mat.xform_vec(LVector3(0, 0, _radius)).length_squared();
286 LPoint3 box_min = box->get_min();
287 LPoint3 box_max = box->get_max();
293 if (center[0] < box_min[0]) {
294 s = center[0] - box_min[0];
297 }
else if (center[0] > box_max[0]) {
298 s = center[0] - box_max[0];
302 if (center[1] < box_min[1]) {
303 s = center[1] - box_min[1];
306 }
else if (center[1] > box_max[1]) {
307 s = center[1] - box_max[1];
311 if (center[2] < box_min[2]) {
312 s = center[2] - box_min[2];
315 }
else if (center[2] > box_max[2]) {
316 s = center[2] - box_max[2];
324 if (collide_cat.is_debug()) {
333 LPoint3 interior = entry.get_wrt_mat().xform_point(center.fmax(box_min).fmin(box_max));
334 new_entry->set_interior_point(interior);
337 LVector3 normal = interior - _center;
339 new_entry->set_surface_point(_center + normal * _radius);
340 new_entry->set_surface_normal(
353 DCAST_INTO_R(ray, entry.
get_from(),
nullptr);
355 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
357 LPoint3 from_origin = ray->get_origin() * wrt_mat;
358 LVector3 from_direction = ray->get_direction() * wrt_mat;
361 if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
373 if (collide_cat.is_debug()) {
380 LPoint3 into_intersection_point = from_origin + t1 * from_direction;
381 new_entry->set_surface_point(into_intersection_point);
386 LVector3 normal = into_intersection_point - get_center();
388 new_entry->set_surface_normal(normal);
398test_intersection_from_segment(
const CollisionEntry &entry)
const {
400 DCAST_INTO_R(segment, entry.
get_from(),
nullptr);
402 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
404 LPoint3 from_a = segment->get_point_a() * wrt_mat;
405 LPoint3 from_b = segment->get_point_b() * wrt_mat;
406 LVector3 from_direction = from_b - from_a;
409 if (!intersects_line(t1, t2, from_a, from_direction, 0.0f)) {
414 if (t2 < 0.0 || t1 > 1.0) {
422 if (collide_cat.is_debug()) {
429 LPoint3 into_intersection_point = from_a + t1 * from_direction;
430 new_entry->set_surface_point(into_intersection_point);
435 LVector3 normal = into_intersection_point - get_center();
437 new_entry->set_surface_normal(normal);
447test_intersection_from_capsule(
const CollisionEntry &entry)
const {
449 DCAST_INTO_R(capsule, entry.
get_from(),
nullptr);
451 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
453 LPoint3 from_a = capsule->get_point_a() * wrt_mat;
454 LPoint3 from_b = capsule->get_point_b() * wrt_mat;
455 LVector3 from_direction = from_b - from_a;
457 LVector3 from_radius_v =
458 LVector3(capsule->get_radius(), 0.0f, 0.0f) * wrt_mat;
459 PN_stdfloat from_radius = length(from_radius_v);
462 if (!intersects_line(t1, t2, from_a, from_direction, from_radius)) {
467 if (t2 < 0.0 || t1 > 1.0) {
473 PN_stdfloat t = (t1 + t2) * (PN_stdfloat)0.5;
474 t = max(t, (PN_stdfloat)0.0);
475 t = min(t, (PN_stdfloat)1.0);
476 LPoint3 inner_point = from_a + t * from_direction;
478 if (collide_cat.is_debug()) {
485 LVector3 normal = inner_point - get_center();
487 new_entry->set_surface_point(get_center() + normal * get_radius());
488 new_entry->set_interior_point(inner_point - normal * from_radius);
493 new_entry->set_surface_normal(normal);
503test_intersection_from_parabola(
const CollisionEntry &entry)
const {
505 DCAST_INTO_R(parabola, entry.
get_from(),
nullptr);
507 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
511 local_p.xform(wrt_mat);
514 if (!intersects_parabola(t, local_p, parabola->
get_t1(), parabola->
get_t2(),
515 local_p.calc_point(parabola->
get_t1()),
516 local_p.calc_point(parabola->
get_t2()))) {
521 if (collide_cat.is_debug()) {
528 LPoint3 into_intersection_point = local_p.calc_point(t);
529 new_entry->set_surface_point(into_intersection_point);
534 LVector3 normal = into_intersection_point - get_center();
536 new_entry->set_surface_normal(normal);
546void CollisionSphere::
548 if (collide_cat.is_debug()) {
550 <<
"Recomputing viz for " << *
this <<
"\n";
553 static const int num_slices = 16;
554 static const int num_stacks = 8;
562 for (
int sl = 0; sl < num_slices; ++sl) {
563 PN_stdfloat longitude0 = (PN_stdfloat)sl / (PN_stdfloat)num_slices;
564 PN_stdfloat longitude1 = (PN_stdfloat)(sl + 1) / (PN_stdfloat)num_slices;
565 vertex.add_data3(compute_point(0.0, longitude0));
566 for (
int st = 1; st < num_stacks; ++st) {
567 PN_stdfloat latitude = (PN_stdfloat)st / (PN_stdfloat)num_stacks;
568 vertex.add_data3(compute_point(latitude, longitude0));
569 vertex.add_data3(compute_point(latitude, longitude1));
571 vertex.add_data3(compute_point(1.0, longitude0));
573 strip->add_next_vertices(num_stacks * 2);
574 strip->close_primitive();
578 geom->add_primitive(strip);
580 _viz_geom->add_geom(geom, get_solid_viz_state());
581 _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state());