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