Panda3D

collisionTube.cxx

00001 // Filename: collisionTube.cxx
00002 // Created by:  drose (25Sep03)
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 #include "collisionTube.h"
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 "look_at.h"
00024 #include "geom.h"
00025 #include "geomNode.h"
00026 #include "geometricBoundingVolume.h"
00027 #include "datagram.h"
00028 #include "datagramIterator.h"
00029 #include "bamReader.h"
00030 #include "bamWriter.h"
00031 #include "cmath.h"
00032 #include "transformState.h"
00033 #include "geom.h"
00034 #include "geomTristrips.h"
00035 #include "geomVertexWriter.h"
00036 #include "boundingSphere.h"
00037 
00038 PStatCollector CollisionTube::_volume_pcollector("Collision Volumes:CollisionTube");
00039 PStatCollector CollisionTube::_test_pcollector("Collision Tests:CollisionTube");
00040 TypeHandle CollisionTube::_type_handle;
00041 
00042 ////////////////////////////////////////////////////////////////////
00043 //     Function: CollisionTube::make_copy
00044 //       Access: Public, Virtual
00045 //  Description:
00046 ////////////////////////////////////////////////////////////////////
00047 CollisionSolid *CollisionTube::
00048 make_copy() {
00049   return new CollisionTube(*this);
00050 }
00051 
00052 ////////////////////////////////////////////////////////////////////
00053 //     Function: CollisionTube::xform
00054 //       Access: Public, Virtual
00055 //  Description: Transforms the solid by the indicated matrix.
00056 ////////////////////////////////////////////////////////////////////
00057 void CollisionTube::
00058 xform(const LMatrix4 &mat) {
00059   _a = _a * mat;
00060   _b = _b * mat;
00061 
00062   // This is a little cheesy and fails miserably in the presence of a
00063   // non-uniform scale.
00064   LVector3 radius_v = LVector3(_radius, 0.0f, 0.0f) * mat;
00065   _radius = length(radius_v);
00066 
00067   recalc_internals();
00068   CollisionSolid::xform(mat);
00069 }
00070 
00071 ////////////////////////////////////////////////////////////////////
00072 //     Function: CollisionTube::get_collision_origin
00073 //       Access: Public, Virtual
00074 //  Description: Returns the point in space deemed to be the "origin"
00075 //               of the solid for collision purposes.  The closest
00076 //               intersection point to this origin point is considered
00077 //               to be the most significant.
00078 ////////////////////////////////////////////////////////////////////
00079 LPoint3 CollisionTube::
00080 get_collision_origin() const {
00081   return get_point_a();
00082 }
00083 
00084 ////////////////////////////////////////////////////////////////////
00085 //     Function: CollisionTube::get_volume_pcollector
00086 //       Access: Public, Virtual
00087 //  Description: Returns a PStatCollector that is used to count the
00088 //               number of bounding volume tests made against a solid
00089 //               of this type in a given frame.
00090 ////////////////////////////////////////////////////////////////////
00091 PStatCollector &CollisionTube::
00092 get_volume_pcollector() {
00093   return _volume_pcollector;
00094 }
00095 
00096 ////////////////////////////////////////////////////////////////////
00097 //     Function: CollisionTube::get_test_pcollector
00098 //       Access: Public, Virtual
00099 //  Description: Returns a PStatCollector that is used to count the
00100 //               number of intersection tests made against a solid
00101 //               of this type in a given frame.
00102 ////////////////////////////////////////////////////////////////////
00103 PStatCollector &CollisionTube::
00104 get_test_pcollector() {
00105   return _test_pcollector;
00106 }
00107 
00108 ////////////////////////////////////////////////////////////////////
00109 //     Function: CollisionTube::output
00110 //       Access: Public, Virtual
00111 //  Description:
00112 ////////////////////////////////////////////////////////////////////
00113 void CollisionTube::
00114 output(ostream &out) const {
00115   out << "tube, a (" << _a << "), b (" << _b << "), r " << _radius;
00116 }
00117 
00118 ////////////////////////////////////////////////////////////////////
00119 //     Function: CollisionTube::compute_internal_bounds
00120 //       Access: Protected, Virtual
00121 //  Description:
00122 ////////////////////////////////////////////////////////////////////
00123 PT(BoundingVolume) CollisionTube::
00124 compute_internal_bounds() const {
00125   PT(BoundingVolume) bound = CollisionSolid::compute_internal_bounds();
00126 
00127   if (bound->is_of_type(GeometricBoundingVolume::get_class_type())) {
00128     GeometricBoundingVolume *gbound;
00129     DCAST_INTO_R(gbound, bound, bound);
00130 
00131     LVector3 vec = (_b - _a);
00132     if (vec.normalize()) {
00133       // The bounding volume includes both endpoints, plus a little
00134       // bit more to include the radius in both directions.
00135       LPoint3 points[2];
00136       points[0] = _a - vec * _radius;
00137       points[1] = _b + vec * _radius;
00138 
00139       gbound->around(points, points + 2);
00140 
00141     } else {
00142       // Both endpoints are coincident; therefore, the bounding volume
00143       // is a sphere.
00144       BoundingSphere sphere(_a, _radius);
00145       gbound->extend_by(&sphere);
00146     }
00147   }
00148 
00149   return bound;
00150 }
00151 
00152 ////////////////////////////////////////////////////////////////////
00153 //     Function: CollisionTube::test_intersection_from_sphere
00154 //       Access: Public, Virtual
00155 //  Description:
00156 ////////////////////////////////////////////////////////////////////
00157 PT(CollisionEntry) CollisionTube::
00158 test_intersection_from_sphere(const CollisionEntry &entry) const {
00159   const CollisionSphere *sphere;
00160   DCAST_INTO_R(sphere, entry.get_from(), 0);
00161 
00162   CPT(TransformState) wrt_space = entry.get_wrt_space();
00163   CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
00164 
00165   const LMatrix4 &wrt_mat = wrt_space->get_mat();
00166 
00167   LPoint3 from_a = sphere->get_center() * wrt_mat;
00168   LPoint3 from_b = from_a;
00169 
00170   LPoint3 contact_point;
00171   PN_stdfloat actual_t = 0.0f;
00172 
00173   if (wrt_prev_space != wrt_space) {
00174     // If the sphere is moving relative to the tube, it becomes a tube
00175     // itself.
00176     from_a = sphere->get_center() * wrt_prev_space->get_mat();
00177   }
00178 
00179   LVector3 from_direction = from_b - from_a;
00180 
00181   LVector3 from_radius_v =
00182     LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
00183   PN_stdfloat from_radius = length(from_radius_v);
00184 
00185   double t1, t2;
00186   if (!intersects_line(t1, t2, from_a, from_direction, from_radius)) {
00187     // No intersection.
00188     return NULL;
00189   }
00190 
00191   if (t2 < 0.0 || t1 > 1.0) {
00192     // Both intersection points are before the start of the segment or
00193     // after the end of the segment.
00194     return NULL;
00195   }
00196 
00197   // doubles, not floats, to satisfy min and max templates.
00198   actual_t = min(1.0, max(0.0, t1));
00199   contact_point = from_a + actual_t * (from_b - from_a);
00200 
00201   if (collide_cat.is_debug()) {
00202     collide_cat.debug()
00203       << "intersection detected from " << entry.get_from_node_path() << " into "
00204       << entry.get_into_node_path() << "\n";
00205   }
00206   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00207 
00208   LPoint3 into_intersection_point;
00209   if (t2 > 1.0) {
00210     // Point b is within the tube.  The first intersection point is
00211     // point b itself.
00212     into_intersection_point = from_b;
00213   } else {
00214     // Point b is outside the tube, and point a is either inside the
00215     // tube or beyond it.  The first intersection point is at t2.
00216     into_intersection_point = from_a + t2 * from_direction;
00217   }
00218   set_intersection_point(new_entry, into_intersection_point, from_radius);
00219 
00220   LPoint3 fake_contact_point;
00221   LVector3 contact_normal;
00222   calculate_surface_point_and_normal(contact_point,
00223                                      from_radius,
00224                                      fake_contact_point,
00225                                      contact_normal);
00226   new_entry->set_contact_pos(contact_point);
00227   new_entry->set_contact_normal(contact_normal);
00228   new_entry->set_t(actual_t);
00229 
00230   return new_entry;
00231 }
00232 
00233 ////////////////////////////////////////////////////////////////////
00234 //     Function: CollisionTube::test_intersection_from_line
00235 //       Access: Public, Virtual
00236 //  Description:
00237 ////////////////////////////////////////////////////////////////////
00238 PT(CollisionEntry) CollisionTube::
00239 test_intersection_from_line(const CollisionEntry &entry) const {
00240   const CollisionLine *line;
00241   DCAST_INTO_R(line, entry.get_from(), 0);
00242 
00243   const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00244 
00245   LPoint3 from_origin = line->get_origin() * wrt_mat;
00246   LVector3 from_direction = line->get_direction() * wrt_mat;
00247 
00248   double t1, t2;
00249   if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
00250     // No intersection.
00251     return NULL;
00252   }
00253 
00254   if (collide_cat.is_debug()) {
00255     collide_cat.debug()
00256       << "intersection detected from " << entry.get_from_node_path()
00257       << " into " << entry.get_into_node_path() << "\n";
00258   }
00259   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00260 
00261   LPoint3 into_intersection_point = from_origin + t1 * from_direction;
00262   set_intersection_point(new_entry, into_intersection_point, 0.0);
00263 
00264   if (has_effective_normal() && line->get_respect_effective_normal()) {
00265     new_entry->set_surface_normal(get_effective_normal());
00266 
00267   } else {
00268     LVector3 normal = into_intersection_point * _inv_mat;
00269     if (normal[1] > _length) {
00270       // The point is within the top endcap.
00271       normal[1] -= _length;
00272     } else if (normal[1] > 0.0f) {
00273       // The point is within the cylinder body.
00274       normal[1] = 0;
00275     }
00276     normal = normalize(normal * _mat);
00277     new_entry->set_surface_normal(normal);
00278   }
00279 
00280   return new_entry;
00281 }
00282 
00283 ////////////////////////////////////////////////////////////////////
00284 //     Function: CollisionTube::test_intersection_from_ray
00285 //       Access: Public, Virtual
00286 //  Description:
00287 ////////////////////////////////////////////////////////////////////
00288 PT(CollisionEntry) CollisionTube::
00289 test_intersection_from_ray(const CollisionEntry &entry) const {
00290   const CollisionRay *ray;
00291   DCAST_INTO_R(ray, entry.get_from(), 0);
00292 
00293   const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00294 
00295   LPoint3 from_origin = ray->get_origin() * wrt_mat;
00296   LVector3 from_direction = ray->get_direction() * wrt_mat;
00297 
00298   double t1, t2;
00299   if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
00300     // No intersection.
00301     return NULL;
00302   }
00303 
00304   if (t2 < 0.0) {
00305     // Both intersection points are before the start of the ray.
00306     return NULL;
00307   }
00308 
00309   if (collide_cat.is_debug()) {
00310     collide_cat.debug()
00311       << "intersection detected from " << entry.get_from_node_path()
00312       << " into " << entry.get_into_node_path() << "\n";
00313   }
00314   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00315 
00316   LPoint3 into_intersection_point;
00317   if (t1 < 0.0) {
00318     // Point a is within the tube.  The first intersection point is
00319     // point a itself.
00320     into_intersection_point = from_origin;
00321   } else {
00322     // Point a is outside the tube.  The first intersection point is
00323     // at t1.
00324     into_intersection_point = from_origin + t1 * from_direction;
00325   }
00326   set_intersection_point(new_entry, into_intersection_point, 0.0);
00327 
00328   if (has_effective_normal() && ray->get_respect_effective_normal()) {
00329     new_entry->set_surface_normal(get_effective_normal());
00330 
00331   } else {
00332     LVector3 normal = into_intersection_point * _inv_mat;
00333     if (normal[1] > _length) {
00334       // The point is within the top endcap.
00335       normal[1] -= _length;
00336     } else if (normal[1] > 0.0f) {
00337       // The point is within the cylinder body.
00338       normal[1] = 0;
00339     }
00340     normal = normalize(normal * _mat);
00341     new_entry->set_surface_normal(normal);
00342   }
00343 
00344   return new_entry;
00345 }
00346 
00347 ////////////////////////////////////////////////////////////////////
00348 //     Function: CollisionTube::test_intersection_from_segment
00349 //       Access: Public, Virtual
00350 //  Description:
00351 ////////////////////////////////////////////////////////////////////
00352 PT(CollisionEntry) CollisionTube::
00353 test_intersection_from_segment(const CollisionEntry &entry) const {
00354   const CollisionSegment *segment;
00355   DCAST_INTO_R(segment, entry.get_from(), 0);
00356 
00357   const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00358 
00359   LPoint3 from_a = segment->get_point_a() * wrt_mat;
00360   LPoint3 from_b = segment->get_point_b() * wrt_mat;
00361   LVector3 from_direction = from_b - from_a;
00362 
00363   double t1, t2;
00364   if (!intersects_line(t1, t2, from_a, from_direction, 0.0f)) {
00365     // No intersection.
00366     return NULL;
00367   }
00368 
00369   if (t2 < 0.0 || t1 > 1.0) {
00370     // Both intersection points are before the start of the segment or
00371     // after the end of the segment.
00372     return NULL;
00373   }
00374 
00375   if (collide_cat.is_debug()) {
00376     collide_cat.debug()
00377       << "intersection detected from " << entry.get_from_node_path()
00378       << " into " << entry.get_into_node_path() << "\n";
00379   }
00380   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00381 
00382   LPoint3 into_intersection_point;
00383   if (t1 < 0.0) {
00384     // Point a is within the tube.  The first intersection point is
00385     // point a itself.
00386     into_intersection_point = from_a;
00387   } else {
00388     // Point a is outside the tube, and point b is either inside the
00389     // tube or beyond it.  The first intersection point is at t1.
00390     into_intersection_point = from_a + t1 * from_direction;
00391   }
00392   set_intersection_point(new_entry, into_intersection_point, 0.0);
00393 
00394   if (has_effective_normal() && segment->get_respect_effective_normal()) {
00395     new_entry->set_surface_normal(get_effective_normal());
00396 
00397   } else {
00398     LVector3 normal = into_intersection_point * _inv_mat;
00399     if (normal[1] > _length) {
00400       // The point is within the top endcap.
00401       normal[1] -= _length;
00402     } else if (normal[1] > 0.0f) {
00403       // The point is within the cylinder body.
00404       normal[1] = 0;
00405     }
00406     normal = normalize(normal * _mat);
00407     new_entry->set_surface_normal(normal);
00408   }
00409 
00410   return new_entry;
00411 }
00412 
00413 ////////////////////////////////////////////////////////////////////
00414 //     Function: CollisionTube::test_intersection_from_parabola
00415 //       Access: Public, Virtual
00416 //  Description:
00417 ////////////////////////////////////////////////////////////////////
00418 PT(CollisionEntry) CollisionTube::
00419 test_intersection_from_parabola(const CollisionEntry &entry) const {
00420   const CollisionParabola *parabola;
00421   DCAST_INTO_R(parabola, entry.get_from(), 0);
00422 
00423   const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00424 
00425   // Convert the parabola into local coordinate space.
00426   LParabola local_p(parabola->get_parabola());
00427   local_p.xform(wrt_mat);
00428 
00429   double t;
00430   if (!intersects_parabola(t, local_p, parabola->get_t1(), parabola->get_t2(),
00431                            local_p.calc_point(parabola->get_t1()),
00432                            local_p.calc_point(parabola->get_t2()))) {
00433     // No intersection.
00434     return NULL;
00435   }
00436 
00437   if (collide_cat.is_debug()) {
00438     collide_cat.debug()
00439       << "intersection detected from " << entry.get_from_node_path()
00440       << " into " << entry.get_into_node_path() << "\n";
00441   }
00442   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00443 
00444   LPoint3 into_intersection_point = local_p.calc_point(t);
00445   set_intersection_point(new_entry, into_intersection_point, 0.0);
00446 
00447   if (has_effective_normal() && parabola->get_respect_effective_normal()) {
00448     new_entry->set_surface_normal(get_effective_normal());
00449 
00450   } else {
00451     LVector3 normal = into_intersection_point * _inv_mat;
00452     if (normal[1] > _length) {
00453       // The point is within the top endcap.
00454       normal[1] -= _length;
00455     } else if (normal[1] > 0.0f) {
00456       // The point is within the cylinder body.
00457       normal[1] = 0;
00458     }
00459     normal = normalize(normal * _mat);
00460     new_entry->set_surface_normal(normal);
00461   }
00462 
00463   return new_entry;
00464 }
00465 
00466 ////////////////////////////////////////////////////////////////////
00467 //     Function: CollisionTube::fill_viz_geom
00468 //       Access: Protected, Virtual
00469 //  Description: Fills the _viz_geom GeomNode up with Geoms suitable
00470 //               for rendering this solid.
00471 ////////////////////////////////////////////////////////////////////
00472 void CollisionTube::
00473 fill_viz_geom() {
00474   if (collide_cat.is_debug()) {
00475     collide_cat.debug()
00476       << "Recomputing viz for " << *this << "\n";
00477   }
00478 
00479   // Generate the vertices such that we draw a tube with one endpoint
00480   // at (0, 0, 0), and another at (0, length, 0).  Then we'll rotate
00481   // and translate it into place with the appropriate look_at matrix.
00482   LVector3 direction = (_b - _a);
00483   PN_stdfloat length = direction.length();
00484 
00485   PT(GeomVertexData) vdata = new GeomVertexData
00486     ("collision", GeomVertexFormat::get_v3(),
00487      Geom::UH_static);
00488   GeomVertexWriter vertex(vdata, InternalName::get_vertex());
00489   
00490   PT(GeomTristrips) strip = new GeomTristrips(Geom::UH_static);
00491   // Generate the first endcap.
00492   static const int num_slices = 8;
00493   static const int num_rings = 4;
00494   int ri, si;
00495   for (ri = 0; ri < num_rings; ri++) {
00496     for (si = 0; si <= num_slices; si++) {
00497       vertex.add_data3(calc_sphere1_vertex(ri, si, num_rings, num_slices));
00498       vertex.add_data3(calc_sphere1_vertex(ri + 1, si, num_rings, num_slices));
00499     }
00500     strip->add_next_vertices((num_slices + 1) * 2);
00501     strip->close_primitive();
00502   }
00503   
00504   // Now the cylinder sides.
00505   for (si = 0; si <= num_slices; si++) {
00506     vertex.add_data3(calc_sphere1_vertex(num_rings, si, num_rings, num_slices));
00507     vertex.add_data3(calc_sphere2_vertex(num_rings, si, num_rings, num_slices,
00508                                           length));
00509   }
00510   strip->add_next_vertices((num_slices + 1) * 2);
00511   strip->close_primitive();
00512   
00513   // And the second endcap.
00514   for (ri = num_rings - 1; ri >= 0; ri--) {
00515     for (si = 0; si <= num_slices; si++) {
00516       vertex.add_data3(calc_sphere2_vertex(ri + 1, si, num_rings, num_slices, length));
00517       vertex.add_data3(calc_sphere2_vertex(ri, si, num_rings, num_slices, length));
00518     }
00519     strip->add_next_vertices((num_slices + 1) * 2);
00520     strip->close_primitive();
00521   }
00522   
00523   PT(Geom) geom = new Geom(vdata);
00524   geom->add_primitive(strip);
00525   
00526   // Now transform the vertices to their actual location.
00527   LMatrix4 mat;
00528   look_at(mat, direction, LVector3(0.0f, 0.0f, 1.0f), CS_zup_right);
00529   mat.set_row(3, _a);
00530   geom->transform_vertices(mat);
00531   
00532   _viz_geom->add_geom(geom, get_solid_viz_state());
00533   _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state());
00534 }
00535 
00536 ////////////////////////////////////////////////////////////////////
00537 //     Function: CollisionTube::recalc_internals
00538 //       Access: Private
00539 //  Description: Should be called internally to recompute the matrix
00540 //               and length when the properties of the tube have
00541 //               changed.
00542 ////////////////////////////////////////////////////////////////////
00543 void CollisionTube::
00544 recalc_internals() {
00545   LVector3 direction = (_b - _a);
00546   _length = direction.length();
00547 
00548   look_at(_mat, direction, LVector3(0.0f, 0.0f, 1.0f), CS_zup_right);
00549   _mat.set_row(3, _a);
00550   _inv_mat.invert_from(_mat);
00551 
00552   mark_viz_stale();
00553   mark_internal_bounds_stale();
00554 }
00555 
00556 ////////////////////////////////////////////////////////////////////
00557 //     Function: CollisionTube::calc_sphere1_vertex
00558 //       Access: Private
00559 //  Description: Calculates a particular vertex on the surface of the
00560 //               first endcap hemisphere, for use in generating the
00561 //               viz geometry.
00562 ////////////////////////////////////////////////////////////////////
00563 LVertex CollisionTube::
00564 calc_sphere1_vertex(int ri, int si, int num_rings, int num_slices) {
00565   PN_stdfloat r = (PN_stdfloat)ri / (PN_stdfloat)num_rings;
00566   PN_stdfloat s = (PN_stdfloat)si / (PN_stdfloat)num_slices;
00567 
00568   // Find the point on the rim, based on the slice.
00569   PN_stdfloat theta = s * 2.0f * MathNumbers::pi;
00570   PN_stdfloat x_rim = ccos(theta);
00571   PN_stdfloat z_rim = csin(theta);
00572 
00573   // Now pull that point in towards the pole, based on the ring.
00574   PN_stdfloat phi = r * 0.5f * MathNumbers::pi;
00575   PN_stdfloat to_pole = csin(phi);
00576 
00577   PN_stdfloat x = _radius * x_rim * to_pole;
00578   PN_stdfloat y = -_radius * ccos(phi);
00579   PN_stdfloat z = _radius * z_rim * to_pole;
00580 
00581   return LVertex(x, y, z);
00582 }
00583 
00584 ////////////////////////////////////////////////////////////////////
00585 //     Function: CollisionTube::calc_sphere2_vertex
00586 //       Access: Private
00587 //  Description: Calculates a particular vertex on the surface of the
00588 //               second endcap hemisphere, for use in generating the
00589 //               viz geometry.
00590 ////////////////////////////////////////////////////////////////////
00591 LVertex CollisionTube::
00592 calc_sphere2_vertex(int ri, int si, int num_rings, int num_slices,
00593                     PN_stdfloat length) {
00594   PN_stdfloat r = (PN_stdfloat)ri / (PN_stdfloat)num_rings;
00595   PN_stdfloat s = (PN_stdfloat)si / (PN_stdfloat)num_slices;
00596 
00597   // Find the point on the rim, based on the slice.
00598   PN_stdfloat theta = s * 2.0f * MathNumbers::pi;
00599   PN_stdfloat x_rim = ccos(theta);
00600   PN_stdfloat z_rim = csin(theta);
00601 
00602   // Now pull that point in towards the pole, based on the ring.
00603   PN_stdfloat phi = r * 0.5f * MathNumbers::pi;
00604   PN_stdfloat to_pole = csin(phi);
00605 
00606   PN_stdfloat x = _radius * x_rim * to_pole;
00607   PN_stdfloat y = length + _radius * ccos(phi);
00608   PN_stdfloat z = _radius * z_rim * to_pole;
00609 
00610   return LVertex(x, y, z);
00611 }
00612 
00613 ////////////////////////////////////////////////////////////////////
00614 //     Function: CollisionTube::intersects_line
00615 //       Access: Private
00616 //  Description: Determine the point(s) of intersection of a parametric
00617 //               line with the tube.  The line is infinite in both
00618 //               directions, and passes through "from" and from+delta.
00619 //               If the line does not intersect the tube, the
00620 //               function returns false, and t1 and t2 are undefined.
00621 //               If it does intersect the tube, it returns true, and
00622 //               t1 and t2 are set to the points along the equation
00623 //               from+t*delta that correspond to the two points of
00624 //               intersection.
00625 ////////////////////////////////////////////////////////////////////
00626 bool CollisionTube::
00627 intersects_line(double &t1, double &t2,
00628                 const LPoint3 &from0, const LVector3 &delta0, 
00629                 PN_stdfloat inflate_radius) const {
00630   // Convert the line into our canonical coordinate space: the tube is
00631   // aligned with the y axis.
00632   LPoint3 from = from0 * _inv_mat;
00633   LVector3 delta = delta0 * _inv_mat;
00634 
00635   PN_stdfloat radius = _radius + inflate_radius;
00636 
00637   // Now project the line into the X-Z plane to test for intersection
00638   // with a 2-d circle around the origin.  The equation for this is
00639   // very similar to the formula for the intersection of a line with a
00640   // sphere; see CollisionSphere::intersects_line() for the complete
00641   // derivation.  It's a little bit simpler because the circle is
00642   // centered on the origin.
00643   LVector2 from2(from[0], from[2]);
00644   LVector2 delta2(delta[0], delta[2]);
00645 
00646   double A = dot(delta2, delta2);
00647 
00648   if (IS_NEARLY_ZERO(A)) {
00649     // If the delta2 is 0, the line is perpendicular to the X-Z plane.
00650     // The whole line intersects with the infinite cylinder if the
00651     // point is within the circle.
00652     if (from2.dot(from2) > radius * radius) {
00653       // Nope, the 2-d point is outside the circle, so no
00654       // intersection.
00655       return false;
00656     }
00657 
00658     if (IS_NEARLY_ZERO(delta[1])) {
00659       // Actually, the whole delta vector is 0, so the line is just a
00660       // point.  In this case, (since we have already shown the point
00661       // is within the infinite cylinder), we intersect if and only if
00662       // the three-dimensional point is between the endcaps.
00663       if (from[1] < -radius || from[1] > _length + radius) {
00664         // Way out.
00665         return false;
00666       }
00667       if (from[1] < 0.0f) {
00668         // Possibly within the first endcap.
00669         if (from.dot(from) > radius * radius) {
00670           return false;
00671         }
00672       } else if (from[1] > _length) {
00673         // Possibly within the second endcap.
00674         from[1] -= _length;
00675         if (from.dot(from) > radius * radius) {
00676           return false;
00677         }
00678       }
00679 
00680       // The point is within the tube!
00681       t1 = t2 = 0.0;
00682       return true;
00683     }
00684 
00685     // The 2-d point is within the circle, so compute our intersection
00686     // points to include the entire vertical slice of the cylinder.
00687     t1 = (-radius - from[1]) / delta[1];
00688     t2 = (_length + radius - from[1]) / delta[1];
00689 
00690   } else {
00691     // The line is not perpendicular to the X-Z plane, so its
00692     // projection into the plane is 2-d line.  Test that 2-d line for
00693     // intersection with the circular projection of the cylinder.
00694 
00695     double B = 2.0f * dot(delta2, from2);
00696     double fc_d2 = dot(from2, from2);
00697     double C = fc_d2 - radius * radius;
00698     
00699     double radical = B*B - 4.0*A*C;
00700     
00701     if (IS_NEARLY_ZERO(radical)) {
00702       // Tangent.
00703       t1 = t2 = -B / (2.0*A);
00704       
00705     } else if (radical < 0.0) {
00706       // No real roots: no intersection with the line.
00707       return false;
00708       
00709     } else {
00710       double reciprocal_2A = 1.0 / (2.0 * A);
00711       double sqrt_radical = sqrtf(radical);
00712       t1 = ( -B - sqrt_radical ) * reciprocal_2A;
00713       t2 = ( -B + sqrt_radical ) * reciprocal_2A;
00714     }
00715   }
00716 
00717   // Now we need to verify that the intersection points fall within
00718   // the length of the cylinder.
00719   PN_stdfloat t1_y = from[1] + t1 * delta[1];
00720   PN_stdfloat t2_y = from[1] + t2 * delta[1];
00721 
00722   if (t1_y < -radius && t2_y < -radius) {
00723     // Both points are way off the bottom of the tube; no
00724     // intersection.
00725     return false;
00726   } else if (t1_y > _length + radius && t2_y > _length + radius) {
00727     // Both points are way off the top of the tube; no intersection.
00728     return false;
00729   }
00730 
00731   if (t1_y < 0.0f) {
00732     // The starting point is off the bottom of the tube.  Test the
00733     // line against the first endcap.
00734     double t1a, t2a;
00735     if (!sphere_intersects_line(t1a, t2a, 0.0f, from, delta, inflate_radius)) {
00736       // If there's no intersection with the endcap, there can't be an
00737       // intersection with the cylinder.
00738       return false;
00739     }
00740     t1 = t1a;
00741 
00742   } else if (t1_y > _length) {
00743     // The starting point is off the top of the tube.  Test the
00744     // line against the second endcap.
00745     double t1b, t2b;
00746     if (!sphere_intersects_line(t1b, t2b, _length, from, delta, inflate_radius)) {
00747       // If there's no intersection with the endcap, there can't be an
00748       // intersection with the cylinder.
00749       return false;
00750     }
00751     t1 = t1b;
00752   }
00753 
00754   if (t2_y < 0.0f) {
00755     // The ending point is off the bottom of the tube.  Test the
00756     // line against the first endcap.
00757     double t1a, t2a;
00758     if (!sphere_intersects_line(t1a, t2a, 0.0f, from, delta, inflate_radius)) {
00759       // If there's no intersection with the endcap, there can't be an
00760       // intersection with the cylinder.
00761       return false;
00762     }
00763     t2 = t2a;
00764 
00765   } else if (t2_y > _length) {
00766     // The ending point is off the top of the tube.  Test the
00767     // line against the second endcap.
00768     double t1b, t2b;
00769     if (!sphere_intersects_line(t1b, t2b, _length, from, delta, inflate_radius)) {
00770       // If there's no intersection with the endcap, there can't be an
00771       // intersection with the cylinder.
00772       return false;
00773     }
00774     t2 = t2b;
00775   }
00776 
00777   return true;
00778 }
00779 
00780 ////////////////////////////////////////////////////////////////////
00781 //     Function: CollisionTube::sphere_intersects_line
00782 //       Access: Private
00783 //  Description: After confirming that the line intersects an infinite
00784 //               cylinder, test whether it intersects one or the other
00785 //               endcaps.  The y parameter specifies the center of the
00786 //               sphere (and hence the particular endcap.
00787 ////////////////////////////////////////////////////////////////////
00788 bool CollisionTube::
00789 sphere_intersects_line(double &t1, double &t2, PN_stdfloat center_y,
00790                        const LPoint3 &from, const LVector3 &delta,
00791                        PN_stdfloat inflate_radius) const {
00792   // See CollisionSphere::intersects_line() for a derivation of the
00793   // formula here.
00794   PN_stdfloat radius = _radius + inflate_radius;
00795 
00796   double A = dot(delta, delta);
00797 
00798   nassertr(A != 0.0, false);
00799 
00800   LVector3 fc = from;
00801   fc[1] -= center_y;
00802   double B = 2.0f* dot(delta, fc);
00803   double fc_d2 = dot(fc, fc);
00804   double C = fc_d2 - radius * radius;
00805 
00806   double radical = B*B - 4.0*A*C;
00807 
00808   if (IS_NEARLY_ZERO(radical)) {
00809     // Tangent.
00810     t1 = t2 = -B / (2.0 * A);
00811     return true;
00812 
00813   } else if (radical < 0.0) {
00814     // No real roots: no intersection with the line.
00815     return false;
00816   }
00817 
00818   double reciprocal_2A = 1.0 / (2.0 * A);
00819   double sqrt_radical = sqrtf(radical);
00820   t1 = ( -B - sqrt_radical ) * reciprocal_2A;
00821   t2 = ( -B + sqrt_radical ) * reciprocal_2A;
00822 
00823   return true;
00824 }
00825 
00826 ////////////////////////////////////////////////////////////////////
00827 //     Function: CollisionTube::intersects_parabola
00828 //       Access: Protected
00829 //  Description: Determine a point of intersection of a parametric
00830 //               parabola with the tube.
00831 //
00832 //               We only consider the segment of the parabola between
00833 //               t1 and t2, which has already been computed as
00834 //               corresponding to points p1 and p2.  If there is an
00835 //               intersection, t is set to the parametric point of
00836 //               intersection, and true is returned; otherwise, false
00837 //               is returned.
00838 ////////////////////////////////////////////////////////////////////
00839 bool CollisionTube::
00840 intersects_parabola(double &t, const LParabola &parabola,
00841                     double t1, double t2,
00842                     const LPoint3 &p1, const LPoint3 &p2) const {
00843   // I don't even want to think about the math to do this calculation
00844   // directly--it's even worse than sphere-parabola.  So I'll use the
00845   // recursive subdivision solution again, just like I did for
00846   // sphere-parabola.
00847 
00848   // First, see if the line segment (p1 - p2) comes sufficiently close
00849   // to the parabola.  Do this by computing the parametric intervening
00850   // point and comparing its distance from the linear intervening
00851   // point.
00852   double tmid = (t1 + t2) * 0.5;
00853 
00854   if (tmid != t1 && tmid != t2) {
00855     LPoint3 pmid = parabola.calc_point(tmid);
00856     LPoint3 pmid2 = (p1 + p2) * 0.5f;
00857 
00858     if ((pmid - pmid2).length_squared() > 0.001f) {
00859       // Subdivide.
00860       if (intersects_parabola(t, parabola, t1, tmid, p1, pmid)) {
00861         return true;
00862       }
00863       return intersects_parabola(t, parabola, tmid, t2, pmid, p2);
00864     }
00865   }
00866 
00867   // The line segment is sufficiently close; compare the segment itself.
00868   double t1a, t2a;
00869   if (!intersects_line(t1a, t2a, p1, p2 - p1, 0.0f)) {
00870     return false;
00871   }
00872 
00873   if (t2a < 0.0 || t1a > 1.0) {
00874     return false;
00875   }
00876 
00877   t = max(t1a, 0.0);
00878   return true;
00879 }
00880 
00881 ////////////////////////////////////////////////////////////////////
00882 //     Function: CollisionTube::calculate_surface_point_and_normal
00883 //       Access: Private
00884 //  Description: Calculates a point that is exactly on the surface
00885 //               of the tube and its corresponding normal, given
00886 //               a point that is supposedly on the surface of the
00887 //               tube.
00888 ////////////////////////////////////////////////////////////////////
00889 void CollisionTube::
00890 calculate_surface_point_and_normal(const LPoint3 &surface_point,
00891                                    double extra_radius,
00892                                    LPoint3 &result_point,
00893                                    LVector3 &result_normal) const {
00894   // Convert the point into our canonical space for analysis.
00895   LPoint3 point = surface_point * _inv_mat;
00896   LVector3 normal;
00897 
00898   if (point[1] <= 0.0) {
00899     // The point is on the first endcap.
00900     normal = point;
00901     if (!normal.normalize()) {
00902       normal.set(0.0, -1.0, 0.0);
00903     }
00904     point = normal * _radius;
00905 
00906   } else if (point[1] >= _length) {
00907     // The point is on the second endcap.
00908     normal.set(point[0], point[1] - _length, point[2]);
00909     if (!normal.normalize()) {
00910       normal.set(0.0, 1.0, 0.0);
00911     }
00912     point = normal * _radius;
00913     point[1] += _length;
00914 
00915   } else {
00916     // The point is within the cylinder part.
00917     LVector2d normal2d(point[0], point[2]);
00918     if (!normal2d.normalize()) {
00919       normal2d.set(0.0, 1.0);
00920     }
00921     normal.set(normal2d[0], 0.0, normal2d[1]);
00922     point.set(normal[0] * _radius, point[1], normal[2] * _radius);
00923   }
00924 
00925   // Now convert the point and normal back into real space.
00926   result_point = point * _mat;
00927   result_normal = normal * _mat;
00928 }
00929 
00930 ////////////////////////////////////////////////////////////////////
00931 //     Function: CollisionTube::set_intersection_point
00932 //       Access: Private
00933 //  Description: After an intersection has been detected, record the
00934 //               computed intersection point in the CollisionEntry,
00935 //               and also compute the relevant normal based on that
00936 //               point.
00937 ////////////////////////////////////////////////////////////////////
00938 void CollisionTube::
00939 set_intersection_point(CollisionEntry *new_entry, 
00940                        const LPoint3 &into_intersection_point, 
00941                        double extra_radius) const {
00942   LPoint3 point;
00943   LVector3 normal;
00944 
00945   calculate_surface_point_and_normal(into_intersection_point,
00946                                      extra_radius,
00947                                      point,
00948                                      normal);
00949 
00950   if (has_effective_normal() && new_entry->get_from()->get_respect_effective_normal()) {
00951     normal = get_effective_normal();
00952   }
00953 
00954   new_entry->set_surface_normal(normal);
00955   new_entry->set_surface_point(point);
00956   // Also adjust the original point into the tube by the amount of
00957   // extra_radius, which should put it on the surface of the tube if
00958   // our collision was tangential.
00959   new_entry->set_interior_point(into_intersection_point - normal * extra_radius);
00960 }
00961 
00962 ////////////////////////////////////////////////////////////////////
00963 //     Function: CollisionTube::register_with_read_factory
00964 //       Access: Public, Static
00965 //  Description: Tells the BamReader how to create objects of type
00966 //               CollisionTube.
00967 ////////////////////////////////////////////////////////////////////
00968 void CollisionTube::
00969 register_with_read_factory() {
00970   BamReader::get_factory()->register_factory(get_class_type(), make_from_bam);
00971 }
00972 
00973 ////////////////////////////////////////////////////////////////////
00974 //     Function: CollisionTube::write_datagram
00975 //       Access: Public, Virtual
00976 //  Description: Writes the contents of this object to the datagram
00977 //               for shipping out to a Bam file.
00978 ////////////////////////////////////////////////////////////////////
00979 void CollisionTube::
00980 write_datagram(BamWriter *manager, Datagram &dg) {
00981   CollisionSolid::write_datagram(manager, dg);
00982   _a.write_datagram(dg);
00983   _b.write_datagram(dg);
00984   dg.add_stdfloat(_radius);
00985 }
00986 
00987 ////////////////////////////////////////////////////////////////////
00988 //     Function: CollisionTube::make_from_bam
00989 //       Access: Protected, Static
00990 //  Description: This function is called by the BamReader's factory
00991 //               when a new object of type CollisionTube is encountered
00992 //               in the Bam file.  It should create the CollisionTube
00993 //               and extract its information from the file.
00994 ////////////////////////////////////////////////////////////////////
00995 TypedWritable *CollisionTube::
00996 make_from_bam(const FactoryParams &params) {
00997   CollisionTube *node = new CollisionTube();
00998   DatagramIterator scan;
00999   BamReader *manager;
01000 
01001   parse_params(params, scan, manager);
01002   node->fillin(scan, manager);
01003 
01004   return node;
01005 }
01006 
01007 ////////////////////////////////////////////////////////////////////
01008 //     Function: CollisionTube::fillin
01009 //       Access: Protected
01010 //  Description: This internal function is called by make_from_bam to
01011 //               read in all of the relevant data from the BamFile for
01012 //               the new CollisionTube.
01013 ////////////////////////////////////////////////////////////////////
01014 void CollisionTube::
01015 fillin(DatagramIterator &scan, BamReader *manager) {
01016   CollisionSolid::fillin(scan, manager);
01017   _a.read_datagram(scan);
01018   _b.read_datagram(scan);
01019   _radius = scan.get_stdfloat();
01020   recalc_internals();
01021 }
 All Classes Functions Variables Enumerations