83compute_internal_bounds()
const {
95 DCAST_INTO_R(sphere, entry.
get_from(),
nullptr);
97 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
99 LPoint3 from_center = sphere->get_center() * wrt_mat;
100 LVector3 from_radius_v =
101 LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
102 PN_stdfloat from_radius = length(from_radius_v);
104 LPoint3 into_center = get_center();
105 PN_stdfloat into_radius = get_radius();
107 LVector3 vec = from_center - into_center;
108 PN_stdfloat dist2 = dot(vec, vec);
109 if (dist2 < (into_radius - from_radius) * (into_radius - from_radius)) {
114 if (collide_cat.is_debug()) {
121 LVector3 surface_normal;
122 PN_stdfloat vec_length = vec.length();
123 if (IS_NEARLY_ZERO(vec_length)) {
127 surface_normal.set(1.0, 0.0, 0.0);
129 surface_normal = vec / -vec_length;
134 new_entry->set_surface_normal(normal);
135 new_entry->set_surface_point(into_center - surface_normal * into_radius);
136 new_entry->set_interior_point(from_center - surface_normal * from_radius);
147 DCAST_INTO_R(line, entry.
get_from(),
nullptr);
149 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
151 LPoint3 from_origin = line->get_origin() * wrt_mat;
152 LVector3 from_direction = line->get_direction() * wrt_mat;
155 if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
161 if (collide_cat.is_debug()) {
168 LPoint3 into_intersection_point = from_origin + t2 * from_direction;
169 new_entry->set_surface_point(into_intersection_point);
174 LVector3 normal = into_intersection_point - get_center();
176 new_entry->set_surface_normal(-normal);
188 DCAST_INTO_R(ray, entry.
get_from(),
nullptr);
190 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
192 LPoint3 from_origin = ray->get_origin() * wrt_mat;
193 LVector3 from_direction = ray->get_direction() * wrt_mat;
196 if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
201 t2 = std::max(t2, 0.0);
203 if (collide_cat.is_debug()) {
210 LPoint3 into_intersection_point;
211 into_intersection_point = from_origin + t2 * from_direction;
212 new_entry->set_surface_point(into_intersection_point);
217 LVector3 normal = into_intersection_point - get_center();
219 new_entry->set_surface_normal(-normal);
229test_intersection_from_segment(
const CollisionEntry &entry)
const {
231 DCAST_INTO_R(segment, entry.
get_from(),
nullptr);
233 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
235 LPoint3 from_a = segment->get_point_a() * wrt_mat;
236 LPoint3 from_b = segment->get_point_b() * wrt_mat;
237 LVector3 from_direction = from_b - from_a;
240 if (!intersects_line(t1, t2, from_a, from_direction, 0.0f)) {
251 }
else if (t1 >= 1.0) {
255 }
else if (t2 <= 1.0) {
257 t = std::min(t2, 1.0);
259 }
else if (t1 >= 0.0) {
261 t = std::max(t1, 0.0);
270 if (collide_cat.is_debug()) {
277 LPoint3 into_intersection_point = from_a + t * from_direction;
278 new_entry->set_surface_point(into_intersection_point);
283 LVector3 normal = into_intersection_point - get_center();
285 new_entry->set_surface_normal(-normal);
295test_intersection_from_capsule(
const CollisionEntry &entry)
const {
297 DCAST_INTO_R(capsule, entry.
get_from(),
nullptr);
299 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
301 LPoint3 from_a = capsule->get_point_a() * wrt_mat;
302 LPoint3 from_b = capsule->get_point_b() * wrt_mat;
304 LVector3 from_radius_v =
305 LVector3(capsule->get_radius(), 0.0f, 0.0f) * wrt_mat;
306 PN_stdfloat from_radius = from_radius_v.length();
308 LPoint3 center = get_center();
309 PN_stdfloat radius = get_radius();
312 PN_stdfloat dist_a = (from_a - center).length();
313 PN_stdfloat dist_b = (from_b - center).length();
314 if (dist_b > dist_a) {
321 if (dist_a < radius - from_radius) {
325 if (collide_cat.is_debug()) {
332 LVector3 normal = center - from_a;
334 new_entry->set_surface_point(get_center() - normal * radius);
335 new_entry->set_interior_point(from_a - normal * from_radius);
340 new_entry->set_surface_normal(normal);
352 DCAST_INTO_R(box, entry.
get_from(),
nullptr);
354 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
356 LPoint3 center = get_center();
357 PN_stdfloat radius_sq = get_radius();
358 radius_sq *= radius_sq;
363 PN_stdfloat max_dist_sq = -1.0;
364 LPoint3 deepest_vertex;
366 for (
int i = 0; i < 8; ++i) {
367 LPoint3 point = wrt_mat.xform_point(box->
get_point(i));
369 PN_stdfloat dist_sq = (point - center).length_squared();
370 if (dist_sq > max_dist_sq) {
371 deepest_vertex = point;
372 max_dist_sq = dist_sq;
376 if (max_dist_sq < radius_sq) {
382 if (collide_cat.is_debug()) {
391 new_entry->set_interior_point(deepest_vertex);
394 LVector3 normal = center - deepest_vertex;
396 new_entry->set_surface_point(center - normal * get_radius());
397 new_entry->set_surface_normal(
408void CollisionInvSphere::
410 if (collide_cat.is_debug()) {
412 <<
"Recomputing viz for " << *
this <<
"\n";
415 static const int num_slices = 16;
416 static const int num_stacks = 8;
424 for (
int sl = 0; sl < num_slices; ++sl) {
425 PN_stdfloat longitude0 = (PN_stdfloat)sl / (PN_stdfloat)num_slices;
426 PN_stdfloat longitude1 = (PN_stdfloat)(sl + 1) / (PN_stdfloat)num_slices;
427 vertex.add_data3(compute_point(0.0, longitude0));
428 for (
int st = 1; st < num_stacks; ++st) {
429 PN_stdfloat latitude = (PN_stdfloat)st / (PN_stdfloat)num_stacks;
430 vertex.add_data3(compute_point(latitude, longitude1));
431 vertex.add_data3(compute_point(latitude, longitude0));
433 vertex.add_data3(compute_point(1.0, longitude0));
435 strip->add_next_vertices(num_stacks * 2);
436 strip->close_primitive();
440 geom->add_primitive(strip);
442 _viz_geom->add_geom(geom, get_solid_viz_state());
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.