Panda3D

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