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 LMatrix4f &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   LVector3f radius_v = LVector3f(_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 LPoint3f 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     LVector3f 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       LPoint3f 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 LMatrix4f &wrt_mat = wrt_space->get_mat();
00166 
00167   LPoint3f from_a = sphere->get_center() * wrt_mat;
00168   LPoint3f from_b = from_a;
00169 
00170   LPoint3f contact_point;
00171   float 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   LVector3f from_direction = from_b - from_a;
00180 
00181   LVector3f from_radius_v =
00182     LVector3f(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
00183   float 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   LPoint3f 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   LPoint3f fake_contact_point;
00221   LVector3f 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 LMatrix4f &wrt_mat = entry.get_wrt_mat();
00244 
00245   LPoint3f from_origin = line->get_origin() * wrt_mat;
00246   LVector3f 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   LPoint3f 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     LVector3f 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 LMatrix4f &wrt_mat = entry.get_wrt_mat();
00294 
00295   LPoint3f from_origin = ray->get_origin() * wrt_mat;
00296   LVector3f 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   LPoint3f 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     LVector3f 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 LMatrix4f &wrt_mat = entry.get_wrt_mat();
00358 
00359   LPoint3f from_a = segment->get_point_a() * wrt_mat;
00360   LPoint3f from_b = segment->get_point_b() * wrt_mat;
00361   LVector3f 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   LPoint3f 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     LVector3f 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 LMatrix4f &wrt_mat = entry.get_wrt_mat();
00424 
00425   // Convert the parabola into local coordinate space.
00426   Parabolaf 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   LPoint3f 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     LVector3f 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   LVector3f direction = (_b - _a);
00483   float 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_data3f(calc_sphere1_vertex(ri, si, num_rings, num_slices));
00498       vertex.add_data3f(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_data3f(calc_sphere1_vertex(num_rings, si, num_rings, num_slices));
00507     vertex.add_data3f(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_data3f(calc_sphere2_vertex(ri + 1, si, num_rings, num_slices, length));
00517       vertex.add_data3f(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   LMatrix4f mat;
00528   look_at(mat, direction, LVector3f(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   LVector3f direction = (_b - _a);
00546   _length = direction.length();
00547 
00548   look_at(_mat, direction, LVector3f(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 Vertexf CollisionTube::
00564 calc_sphere1_vertex(int ri, int si, int num_rings, int num_slices) {
00565   float r = (float)ri / (float)num_rings;
00566   float s = (float)si / (float)num_slices;
00567 
00568   // Find the point on the rim, based on the slice.
00569   float theta = s * 2.0f * MathNumbers::pi_f;
00570   float x_rim = ccos(theta);
00571   float z_rim = csin(theta);
00572 
00573   // Now pull that point in towards the pole, based on the ring.
00574   float phi = r * 0.5f * MathNumbers::pi_f;
00575   float to_pole = csin(phi);
00576 
00577   float x = _radius * x_rim * to_pole;
00578   float y = -_radius * ccos(phi);
00579   float z = _radius * z_rim * to_pole;
00580 
00581   return Vertexf(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 Vertexf CollisionTube::
00592 calc_sphere2_vertex(int ri, int si, int num_rings, int num_slices,
00593                     float length) {
00594   float r = (float)ri / (float)num_rings;
00595   float s = (float)si / (float)num_slices;
00596 
00597   // Find the point on the rim, based on the slice.
00598   float theta = s * 2.0f * MathNumbers::pi_f;
00599   float x_rim = ccos(theta);
00600   float z_rim = csin(theta);
00601 
00602   // Now pull that point in towards the pole, based on the ring.
00603   float phi = r * 0.5f * MathNumbers::pi_f;
00604   float to_pole = csin(phi);
00605 
00606   float x = _radius * x_rim * to_pole;
00607   float y = length + _radius * ccos(phi);
00608   float z = _radius * z_rim * to_pole;
00609 
00610   return Vertexf(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                 LPoint3f from, LVector3f delta, float inflate_radius) const {
00629   // Convert the line into our canonical coordinate space: the tube is
00630   // aligned with the y axis.
00631   from = from * _inv_mat;
00632   delta = delta * _inv_mat;
00633 
00634   float radius = _radius + inflate_radius;
00635 
00636   // Now project the line into the X-Z plane to test for intersection
00637   // with a 2-d circle around the origin.  The equation for this is
00638   // very similar to the formula for the intersection of a line with a
00639   // sphere; see CollisionSphere::intersects_line() for the complete
00640   // derivation.  It's a little bit simpler because the circle is
00641   // centered on the origin.
00642   LVector2f from2(from[0], from[2]);
00643   LVector2f delta2(delta[0], delta[2]);
00644 
00645   double A = dot(delta2, delta2);
00646 
00647   if (IS_NEARLY_ZERO(A)) {
00648     // If the delta2 is 0, the line is perpendicular to the X-Z plane.
00649     // The whole line intersects with the infinite cylinder if the
00650     // point is within the circle.
00651     if (from2.dot(from2) > radius * radius) {
00652       // Nope, the 2-d point is outside the circle, so no
00653       // intersection.
00654       return false;
00655     }
00656 
00657     if (IS_NEARLY_ZERO(delta[1])) {
00658       // Actually, the whole delta vector is 0, so the line is just a
00659       // point.  In this case, (since we have already shown the point
00660       // is within the infinite cylinder), we intersect if and only if
00661       // the three-dimensional point is between the endcaps.
00662       if (from[1] < -radius || from[1] > _length + radius) {
00663         // Way out.
00664         return false;
00665       }
00666       if (from[1] < 0.0f) {
00667         // Possibly within the first endcap.
00668         if (from.dot(from) > radius * radius) {
00669           return false;
00670         }
00671       } else if (from[1] > _length) {
00672         // Possibly within the second endcap.
00673         from[1] -= _length;
00674         if (from.dot(from) > radius * radius) {
00675           return false;
00676         }
00677       }
00678 
00679       // The point is within the tube!
00680       t1 = t2 = 0.0;
00681       return true;
00682     }
00683 
00684     // The 2-d point is within the circle, so compute our intersection
00685     // points to include the entire vertical slice of the cylinder.
00686     t1 = (-radius - from[1]) / delta[1];
00687     t2 = (_length + radius - from[1]) / delta[1];
00688 
00689   } else {
00690     // The line is not perpendicular to the X-Z plane, so its
00691     // projection into the plane is 2-d line.  Test that 2-d line for
00692     // intersection with the circular projection of the cylinder.
00693 
00694     double B = 2.0f * dot(delta2, from2);
00695     double fc_d2 = dot(from2, from2);
00696     double C = fc_d2 - radius * radius;
00697     
00698     double radical = B*B - 4.0*A*C;
00699     
00700     if (IS_NEARLY_ZERO(radical)) {
00701       // Tangent.
00702       t1 = t2 = -B / (2.0*A);
00703       
00704     } else if (radical < 0.0) {
00705       // No real roots: no intersection with the line.
00706       return false;
00707       
00708     } else {
00709       double reciprocal_2A = 1.0 / (2.0 * A);
00710       double sqrt_radical = sqrtf(radical);
00711       t1 = ( -B - sqrt_radical ) * reciprocal_2A;
00712       t2 = ( -B + sqrt_radical ) * reciprocal_2A;
00713     }
00714   }
00715 
00716   // Now we need to verify that the intersection points fall within
00717   // the length of the cylinder.
00718   float t1_y = from[1] + t1 * delta[1];
00719   float t2_y = from[1] + t2 * delta[1];
00720 
00721   if (t1_y < -radius && t2_y < -radius) {
00722     // Both points are way off the bottom of the tube; no
00723     // intersection.
00724     return false;
00725   } else if (t1_y > _length + radius && t2_y > _length + radius) {
00726     // Both points are way off the top of the tube; no intersection.
00727     return false;
00728   }
00729 
00730   if (t1_y < 0.0f) {
00731     // The starting point is off the bottom of the tube.  Test the
00732     // line against the first endcap.
00733     double t1a, t2a;
00734     if (!sphere_intersects_line(t1a, t2a, 0.0f, from, delta, inflate_radius)) {
00735       // If there's no intersection with the endcap, there can't be an
00736       // intersection with the cylinder.
00737       return false;
00738     }
00739     t1 = t1a;
00740 
00741   } else if (t1_y > _length) {
00742     // The starting point is off the top of the tube.  Test the
00743     // line against the second endcap.
00744     double t1b, t2b;
00745     if (!sphere_intersects_line(t1b, t2b, _length, from, delta, inflate_radius)) {
00746       // If there's no intersection with the endcap, there can't be an
00747       // intersection with the cylinder.
00748       return false;
00749     }
00750     t1 = t1b;
00751   }
00752 
00753   if (t2_y < 0.0f) {
00754     // The ending point is off the bottom of the tube.  Test the
00755     // line against the first endcap.
00756     double t1a, t2a;
00757     if (!sphere_intersects_line(t1a, t2a, 0.0f, from, delta, inflate_radius)) {
00758       // If there's no intersection with the endcap, there can't be an
00759       // intersection with the cylinder.
00760       return false;
00761     }
00762     t2 = t2a;
00763 
00764   } else if (t2_y > _length) {
00765     // The ending point is off the top of the tube.  Test the
00766     // line against the second endcap.
00767     double t1b, t2b;
00768     if (!sphere_intersects_line(t1b, t2b, _length, from, delta, inflate_radius)) {
00769       // If there's no intersection with the endcap, there can't be an
00770       // intersection with the cylinder.
00771       return false;
00772     }
00773     t2 = t2b;
00774   }
00775 
00776   return true;
00777 }
00778 
00779 ////////////////////////////////////////////////////////////////////
00780 //     Function: CollisionTube::sphere_intersects_line
00781 //       Access: Private
00782 //  Description: After confirming that the line intersects an infinite
00783 //               cylinder, test whether it intersects one or the other
00784 //               endcaps.  The y parameter specifies the center of the
00785 //               sphere (and hence the particular endcap.
00786 ////////////////////////////////////////////////////////////////////
00787 bool CollisionTube::
00788 sphere_intersects_line(double &t1, double &t2, float center_y,
00789                        const LPoint3f &from, const LVector3f &delta,
00790                        float inflate_radius) const {
00791   // See CollisionSphere::intersects_line() for a derivation of the
00792   // formula here.
00793   float radius = _radius + inflate_radius;
00794 
00795   double A = dot(delta, delta);
00796 
00797   nassertr(A != 0.0, false);
00798 
00799   LVector3f fc = from;
00800   fc[1] -= center_y;
00801   double B = 2.0f* dot(delta, fc);
00802   double fc_d2 = dot(fc, fc);
00803   double C = fc_d2 - radius * radius;
00804 
00805   double radical = B*B - 4.0*A*C;
00806 
00807   if (IS_NEARLY_ZERO(radical)) {
00808     // Tangent.
00809     t1 = t2 = -B / (2.0 * A);
00810     return true;
00811 
00812   } else if (radical < 0.0) {
00813     // No real roots: no intersection with the line.
00814     return false;
00815   }
00816 
00817   double reciprocal_2A = 1.0 / (2.0 * A);
00818   double sqrt_radical = sqrtf(radical);
00819   t1 = ( -B - sqrt_radical ) * reciprocal_2A;
00820   t2 = ( -B + sqrt_radical ) * reciprocal_2A;
00821 
00822   return true;
00823 }
00824 
00825 ////////////////////////////////////////////////////////////////////
00826 //     Function: CollisionTube::intersects_parabola
00827 //       Access: Protected
00828 //  Description: Determine a point of intersection of a parametric
00829 //               parabola with the tube.
00830 //
00831 //               We only consider the segment of the parabola between
00832 //               t1 and t2, which has already been computed as
00833 //               corresponding to points p1 and p2.  If there is an
00834 //               intersection, t is set to the parametric point of
00835 //               intersection, and true is returned; otherwise, false
00836 //               is returned.
00837 ////////////////////////////////////////////////////////////////////
00838 bool CollisionTube::
00839 intersects_parabola(double &t, const Parabolaf &parabola,
00840                     double t1, double t2,
00841                     const LPoint3f &p1, const LPoint3f &p2) const {
00842   // I don't even want to think about the math to do this calculation
00843   // directly--it's even worse than sphere-parabola.  So I'll use the
00844   // recursive subdivision solution again, just like I did for
00845   // sphere-parabola.
00846 
00847   // First, see if the line segment (p1 - p2) comes sufficiently close
00848   // to the parabola.  Do this by computing the parametric intervening
00849   // point and comparing its distance from the linear intervening
00850   // point.
00851   double tmid = (t1 + t2) * 0.5;
00852 
00853   if (tmid != t1 && tmid != t2) {
00854     LPoint3f pmid = parabola.calc_point(tmid);
00855     LPoint3f pmid2 = (p1 + p2) * 0.5f;
00856 
00857     if ((pmid - pmid2).length_squared() > 0.001f) {
00858       // Subdivide.
00859       if (intersects_parabola(t, parabola, t1, tmid, p1, pmid)) {
00860         return true;
00861       }
00862       return intersects_parabola(t, parabola, tmid, t2, pmid, p2);
00863     }
00864   }
00865 
00866   // The line segment is sufficiently close; compare the segment itself.
00867   double t1a, t2a;
00868   if (!intersects_line(t1a, t2a, p1, p2 - p1, 0.0f)) {
00869     return false;
00870   }
00871 
00872   if (t2a < 0.0 || t1a > 1.0) {
00873     return false;
00874   }
00875 
00876   t = max(t1a, 0.0);
00877   return true;
00878 }
00879 
00880 ////////////////////////////////////////////////////////////////////
00881 //     Function: CollisionTube::calculate_surface_point_and_normal
00882 //       Access: Private
00883 //  Description: Calculates a point that is exactly on the surface
00884 //               of the tube and its corresponding normal, given
00885 //               a point that is supposedly on the surface of the
00886 //               tube.
00887 ////////////////////////////////////////////////////////////////////
00888 void CollisionTube::
00889 calculate_surface_point_and_normal(const LPoint3f &surface_point,
00890                                    double extra_radius,
00891                                    LPoint3f &result_point,
00892                                    LVector3f &result_normal) const {
00893   // Convert the point into our canonical space for analysis.
00894   LPoint3f point = surface_point * _inv_mat;
00895   LVector3f normal;
00896 
00897   if (point[1] <= 0.0) {
00898     // The point is on the first endcap.
00899     normal = point;
00900     if (!normal.normalize()) {
00901       normal.set(0.0, -1.0, 0.0);
00902     }
00903     point = normal * _radius;
00904 
00905   } else if (point[1] >= _length) {
00906     // The point is on the second endcap.
00907     normal.set(point[0], point[1] - _length, point[2]);
00908     if (!normal.normalize()) {
00909       normal.set(0.0, 1.0, 0.0);
00910     }
00911     point = normal * _radius;
00912     point[1] += _length;
00913 
00914   } else {
00915     // The point is within the cylinder part.
00916     LVector2d normal2d(point[0], point[2]);
00917     if (!normal2d.normalize()) {
00918       normal2d.set(0.0, 1.0);
00919     }
00920     normal.set(normal2d[0], 0.0, normal2d[1]);
00921     point.set(normal[0] * _radius, point[1], normal[2] * _radius);
00922   }
00923 
00924   // Now convert the point and normal back into real space.
00925   result_point = point * _mat;
00926   result_normal = normal * _mat;
00927 }
00928 
00929 ////////////////////////////////////////////////////////////////////
00930 //     Function: CollisionTube::set_intersection_point
00931 //       Access: Private
00932 //  Description: After an intersection has been detected, record the
00933 //               computed intersection point in the CollisionEntry,
00934 //               and also compute the relevant normal based on that
00935 //               point.
00936 ////////////////////////////////////////////////////////////////////
00937 void CollisionTube::
00938 set_intersection_point(CollisionEntry *new_entry, 
00939                        const LPoint3f &into_intersection_point, 
00940                        double extra_radius) const {
00941   LPoint3f point;
00942   LVector3f normal;
00943 
00944   calculate_surface_point_and_normal(into_intersection_point,
00945                                      extra_radius,
00946                                      point,
00947                                      normal);
00948 
00949   if (has_effective_normal() && new_entry->get_from()->get_respect_effective_normal()) {
00950     normal = get_effective_normal();
00951   }
00952 
00953   new_entry->set_surface_normal(normal);
00954   new_entry->set_surface_point(point);
00955   // Also adjust the original point into the tube by the amount of
00956   // extra_radius, which should put it on the surface of the tube if
00957   // our collision was tangential.
00958   new_entry->set_interior_point(into_intersection_point - normal * extra_radius);
00959 }
00960 
00961 ////////////////////////////////////////////////////////////////////
00962 //     Function: CollisionTube::register_with_read_factory
00963 //       Access: Public, Static
00964 //  Description: Tells the BamReader how to create objects of type
00965 //               CollisionTube.
00966 ////////////////////////////////////////////////////////////////////
00967 void CollisionTube::
00968 register_with_read_factory() {
00969   BamReader::get_factory()->register_factory(get_class_type(), make_from_bam);
00970 }
00971 
00972 ////////////////////////////////////////////////////////////////////
00973 //     Function: CollisionTube::write_datagram
00974 //       Access: Public, Virtual
00975 //  Description: Writes the contents of this object to the datagram
00976 //               for shipping out to a Bam file.
00977 ////////////////////////////////////////////////////////////////////
00978 void CollisionTube::
00979 write_datagram(BamWriter *manager, Datagram &dg) {
00980   CollisionSolid::write_datagram(manager, dg);
00981   _a.write_datagram(dg);
00982   _b.write_datagram(dg);
00983   dg.add_float32(_radius);
00984 }
00985 
00986 ////////////////////////////////////////////////////////////////////
00987 //     Function: CollisionTube::make_from_bam
00988 //       Access: Protected, Static
00989 //  Description: This function is called by the BamReader's factory
00990 //               when a new object of type CollisionTube is encountered
00991 //               in the Bam file.  It should create the CollisionTube
00992 //               and extract its information from the file.
00993 ////////////////////////////////////////////////////////////////////
00994 TypedWritable *CollisionTube::
00995 make_from_bam(const FactoryParams &params) {
00996   CollisionTube *node = new CollisionTube();
00997   DatagramIterator scan;
00998   BamReader *manager;
00999 
01000   parse_params(params, scan, manager);
01001   node->fillin(scan, manager);
01002 
01003   return node;
01004 }
01005 
01006 ////////////////////////////////////////////////////////////////////
01007 //     Function: CollisionTube::fillin
01008 //       Access: Protected
01009 //  Description: This internal function is called by make_from_bam to
01010 //               read in all of the relevant data from the BamFile for
01011 //               the new CollisionTube.
01012 ////////////////////////////////////////////////////////////////////
01013 void CollisionTube::
01014 fillin(DatagramIterator &scan, BamReader *manager) {
01015   CollisionSolid::fillin(scan, manager);
01016   _a.read_datagram(scan);
01017   _b.read_datagram(scan);
01018   _radius = scan.get_float32();
01019   recalc_internals();
01020 }
 All Classes Functions Variables Enumerations