Panda3D
|
00001 // Filename: collisionSphere.cxx 00002 // Created by: drose (24Apr00) 00003 // 00004 //////////////////////////////////////////////////////////////////// 00005 // 00006 // PANDA 3D SOFTWARE 00007 // Copyright (c) Carnegie Mellon University. All rights reserved. 00008 // 00009 // All use of this software is subject to the terms of the revised BSD 00010 // license. You should have received a copy of this license along 00011 // with this source code in a file named "LICENSE." 00012 // 00013 //////////////////////////////////////////////////////////////////// 00014 00015 00016 #include "collisionDSSolid.h" 00017 #include "collisionSphere.h" 00018 #include "collisionLine.h" 00019 #include "collisionRay.h" 00020 #include "collisionSegment.h" 00021 #include "collisionHandler.h" 00022 #include "collisionEntry.h" 00023 #include "config_collide.h" 00024 #include "boundingSphere.h" 00025 #include "datagram.h" 00026 #include "datagramIterator.h" 00027 #include "bamReader.h" 00028 #include "bamWriter.h" 00029 #include "nearly_zero.h" 00030 #include "cmath.h" 00031 #include "mathNumbers.h" 00032 #include "geom.h" 00033 #include "geomTristrips.h" 00034 #include "geomVertexWriter.h" 00035 00036 PStatCollector CollisionSphere::_volume_pcollector( 00037 "Collision Volumes:CollisionSphere"); 00038 PStatCollector CollisionSphere::_test_pcollector( 00039 "Collision Tests:CollisionSphere"); 00040 TypeHandle CollisionSphere::_type_handle; 00041 00042 //////////////////////////////////////////////////////////////////// 00043 // Function: CollisionSphere::make_copy 00044 // Access: Public, Virtual 00045 // Description: 00046 //////////////////////////////////////////////////////////////////// 00047 CollisionSolid *CollisionSphere:: 00048 make_copy() { 00049 return new CollisionSphere(*this); 00050 } 00051 00052 //////////////////////////////////////////////////////////////////// 00053 // Function: CollisionSphere::test_intersection 00054 // Access: Public, Virtual 00055 // Description: 00056 //////////////////////////////////////////////////////////////////// 00057 PT(CollisionEntry) CollisionSphere:: 00058 test_intersection(const CollisionEntry &entry) const { 00059 return entry.get_into()->test_intersection_from_sphere(entry); 00060 } 00061 00062 //////////////////////////////////////////////////////////////////// 00063 // Function: CollisionSphere::xform 00064 // Access: Public, Virtual 00065 // Description: Transforms the solid by the indicated matrix. 00066 //////////////////////////////////////////////////////////////////// 00067 void CollisionSphere:: 00068 xform(const LMatrix4f &mat) { 00069 _center = _center * mat; 00070 00071 // This is a little cheesy and fails miserably in the presence of a 00072 // non-uniform scale. 00073 LVector3f radius_v = LVector3f(_radius, 0.0f, 0.0f) * mat; 00074 _radius = length(radius_v); 00075 mark_viz_stale(); 00076 mark_internal_bounds_stale(); 00077 } 00078 00079 //////////////////////////////////////////////////////////////////// 00080 // Function: CollisionSphere::get_collision_origin 00081 // Access: Public, Virtual 00082 // Description: Returns the point in space deemed to be the "origin" 00083 // of the solid for collision purposes. The closest 00084 // intersection point to this origin point is considered 00085 // to be the most significant. 00086 //////////////////////////////////////////////////////////////////// 00087 LPoint3f CollisionSphere:: 00088 get_collision_origin() const { 00089 return get_center(); 00090 } 00091 00092 //////////////////////////////////////////////////////////////////// 00093 // Function: CollisionSphere::get_volume_pcollector 00094 // Access: Public, Virtual 00095 // Description: Returns a PStatCollector that is used to count the 00096 // number of bounding volume tests made against a solid 00097 // of this type in a given frame. 00098 //////////////////////////////////////////////////////////////////// 00099 PStatCollector &CollisionSphere:: 00100 get_volume_pcollector() { 00101 return _volume_pcollector; 00102 } 00103 00104 //////////////////////////////////////////////////////////////////// 00105 // Function: CollisionSphere::get_test_pcollector 00106 // Access: Public, Virtual 00107 // Description: Returns a PStatCollector that is used to count the 00108 // number of intersection tests made against a solid 00109 // of this type in a given frame. 00110 //////////////////////////////////////////////////////////////////// 00111 PStatCollector &CollisionSphere:: 00112 get_test_pcollector() { 00113 return _test_pcollector; 00114 } 00115 00116 //////////////////////////////////////////////////////////////////// 00117 // Function: CollisionSphere::output 00118 // Access: Public, Virtual 00119 // Description: 00120 //////////////////////////////////////////////////////////////////// 00121 void CollisionSphere:: 00122 output(ostream &out) const { 00123 out << "sphere, c (" << get_center() << "), r " << get_radius(); 00124 } 00125 00126 //////////////////////////////////////////////////////////////////// 00127 // Function: CollisionSphere::compute_internal_bounds 00128 // Access: Protected, Virtual 00129 // Description: 00130 //////////////////////////////////////////////////////////////////// 00131 PT(BoundingVolume) CollisionSphere:: 00132 compute_internal_bounds() const { 00133 return new BoundingSphere(_center, _radius); 00134 } 00135 #define USE_DS_SOLID_PLANES 1 00136 //////////////////////////////////////////////////////////////////// 00137 // Function: CollisionSphere::test_intersection_from_ds_solid 00138 // Access: Public, Virtual 00139 // Description: 00140 //////////////////////////////////////////////////////////////////// 00141 PT(CollisionEntry) CollisionSphere:: 00142 test_intersection_from_ds_solid(const CollisionEntry &entry) const { 00143 const CollisionDSSolid *ds_solid; 00144 DCAST_INTO_R(ds_solid, entry.get_from(), 0); 00145 cerr<<"CollisionSphere::test_intersection_from_ds_solid\n"; 00146 00147 CPT(TransformState) wrt_space = entry.get_wrt_space(); 00148 const LMatrix4f &wrt_mat = wrt_space->get_mat(); 00149 00150 LPoint3f into_center = get_center(); 00151 float into_radius = get_radius(); 00152 00153 LPoint3f sa_center = ds_solid->get_center_a() * wrt_mat; 00154 float sa_radius = length( 00155 LVector3f(ds_solid->get_radius_a(), 0.0f, 0.0f) * wrt_mat); 00156 LVector3f sa_vec = sa_center - into_center; 00157 float sa_distance_squared = dot(sa_vec, sa_vec); 00158 float sa_and_into_radii_squared = ( 00159 sa_radius + into_radius) * (sa_radius + into_radius); 00160 if (sa_distance_squared > sa_and_into_radii_squared) { 00161 // No intersection. 00162 return NULL; 00163 } 00164 00165 LPoint3f sb_center = ds_solid->get_center_b() * wrt_mat; 00166 float sb_radius = length( 00167 LVector3f(ds_solid->get_radius_b(), 0.0f, 0.0f) * wrt_mat); 00168 LVector3f sb_vec = sb_center - into_center; 00169 float sb_distance_squared = dot(sb_vec, sb_vec); 00170 float sb_and_into_radii_squared = ( 00171 sb_radius + into_radius) * (sb_radius + into_radius); 00172 if (sb_distance_squared > sb_and_into_radii_squared) { 00173 // No intersection. 00174 return NULL; 00175 } 00176 00177 #if USE_DS_SOLID_PLANES 00178 CPT(TransformState) inv_wrt_space = entry.get_inv_wrt_space(); 00179 const LMatrix4f &inv_wrt_mat = inv_wrt_space->get_mat(); 00180 00181 LPoint3f inv_into_center = get_center() * inv_wrt_mat; 00182 float inv_into_radius = length( 00183 LVector3f(get_radius(), 0.0f, 0.0f) * inv_wrt_mat); 00184 00185 float pa_distance = 00186 ds_solid->dist_to_plane_a(inv_into_center) - inv_into_radius; 00187 if (pa_distance > 0.0f) { 00188 // No intersection. 00189 return NULL; 00190 } 00191 00192 float pb_distance = 00193 ds_solid->dist_to_plane_b(inv_into_center) - inv_into_radius; 00194 if (pb_distance > 0.0f) { 00195 // No intersection. 00196 return NULL; 00197 } 00198 #endif 00199 00200 LVector3f lens_center = ds_solid->get_collision_origin() * wrt_mat; 00201 float lens_radius = length( 00202 LVector3f(ds_solid->get_lens_radius(), 0.0f, 0.0f) * wrt_mat); 00203 LVector3f lens_vec = lens_center - into_center; 00204 // float lens_distance_squared = dot(lens_vec, lens_vec); 00205 00206 LVector3f surface_normal; // into 00207 //LPoint3f surface_point; // into 00208 LPoint3f interior_point; // from 00209 float sa_distance = sqrtf(sa_distance_squared) - sa_radius; 00210 float sb_distance = sqrtf(sb_distance_squared) - sb_radius; 00211 pa_distance = ds_solid->dist_to_plane_a(inv_into_center); 00212 pb_distance = ds_solid->dist_to_plane_b(inv_into_center); 00213 #if USE_DS_SOLID_PLANES 00214 cerr 00215 <<" sa_distance:"<<sa_distance 00216 <<" sb_distance:"<<sb_distance 00217 <<" pa_distance:"<<pa_distance 00218 <<" pb_distance:"<<pb_distance<<"\n"; 00219 if ((sa_distance > pa_distance && sa_distance > pb_distance) || 00220 (sb_distance > pa_distance && sb_distance > pb_distance)) { 00221 #else 00222 cerr 00223 <<" sa_distance:"<<sa_distance 00224 <<" sb_distance:"<<sb_distance<<"\n"; 00225 #endif 00226 LVector3f *primary_vec; 00227 LPoint3f *primary_center; 00228 float primary_radius; 00229 LPoint3f *secondary_center; 00230 float secondary_radius; 00231 if (sa_distance > sb_distance) { 00232 // sphere_a is the furthest 00233 cerr<<"sphere_a is the furthest\n"; 00234 primary_vec = &sa_vec; 00235 primary_center = &sa_center; 00236 primary_radius = sa_radius; 00237 secondary_center = &sb_center; 00238 secondary_radius = sb_radius; 00239 } else { 00240 // sphere_b is the furthest 00241 cerr<<"sphere_b is the furthest\n"; 00242 primary_vec = &sb_vec; 00243 primary_center = &sb_center; 00244 primary_radius = sb_radius; 00245 secondary_center = &sa_center; 00246 secondary_radius = sa_radius; 00247 } 00248 00249 float vec_length = primary_vec->length(); 00250 if (IS_NEARLY_ZERO(vec_length)) { 00251 // The centers are coincident, use an arbitrary normal. 00252 surface_normal.set(1.0, 0.0, 0.0); 00253 } else { 00254 // Lens face 00255 surface_normal = *primary_vec / vec_length; 00256 } 00257 interior_point = *primary_center - surface_normal * primary_radius; 00258 00259 float temp_length_squared = 00260 (interior_point - *secondary_center).length_squared(); 00261 if (temp_length_squared > (secondary_radius * secondary_radius)) { 00262 cerr<<"foo\n"; 00263 LVector3f a = (*primary_center - lens_center).normalize(); 00264 LVector3f b = 00265 cross(cross(a, (into_center - lens_center).normalize()), a).normalize(); 00266 interior_point = lens_center + b * lens_radius; 00267 surface_normal = (interior_point - into_center).normalize(); 00268 } 00269 #if USE_DS_SOLID_PLANES 00270 } else { 00271 if (pa_distance > pb_distance) { 00272 // plane_a is the furthest 00273 cerr<<"plane_a is the furthest\n"; 00274 surface_normal = -(ds_solid->get_plane_a().get_normal() * wrt_mat); 00275 float d = length( 00276 LVector3f(pa_distance, 0.0f, 0.0f) * wrt_mat); 00277 interior_point = into_center + surface_normal * d; 00278 } else { 00279 // plane_b is the furthest 00280 cerr<<"plane_b is the furthest\n"; 00281 //surface_normal = -(ds_solid->get_plane_b().get_normal() * wrt_mat); 00282 surface_normal = -ds_solid->get_plane_b().get_normal(); 00283 surface_normal = surface_normal * wrt_mat; 00284 float d = length( 00285 LVector3f(pb_distance, 0.0f, 0.0f) * wrt_mat); 00286 interior_point = into_center + surface_normal * d; 00287 } 00288 } 00289 #endif 00290 00291 if (collide_cat.is_debug()) { 00292 collide_cat.debug() 00293 << "intersection detected from " << entry.get_from_node_path() 00294 << " into " << entry.get_into_node_path() << "\n"; 00295 } 00296 00297 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00298 new_entry->set_surface_normal(surface_normal); 00299 new_entry->set_surface_point(into_center + surface_normal * into_radius); 00300 new_entry->set_interior_point(interior_point); 00301 return new_entry; 00302 } 00303 00304 //////////////////////////////////////////////////////////////////// 00305 // Function: CollisionSphere::test_intersection_from_sphere 00306 // Access: Public, Virtual 00307 // Description: 00308 //////////////////////////////////////////////////////////////////// 00309 PT(CollisionEntry) CollisionSphere:: 00310 test_intersection_from_sphere(const CollisionEntry &entry) const { 00311 const CollisionSphere *sphere; 00312 DCAST_INTO_R(sphere, entry.get_from(), 0); 00313 00314 CPT(TransformState) wrt_space = entry.get_wrt_space(); 00315 00316 const LMatrix4f &wrt_mat = wrt_space->get_mat(); 00317 00318 LPoint3f from_b = sphere->get_center() * wrt_mat; 00319 00320 LPoint3f into_center(get_center()); 00321 float into_radius(get_radius()); 00322 00323 LVector3f from_radius_v = 00324 LVector3f(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat; 00325 float from_radius = length(from_radius_v); 00326 00327 LPoint3f into_intersection_point(from_b); 00328 double t1, t2; 00329 LPoint3f contact_point(into_intersection_point); 00330 float actual_t = 0.0f; 00331 00332 LVector3f vec = from_b - into_center; 00333 float dist2 = dot(vec, vec); 00334 if (dist2 > (into_radius + from_radius) * (into_radius + from_radius)) { 00335 // No intersection with the current position. Check the delta 00336 // from the previous frame. 00337 CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space(); 00338 LPoint3f from_a = sphere->get_center() * wrt_prev_space->get_mat(); 00339 00340 if (!from_a.almost_equal(from_b)) { 00341 LVector3f from_direction = from_b - from_a; 00342 if (!intersects_line(t1, t2, from_a, from_direction, from_radius)) { 00343 // No intersection. 00344 return NULL; 00345 } 00346 00347 if (t2 < 0.0 || t1 > 1.0) { 00348 // Both intersection points are before the start of the segment or 00349 // after the end of the segment. 00350 return NULL; 00351 } 00352 00353 // doubles, not floats, to satisfy min and max templates. 00354 actual_t = min(1.0, max(0.0, t1)); 00355 contact_point = from_a + actual_t * (from_b - from_a); 00356 00357 if (t1 < 0.0) { 00358 // Point a is within the sphere. The first intersection point is 00359 // point a itself. 00360 into_intersection_point = from_a; 00361 } else { 00362 // Point a is outside the sphere, and point b is either inside the 00363 // sphere or beyond it. The first intersection point is at t1. 00364 into_intersection_point = from_a + t1 * from_direction; 00365 } 00366 } else { 00367 // No delta, therefore no intersection. 00368 return NULL; 00369 } 00370 } 00371 00372 if (collide_cat.is_debug()) { 00373 collide_cat.debug() 00374 << "intersection detected from " << entry.get_from_node_path() 00375 << " into " << entry.get_into_node_path() << "\n"; 00376 } 00377 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00378 00379 LPoint3f from_center = sphere->get_center() * wrt_mat; 00380 00381 LVector3f surface_normal; 00382 LVector3f v(into_intersection_point - into_center); 00383 float vec_length = v.length(); 00384 if (IS_NEARLY_ZERO(vec_length)) { 00385 // If we don't have a collision normal (e.g. the centers are 00386 // exactly coincident), then make up an arbitrary normal--any one 00387 // is as good as any other. 00388 surface_normal.set(1.0, 0.0, 0.0); 00389 } else { 00390 surface_normal = v / vec_length; 00391 } 00392 00393 LVector3f eff_normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : surface_normal; 00394 00395 LVector3f contact_normal; 00396 LVector3f v2 = contact_point - into_center; 00397 float v2_len = v2.length(); 00398 if (IS_NEARLY_ZERO(v2_len)) { 00399 // If we don't have a collision normal (e.g. the centers are 00400 // exactly coincident), then make up an arbitrary normal--any one 00401 // is as good as any other. 00402 contact_normal.set(1.0, 0.0, 0.0); 00403 } else { 00404 contact_normal = v2 / v2_len; 00405 } 00406 00407 new_entry->set_surface_normal(eff_normal); 00408 new_entry->set_surface_point(into_center + surface_normal * into_radius); 00409 new_entry->set_interior_point(from_center - surface_normal * from_radius); 00410 new_entry->set_contact_pos(contact_point); 00411 new_entry->set_contact_normal(contact_normal); 00412 new_entry->set_t(actual_t); 00413 00414 return new_entry; 00415 } 00416 00417 //////////////////////////////////////////////////////////////////// 00418 // Function: CollisionSphere::test_intersection_from_line 00419 // Access: Public, Virtual 00420 // Description: 00421 //////////////////////////////////////////////////////////////////// 00422 PT(CollisionEntry) CollisionSphere:: 00423 test_intersection_from_line(const CollisionEntry &entry) const { 00424 const CollisionLine *line; 00425 DCAST_INTO_R(line, entry.get_from(), 0); 00426 00427 const LMatrix4f &wrt_mat = entry.get_wrt_mat(); 00428 00429 LPoint3f from_origin = line->get_origin() * wrt_mat; 00430 LVector3f from_direction = line->get_direction() * wrt_mat; 00431 00432 double t1, t2; 00433 if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) { 00434 // No intersection. 00435 return NULL; 00436 } 00437 00438 if (collide_cat.is_debug()) { 00439 collide_cat.debug() 00440 << "intersection detected from " << entry.get_from_node_path() 00441 << " into " << entry.get_into_node_path() << "\n"; 00442 } 00443 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00444 00445 LPoint3f into_intersection_point = from_origin + t1 * from_direction; 00446 new_entry->set_surface_point(into_intersection_point); 00447 00448 if (has_effective_normal() && line->get_respect_effective_normal()) { 00449 new_entry->set_surface_normal(get_effective_normal()); 00450 } else { 00451 LVector3f normal = into_intersection_point - get_center(); 00452 normal.normalize(); 00453 new_entry->set_surface_normal(normal); 00454 } 00455 00456 return new_entry; 00457 } 00458 00459 //////////////////////////////////////////////////////////////////// 00460 // Function: CollisionSphere::test_intersection_from_box 00461 // Access: Public, Virtual 00462 // Description: Double dispatch point for box as a FROM object 00463 //////////////////////////////////////////////////////////////////// 00464 PT(CollisionEntry) CollisionSphere:: 00465 test_intersection_from_box(const CollisionEntry &entry) const { 00466 const CollisionBox *box; 00467 DCAST_INTO_R(box, entry.get_from(), 0); 00468 00469 CPT(TransformState) wrt_space = entry.get_wrt_space(); 00470 CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space(); 00471 00472 const LMatrix4f &wrt_mat = wrt_space->get_mat(); 00473 00474 CollisionBox local_b( *box ); 00475 local_b.xform( wrt_mat ); 00476 00477 LPoint3f from_center = local_b.get_center(); 00478 00479 LPoint3f orig_center = get_center(); 00480 LPoint3f to_center = orig_center; 00481 bool moved_from_center = false; 00482 float t = 1.0f; 00483 LPoint3f contact_point(from_center); 00484 float actual_t = 1.0f; 00485 00486 float to_radius = get_radius(); 00487 float to_radius_2 = to_radius * to_radius; 00488 00489 int ip; 00490 float max_dist,dist; 00491 bool intersect; 00492 Planef plane; 00493 LVector3f normal; 00494 00495 for( ip = 0, intersect=false; ip < 6 && !intersect; ip++ ){ 00496 plane = local_b.get_plane( ip ); 00497 if (local_b.get_plane_points(ip).size() < 3) { 00498 continue; 00499 } 00500 normal = (has_effective_normal() && box->get_respect_effective_normal()) ? get_effective_normal() : plane.get_normal(); 00501 00502 #ifndef NDEBUG 00503 /* 00504 if (!IS_THRESHOLD_EQUAL(normal.length_squared(), 1.0f, 0.001), NULL) { 00505 collide_cat.info() 00506 << "polygon being collided with " << entry.get_into_node_path() 00507 << " has normal " << normal << " of length " << normal.length() 00508 << "\n"; 00509 normal.normalize(); 00510 } 00511 */ 00512 #endif 00513 00514 // The nearest point within the plane to our center is the 00515 // intersection of the line (center, center - normal) with the plane. 00516 00517 if (!plane.intersects_line(dist, to_center, -(plane.get_normal()))) { 00518 // No intersection with plane? This means the plane's effective 00519 // normal was within the plane itself. A useless polygon. 00520 continue; 00521 } 00522 00523 if (dist > to_radius || dist < -to_radius) { 00524 // No intersection with the plane. 00525 continue; 00526 } 00527 00528 LPoint2f p = local_b.to_2d(to_center - dist * plane.get_normal(), ip); 00529 float edge_dist = 0.0f; 00530 00531 edge_dist = local_b.dist_to_polygon(p, local_b.get_plane_points(ip)); 00532 00533 if(edge_dist < 0) { 00534 intersect = true; 00535 continue; 00536 } 00537 00538 if((edge_dist > 0) && 00539 ((edge_dist * edge_dist + dist * dist) > to_radius_2)) { 00540 // No intersection; the circle is outside the polygon. 00541 continue; 00542 } 00543 00544 // The sphere appears to intersect the polygon. If the edge is less 00545 // than to_radius away, the sphere may be resting on an edge of 00546 // the polygon. Determine how far the center of the sphere must 00547 // remain from the plane, based on its distance from the nearest 00548 // edge. 00549 00550 max_dist = to_radius; 00551 if (edge_dist >= 0.0f) { 00552 float max_dist_2 = max(to_radius_2 - edge_dist * edge_dist, 0.0f); 00553 max_dist = csqrt(max_dist_2); 00554 } 00555 00556 if (dist > max_dist) { 00557 // There's no intersection: the sphere is hanging off the edge. 00558 continue; 00559 } 00560 intersect = true; 00561 } 00562 if( !intersect ) 00563 return NULL; 00564 00565 if (collide_cat.is_debug()) { 00566 collide_cat.debug() 00567 << "intersection detected from " << entry.get_from_node_path() 00568 << " into " << entry.get_into_node_path() << "\n"; 00569 } 00570 00571 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00572 00573 float into_depth = max_dist - dist; 00574 00575 new_entry->set_surface_normal(normal); 00576 new_entry->set_surface_point(to_center - normal * dist); 00577 new_entry->set_interior_point(to_center - normal * (dist + into_depth)); 00578 new_entry->set_contact_pos(contact_point); 00579 new_entry->set_contact_normal(plane.get_normal()); 00580 new_entry->set_t(actual_t); 00581 00582 return new_entry; 00583 } 00584 00585 //////////////////////////////////////////////////////////////////// 00586 // Function: CollisionSphere::test_intersection_from_ray 00587 // Access: Public, Virtual 00588 // Description: 00589 //////////////////////////////////////////////////////////////////// 00590 PT(CollisionEntry) CollisionSphere:: 00591 test_intersection_from_ray(const CollisionEntry &entry) const { 00592 const CollisionRay *ray; 00593 DCAST_INTO_R(ray, entry.get_from(), 0); 00594 00595 const LMatrix4f &wrt_mat = entry.get_wrt_mat(); 00596 00597 LPoint3f from_origin = ray->get_origin() * wrt_mat; 00598 LVector3f from_direction = ray->get_direction() * wrt_mat; 00599 00600 double t1, t2; 00601 if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) { 00602 // No intersection. 00603 return NULL; 00604 } 00605 00606 if (t2 < 0.0) { 00607 // Both intersection points are before the start of the ray. 00608 return NULL; 00609 } 00610 00611 t1 = max(t1, 0.0); 00612 00613 if (collide_cat.is_debug()) { 00614 collide_cat.debug() 00615 << "intersection detected from " << entry.get_from_node_path() 00616 << " into " << entry.get_into_node_path() << "\n"; 00617 } 00618 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00619 00620 LPoint3f into_intersection_point = from_origin + t1 * from_direction; 00621 new_entry->set_surface_point(into_intersection_point); 00622 00623 if (has_effective_normal() && ray->get_respect_effective_normal()) { 00624 new_entry->set_surface_normal(get_effective_normal()); 00625 } else { 00626 LVector3f normal = into_intersection_point - get_center(); 00627 normal.normalize(); 00628 new_entry->set_surface_normal(normal); 00629 } 00630 00631 return new_entry; 00632 } 00633 00634 //////////////////////////////////////////////////////////////////// 00635 // Function: CollisionSphere::test_intersection_from_segment 00636 // Access: Public, Virtual 00637 // Description: 00638 //////////////////////////////////////////////////////////////////// 00639 PT(CollisionEntry) CollisionSphere:: 00640 test_intersection_from_segment(const CollisionEntry &entry) const { 00641 const CollisionSegment *segment; 00642 DCAST_INTO_R(segment, entry.get_from(), 0); 00643 00644 const LMatrix4f &wrt_mat = entry.get_wrt_mat(); 00645 00646 LPoint3f from_a = segment->get_point_a() * wrt_mat; 00647 LPoint3f from_b = segment->get_point_b() * wrt_mat; 00648 LVector3f from_direction = from_b - from_a; 00649 00650 double t1, t2; 00651 if (!intersects_line(t1, t2, from_a, from_direction, 0.0f)) { 00652 // No intersection. 00653 return NULL; 00654 } 00655 00656 if (t2 < 0.0 || t1 > 1.0) { 00657 // Both intersection points are before the start of the segment or 00658 // after the end of the segment. 00659 return NULL; 00660 } 00661 00662 t1 = max(t1, 0.0); 00663 00664 if (collide_cat.is_debug()) { 00665 collide_cat.debug() 00666 << "intersection detected from " << entry.get_from_node_path() 00667 << " into " << entry.get_into_node_path() << "\n"; 00668 } 00669 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00670 00671 LPoint3f into_intersection_point = from_a + t1 * from_direction; 00672 new_entry->set_surface_point(into_intersection_point); 00673 00674 if (has_effective_normal() && segment->get_respect_effective_normal()) { 00675 new_entry->set_surface_normal(get_effective_normal()); 00676 } else { 00677 LVector3f normal = into_intersection_point - get_center(); 00678 normal.normalize(); 00679 new_entry->set_surface_normal(normal); 00680 } 00681 00682 return new_entry; 00683 } 00684 00685 //////////////////////////////////////////////////////////////////// 00686 // Function: CollisionSphere::test_intersection_from_parabola 00687 // Access: Public, Virtual 00688 // Description: 00689 //////////////////////////////////////////////////////////////////// 00690 PT(CollisionEntry) CollisionSphere:: 00691 test_intersection_from_parabola(const CollisionEntry &entry) const { 00692 const CollisionParabola *parabola; 00693 DCAST_INTO_R(parabola, entry.get_from(), 0); 00694 00695 const LMatrix4f &wrt_mat = entry.get_wrt_mat(); 00696 00697 // Convert the parabola into local coordinate space. 00698 Parabolaf local_p(parabola->get_parabola()); 00699 local_p.xform(wrt_mat); 00700 00701 double t; 00702 if (!intersects_parabola(t, local_p, parabola->get_t1(), parabola->get_t2(), 00703 local_p.calc_point(parabola->get_t1()), 00704 local_p.calc_point(parabola->get_t2()))) { 00705 // No intersection. 00706 return NULL; 00707 } 00708 00709 if (collide_cat.is_debug()) { 00710 collide_cat.debug() 00711 << "intersection detected from " << entry.get_from_node_path() 00712 << " into " << entry.get_into_node_path() << "\n"; 00713 } 00714 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00715 00716 LPoint3f into_intersection_point = local_p.calc_point(t); 00717 new_entry->set_surface_point(into_intersection_point); 00718 00719 if (has_effective_normal() && parabola->get_respect_effective_normal()) { 00720 new_entry->set_surface_normal(get_effective_normal()); 00721 } else { 00722 LVector3f normal = into_intersection_point - get_center(); 00723 normal.normalize(); 00724 new_entry->set_surface_normal(normal); 00725 } 00726 00727 return new_entry; 00728 } 00729 00730 //////////////////////////////////////////////////////////////////// 00731 // Function: CollisionSphere::fill_viz_geom 00732 // Access: Protected, Virtual 00733 // Description: Fills the _viz_geom GeomNode up with Geoms suitable 00734 // for rendering this solid. 00735 //////////////////////////////////////////////////////////////////// 00736 void CollisionSphere:: 00737 fill_viz_geom() { 00738 if (collide_cat.is_debug()) { 00739 collide_cat.debug() 00740 << "Recomputing viz for " << *this << "\n"; 00741 } 00742 00743 static const int num_slices = 16; 00744 static const int num_stacks = 8; 00745 00746 PT(GeomVertexData) vdata = new GeomVertexData 00747 ("collision", GeomVertexFormat::get_v3(), 00748 Geom::UH_static); 00749 GeomVertexWriter vertex(vdata, InternalName::get_vertex()); 00750 00751 PT(GeomTristrips) strip = new GeomTristrips(Geom::UH_static); 00752 for (int sl = 0; sl < num_slices; ++sl) { 00753 float longitude0 = (float)sl / (float)num_slices; 00754 float longitude1 = (float)(sl + 1) / (float)num_slices; 00755 vertex.add_data3f(compute_point(0.0, longitude0)); 00756 for (int st = 1; st < num_stacks; ++st) { 00757 float latitude = (float)st / (float)num_stacks; 00758 vertex.add_data3f(compute_point(latitude, longitude0)); 00759 vertex.add_data3f(compute_point(latitude, longitude1)); 00760 } 00761 vertex.add_data3f(compute_point(1.0, longitude0)); 00762 00763 strip->add_next_vertices(num_stacks * 2); 00764 strip->close_primitive(); 00765 } 00766 00767 PT(Geom) geom = new Geom(vdata); 00768 geom->add_primitive(strip); 00769 00770 _viz_geom->add_geom(geom, get_solid_viz_state()); 00771 _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state()); 00772 } 00773 00774 //////////////////////////////////////////////////////////////////// 00775 // Function: CollisionSphere::intersects_line 00776 // Access: Protected 00777 // Description: Determine the point(s) of intersection of a parametric 00778 // line with the sphere. The line is infinite in both 00779 // directions, and passes through "from" and from+delta. 00780 // If the line does not intersect the sphere, the 00781 // function returns false, and t1 and t2 are undefined. 00782 // If it does intersect the sphere, it returns true, and 00783 // t1 and t2 are set to the points along the equation 00784 // from+t*delta that correspond to the two points of 00785 // intersection. 00786 //////////////////////////////////////////////////////////////////// 00787 bool CollisionSphere:: 00788 intersects_line(double &t1, double &t2, 00789 const LPoint3f &from, const LVector3f &delta, 00790 float inflate_radius) const { 00791 // Solve the equation for the intersection of a line with a sphere 00792 // using the quadratic equation. 00793 00794 // A line segment from f to f+d is defined as all P such that 00795 // P = f + td for 0 <= t <= 1. 00796 00797 // A sphere with radius r about point c is defined as all P such 00798 // that r^2 = (P - c)^2. 00799 00800 // Substituting P in the above we have: 00801 00802 // r^2 = (f + td - c)^2 = 00803 // (f^2 + ftd - fc + ftd + t^2d^2 - tdc - fc - tdc + c^2) = 00804 // t^2(d^2) + t(fd + fd - dc - dc) + (f^2 - fc - fc + c^2) = 00805 // t^2(d^2) + t(2d(f - c)) + (f - c)^2 00806 00807 // Thus, the equation is quadratic in t, and we have 00808 // at^2 + bt + c = 0 00809 00810 // Where a = d^2 00811 // b = 2d(f - c) 00812 // c = (f - c)^2 - r^2 00813 00814 // Solving for t using the quadratic equation gives us the point of 00815 // intersection along the line segment. Actually, there are two 00816 // solutions (since it is quadratic): one for the front of the 00817 // sphere, and one for the back. In the case where the line is 00818 // tangent to the sphere, there is only one solution (and the 00819 // radical is zero). 00820 00821 double A = dot(delta, delta); 00822 00823 nassertr(A != 0.0, false); 00824 00825 LVector3f fc = from - get_center(); 00826 double B = 2.0f* dot(delta, fc); 00827 double fc_d2 = dot(fc, fc); 00828 double radius = get_radius() + inflate_radius; 00829 double C = fc_d2 - radius * radius; 00830 00831 double radical = B*B - 4.0*A*C; 00832 00833 if (IS_NEARLY_ZERO(radical)) { 00834 // Tangent. 00835 t1 = t2 = -B /(2.0*A); 00836 return true; 00837 00838 } else if (radical < 0.0) { 00839 // No real roots: no intersection with the line. 00840 return false; 00841 } 00842 00843 double reciprocal_2A = 1.0/(2.0*A); 00844 double sqrt_radical = sqrtf(radical); 00845 t1 = ( -B - sqrt_radical ) * reciprocal_2A; 00846 t2 = ( -B + sqrt_radical ) * reciprocal_2A; 00847 00848 return true; 00849 } 00850 00851 //////////////////////////////////////////////////////////////////// 00852 // Function: CollisionSphere::intersects_parabola 00853 // Access: Protected 00854 // Description: Determine a point of intersection of a parametric 00855 // parabola with the sphere. 00856 // 00857 // We only consider the segment of the parabola between 00858 // t1 and t2, which has already been computed as 00859 // corresponding to points p1 and p2. If there is an 00860 // intersection, t is set to the parametric point of 00861 // intersection, and true is returned; otherwise, false 00862 // is returned. 00863 //////////////////////////////////////////////////////////////////// 00864 bool CollisionSphere:: 00865 intersects_parabola(double &t, const Parabolaf ¶bola, 00866 double t1, double t2, 00867 const LPoint3f &p1, const LPoint3f &p2) const { 00868 if (t1 == t2) { 00869 // Special case: a single point. 00870 if ((p1 - _center).length_squared() > _radius * _radius) { 00871 // No intersection. 00872 return false; 00873 } 00874 t = t1; 00875 return true; 00876 } 00877 00878 // To directly test for intersection between a parabola (quadratic) 00879 // and a sphere (also quadratic) requires solving a quartic 00880 // equation. Doable, but hard, and I'm a programmer, not a 00881 // mathematician. So I'll solve it the programmer's way instead, by 00882 // approximating the parabola with a series of line segments. 00883 // Hence, this function works by recursively subdividing the 00884 // parabola as necessary. 00885 00886 // First, see if the line segment (p1 - p2) comes sufficiently close 00887 // to the parabola. Do this by computing the parametric intervening 00888 // point and comparing its distance from the linear intervening 00889 // point. 00890 double tmid = (t1 + t2) * 0.5; 00891 if (tmid != t1 && tmid != t2) { 00892 LPoint3f pmid = parabola.calc_point(tmid); 00893 LPoint3f pmid2 = (p1 + p2) * 0.5f; 00894 00895 if ((pmid - pmid2).length_squared() > 0.001f) { 00896 // Subdivide. 00897 if (intersects_parabola(t, parabola, t1, tmid, p1, pmid)) { 00898 return true; 00899 } 00900 return intersects_parabola(t, parabola, tmid, t2, pmid, p2); 00901 } 00902 } 00903 00904 // The line segment is sufficiently close; compare the segment itself. 00905 double t1a, t2a; 00906 if (!intersects_line(t1a, t2a, p1, p2 - p1, 0.0f)) { 00907 return false; 00908 } 00909 00910 if (t2a < 0.0 || t1a > 1.0) { 00911 return false; 00912 } 00913 00914 t = max(t1a, 0.0); 00915 return true; 00916 } 00917 00918 //////////////////////////////////////////////////////////////////// 00919 // Function: CollisionSphere::compute_point 00920 // Access: Protected 00921 // Description: Returns a point on the surface of the sphere. 00922 // latitude and longitude range from 0.0 to 1.0. This 00923 // is used by fill_viz_geom() to create a visible 00924 // representation of the sphere. 00925 //////////////////////////////////////////////////////////////////// 00926 Vertexf CollisionSphere:: 00927 compute_point(float latitude, float longitude) const { 00928 float s1, c1; 00929 csincos(latitude * MathNumbers::pi_f, &s1, &c1); 00930 00931 float s2, c2; 00932 csincos(longitude * 2.0f * MathNumbers::pi_f, &s2, &c2); 00933 00934 Vertexf p(s1 * c2, s1 * s2, c1); 00935 return p * get_radius() + get_center(); 00936 } 00937 00938 //////////////////////////////////////////////////////////////////// 00939 // Function: CollisionSphere::register_with_read_factory 00940 // Access: Public, Static 00941 // Description: Factory method to generate a CollisionSphere object 00942 //////////////////////////////////////////////////////////////////// 00943 void CollisionSphere:: 00944 register_with_read_factory() { 00945 BamReader::get_factory()->register_factory(get_class_type(), make_CollisionSphere); 00946 } 00947 00948 //////////////////////////////////////////////////////////////////// 00949 // Function: CollisionSphere::write_datagram 00950 // Access: Public 00951 // Description: Function to write the important information in 00952 // the particular object to a Datagram 00953 //////////////////////////////////////////////////////////////////// 00954 void CollisionSphere:: 00955 write_datagram(BamWriter *manager, Datagram &me) { 00956 CollisionSolid::write_datagram(manager, me); 00957 _center.write_datagram(me); 00958 me.add_float32(_radius); 00959 } 00960 00961 //////////////////////////////////////////////////////////////////// 00962 // Function: CollisionSphere::make_CollisionSphere 00963 // Access: Protected 00964 // Description: Factory method to generate a CollisionSphere object 00965 //////////////////////////////////////////////////////////////////// 00966 TypedWritable *CollisionSphere:: 00967 make_CollisionSphere(const FactoryParams ¶ms) { 00968 CollisionSphere *me = new CollisionSphere; 00969 DatagramIterator scan; 00970 BamReader *manager; 00971 00972 parse_params(params, scan, manager); 00973 me->fillin(scan, manager); 00974 return me; 00975 } 00976 00977 //////////////////////////////////////////////////////////////////// 00978 // Function: CollisionSphere::fillin 00979 // Access: Protected 00980 // Description: Function that reads out of the datagram (or asks 00981 // manager to read) all of the data that is needed to 00982 // re-create this object and stores it in the appropiate 00983 // place 00984 //////////////////////////////////////////////////////////////////// 00985 void CollisionSphere:: 00986 fillin(DatagramIterator& scan, BamReader* manager) { 00987 CollisionSolid::fillin(scan, manager); 00988 _center.read_datagram(scan); 00989 _radius = scan.get_float32(); 00990 }