Panda3D
 All Classes Functions Variables Enumerations
collisionSphere.cxx
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 &parabola,
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 &params) {
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 }
 All Classes Functions Variables Enumerations