introduce CollisionTube

panda/src/collide/Sources.pp

@@ -24,9 +24,10 @@
     collisionRay.h \
     collisionRecorder.I collisionRecorder.h \
     collisionSegment.I collisionSegment.h  \
-    collisionSolid.I collisionSolid.h collisionSphere.I  \
+    collisionSolid.I collisionSolid.h \
-    collisionSphere.h \
+    collisionSphere.I collisionSphere.h \
     collisionTraverser.I collisionTraverser.h  \
+    collisionTube.I collisionTube.h \
     collisionVisualizer.I collisionVisualizer.h \
     config_collide.h
     
@@ -44,8 +45,10 @@
     collisionPolygon.cxx collisionRay.cxx \
     collisionRecorder.cxx \
     collisionSegment.cxx  \
-    collisionSolid.cxx collisionSphere.cxx  \
+    collisionSolid.cxx \
+    collisionSphere.cxx  \
     collisionTraverser.cxx \
+    collisionTube.cxx  \
     collisionVisualizer.cxx \
     config_collide.cxx 
 
@@ -63,9 +66,10 @@
     collisionPolygon.I collisionPolygon.h collisionRay.I collisionRay.h \
     collisionRecorder.I collisionRecorder.h \
     collisionSegment.I collisionSegment.h \
-    collisionSolid.I collisionSolid.h collisionSphere.I \
+    collisionSolid.I collisionSolid.h \
-    collisionSphere.h \
+    collisionSphere.I collisionSphere.h \
     collisionTraverser.I collisionTraverser.h \
+    collisionTube.I collisionTube.h \
     collisionVisualizer.I collisionVisualizer.h
 
   #define IGATESCAN all

panda/src/collide/collide_composite2.cxx

@@ -6,4 +6,5 @@
 #include "collisionSolid.cxx"
 #include "collisionSphere.cxx"
 #include "collisionTraverser.cxx"
+#include "collisionTube.cxx"
 #include "collisionVisualizer.cxx"

panda/src/collide/collisionPlane.I

@@ -18,7 +18,7 @@
 
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionPlane::Default Constructor
-//       Access: Public
+//       Access: Protected
 //  Description: This is only for the convenience of CollisionPolygon.
 //               Normally, you should not attempt to create an
 //               uninitialized CollisionPlane.

panda/src/collide/collisionPolygon.I

@@ -60,11 +60,12 @@ CollisionPolygon(const LPoint3f *begin, const LPoint3f *end) {
 
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionPolygon::Constructor
-//       Access: Public
+//       Access: Private
-//  Description:
+//  Description: Creates an invalid polygon.  Only used when reading
+//               from a bam file.
 ////////////////////////////////////////////////////////////////////
 INLINE CollisionPolygon::
-CollisionPolygon(void) {
+CollisionPolygon() {
 }
 
 ////////////////////////////////////////////////////////////////////

panda/src/collide/collisionPolygon.h

@@ -19,11 +19,11 @@
 #ifndef COLLISIONPOLYGON_H
 #define COLLISIONPOLYGON_H
 
-#include <pandabase.h>
+#include "pandabase.h"
 
 #include "collisionPlane.h"
 
-#include <vector_LPoint2f.h>
+#include "vector_LPoint2f.h"
 
 ///////////////////////////////////////////////////////////////////
 //       Class : CollisionPolygon
@@ -37,6 +37,9 @@ PUBLISHED:
                           const LPoint3f &c, const LPoint3f &d);
   INLINE CollisionPolygon(const LPoint3f *begin, const LPoint3f *end);
 
+private:
+  INLINE CollisionPolygon();
+
 public:
   CollisionPolygon(const CollisionPolygon &copy);
 
@@ -56,10 +59,8 @@ public:
   virtual void write(ostream &out, int indent_level = 0) const;
 
 protected:
-  INLINE CollisionPolygon(void);
   virtual BoundingVolume *recompute_bound();
 
-protected:
   virtual PT(CollisionEntry)
   test_intersection_from_sphere(const CollisionEntry &entry) const;
   virtual PT(CollisionEntry)

panda/src/collide/collisionSphere.I

@@ -40,9 +40,10 @@ CollisionSphere(float cx, float cy, float cz, float radius) :
 }
 
 ////////////////////////////////////////////////////////////////////
-//     Function: CollisionSphere::Constructor
+//     Function: CollisionSphere::Default constructor
-//       Access: Public
+//       Access: Private
-//  Description:
+//  Description: Creates an invalid sphere.  Only used when reading
+//               from a bam file.
 ////////////////////////////////////////////////////////////////////
 INLINE CollisionSphere::
 CollisionSphere() {

panda/src/collide/collisionSphere.cxx

@@ -64,7 +64,7 @@ xform(const LMatrix4f &mat) {
   _center = _center * mat;
 
   // This is a little cheesy and fails miserably in the presence of a
-  // non-proportionate scale.
+  // non-uniform scale.
   LVector3f radius_v = LVector3f(_radius, 0.0f, 0.0f) * mat;
   _radius = length(radius_v);
 
@@ -200,8 +200,12 @@ test_intersection_from_ray(const CollisionEntry &entry) const {
 
   LPoint3f into_intersection_point;
   if (t1 < 0.0) {
-    into_intersection_point = from_origin + t2 * from_direction;
+    // Point a is within the sphere.  The first intersection point is
+    // point a itself.
+    into_intersection_point = from_origin;
   } else {
+    // Point a is outside the sphere.  The first intersection point is
+    // at t1.
     into_intersection_point = from_origin + t1 * from_direction;
   }
   new_entry->set_into_intersection_point(into_intersection_point);
@@ -282,8 +286,8 @@ fill_viz_geom() {
 
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionSphere::intersects_line
-//       Access: Protected
+//       Access: Private
-//  Description: Determine the point(s) of intersect of a parametric
+//  Description: Determine the point(s) of intersection of a parametric
 //               line with the sphere.  The line is infinite in both
 //               directions, and passes through "from" and from+delta.
 //               If the line does not intersect the sphere, the

panda/src/collide/collisionSphere.h

@@ -19,7 +19,7 @@
 #ifndef COLLISIONSPHERE_H
 #define COLLISIONSPHERE_H
 
-#include <pandabase.h>
+#include "pandabase.h"
 
 #include "collisionSolid.h"
 
@@ -32,6 +32,9 @@ PUBLISHED:
   INLINE CollisionSphere(const LPoint3f &center, float radius);
   INLINE CollisionSphere(float cx, float cy, float cz, float radius);
 
+private:
+  INLINE CollisionSphere();
+
 public:
   INLINE CollisionSphere(const CollisionSphere &copy);
   virtual CollisionSolid *make_copy();
@@ -53,10 +56,8 @@ PUBLISHED:
   INLINE float get_radius() const;
 
 protected:
-  INLINE CollisionSphere(void);
   virtual BoundingVolume *recompute_bound();
 
-protected:
   virtual PT(CollisionEntry)
   test_intersection_from_sphere(const CollisionEntry &entry) const;
   virtual PT(CollisionEntry)
@@ -66,6 +67,7 @@ protected:
 
   virtual void fill_viz_geom();
 
+private:
   bool intersects_line(double &t1, double &t2,
                        const LPoint3f &from, const LVector3f &delta) const;
 

panda/src/collide/collisionTube.I

@@ -0,0 +1,157 @@
+// Filename: collisionTube.I
+// Created by:  drose (25Sep03)
+//
+////////////////////////////////////////////////////////////////////
+//
+// PANDA 3D SOFTWARE
+// Copyright (c) 2001, Disney Enterprises, Inc.  All rights reserved
+//
+// All use of this software is subject to the terms of the Panda 3d
+// Software license.  You should have received a copy of this license
+// along with this source code; you will also find a current copy of
+// the license at http://www.panda3d.org/license.txt .
+//
+// To contact the maintainers of this program write to
+// panda3d@yahoogroups.com .
+//
+////////////////////////////////////////////////////////////////////
+
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::Constructor
+//       Access: Public
+//  Description:
+////////////////////////////////////////////////////////////////////
+INLINE CollisionTube::
+CollisionTube(const LPoint3f &a, const LPoint3f &b, float radius) :
+  _a(a), _b(b), _radius(radius)
+{
+  recalc_internals();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::Constructor
+//       Access: Public
+//  Description:
+////////////////////////////////////////////////////////////////////
+INLINE CollisionTube::
+CollisionTube(float ax, float ay, float az,
+              float bx, float by, float bz,
+              float radius) :
+  _a(ax, ay, az), _b(bx, by, bz), _radius(radius)
+{
+  recalc_internals();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::Default constructor
+//       Access: Private
+//  Description: Creates an invalid tube.  Only used when reading
+//               from a bam file.
+////////////////////////////////////////////////////////////////////
+INLINE CollisionTube::
+CollisionTube() {
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::Copy Constructor
+//       Access: Public
+//  Description:
+////////////////////////////////////////////////////////////////////
+INLINE CollisionTube::
+CollisionTube(const CollisionTube &copy) :
+  CollisionSolid(copy),
+  _a(copy._a),
+  _b(copy._b),
+  _radius(copy._radius)
+{
+  recalc_internals();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::set_point_a
+//       Access: Public
+//  Description:
+////////////////////////////////////////////////////////////////////
+INLINE void CollisionTube::
+set_point_a(const LPoint3f &a) {
+  _a = a;
+  recalc_internals();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::set_point_a
+//       Access: Public
+//  Description:
+////////////////////////////////////////////////////////////////////
+INLINE void CollisionTube::
+set_point_a(float x, float y, float z) {
+  set_point_a(LPoint3f(x, y, z));
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::get_point_a
+//       Access: Public
+//  Description:
+////////////////////////////////////////////////////////////////////
+INLINE const LPoint3f &CollisionTube::
+get_point_a() const {
+  return _a;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::set_point_b
+//       Access: Public
+//  Description:
+////////////////////////////////////////////////////////////////////
+INLINE void CollisionTube::
+set_point_b(const LPoint3f &b) {
+  _b = b;
+  recalc_internals();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::set_point_b
+//       Access: Public
+//  Description:
+////////////////////////////////////////////////////////////////////
+INLINE void CollisionTube::
+set_point_b(float x, float y, float z) {
+  set_point_b(LPoint3f(x, y, z));
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::get_point_b
+//       Access: Public
+//  Description:
+////////////////////////////////////////////////////////////////////
+INLINE const LPoint3f &CollisionTube::
+get_point_b() const {
+  return _b;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::set_radius
+//       Access: Public
+//  Description:
+////////////////////////////////////////////////////////////////////
+INLINE void CollisionTube::
+set_radius(float radius) {
+  _radius = radius;
+
+  // We don't need to call recalc_internals(), since the radius
+  // doesn't change either of those properties.
+  mark_bound_stale();
+  mark_viz_stale();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::get_radius
+//       Access: Public
+//  Description:
+////////////////////////////////////////////////////////////////////
+INLINE float CollisionTube::
+get_radius() const {
+  return _radius;
+}
+

panda/src/collide/collisionTube.cxx

@@ -0,0 +1,691 @@
+// Filename: collisionTube.cxx
+// Created by:  drose (25Sep03)
+//
+////////////////////////////////////////////////////////////////////
+//
+// PANDA 3D SOFTWARE
+// Copyright (c) 2001, Disney Enterprises, Inc.  All rights reserved
+//
+// All use of this software is subject to the terms of the Panda 3d
+// Software license.  You should have received a copy of this license
+// along with this source code; you will also find a current copy of
+// the license at http://www.panda3d.org/license.txt .
+//
+// To contact the maintainers of this program write to
+// panda3d@yahoogroups.com .
+//
+////////////////////////////////////////////////////////////////////
+
+#include "collisionTube.h"
+#include "collisionHandler.h"
+#include "collisionEntry.h"
+#include "config_collide.h"
+
+#include "look_at.h"
+#include "geom.h"
+#include "geomNode.h"
+#include "geomTristrip.h"
+#include "geometricBoundingVolume.h"
+#include "datagram.h"
+#include "datagramIterator.h"
+#include "bamReader.h"
+#include "bamWriter.h"
+
+TypeHandle CollisionTube::_type_handle;
+
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::make_copy
+//       Access: Public, Virtual
+//  Description:
+////////////////////////////////////////////////////////////////////
+CollisionSolid *CollisionTube::
+make_copy() {
+  return new CollisionTube(*this);
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::xform
+//       Access: Public, Virtual
+//  Description: Transforms the solid by the indicated matrix.
+////////////////////////////////////////////////////////////////////
+void CollisionTube::
+xform(const LMatrix4f &mat) {
+  _a = _a * mat;
+  _b = _b * mat;
+
+  // This is a little cheesy and fails miserably in the presence of a
+  // non-uniform scale.
+  LVector3f radius_v = LVector3f(_radius, 0.0f, 0.0f) * mat;
+  _radius = length(radius_v);
+
+  recalc_internals();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::get_collision_origin
+//       Access: Public, Virtual
+//  Description: Returns the point in space deemed to be the "origin"
+//               of the solid for collision purposes.  The closest
+//               intersection point to this origin point is considered
+//               to be the most significant.
+////////////////////////////////////////////////////////////////////
+LPoint3f CollisionTube::
+get_collision_origin() const {
+  return get_point_a();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::output
+//       Access: Public, Virtual
+//  Description:
+////////////////////////////////////////////////////////////////////
+void CollisionTube::
+output(ostream &out) const {
+  out << "tube, a (" << _a << "), b (" << _b << "), r " << _radius;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::recompute_bound
+//       Access: Protected, Virtual
+//  Description:
+////////////////////////////////////////////////////////////////////
+BoundingVolume *CollisionTube::
+recompute_bound() {
+  BoundingVolume *bound = BoundedObject::recompute_bound();
+
+  if (bound->is_of_type(GeometricBoundingVolume::get_class_type())) {
+    GeometricBoundingVolume *gbound;
+    DCAST_INTO_R(gbound, bound, bound);
+
+    LVector3f vec = (_b - _a);
+    if (vec.normalize()) {
+      // The bounding volume includes both endpoints, plus a little
+      // bit more to include the radius in both directions.
+      LPoint3f points[2];
+      points[0] = _a - vec * _radius;
+      points[1] = _b + vec * _radius;
+
+      gbound->around(points, points + 2);
+
+    } else {
+      // Both endpoints are coincident; therefore, the bounding volume
+      // is a sphere.
+      BoundingSphere sphere(_a, _radius);
+      gbound->extend_by(&sphere);
+    }
+  }
+
+  return bound;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::test_intersection_from_sphere
+//       Access: Public, Virtual
+//  Description:
+////////////////////////////////////////////////////////////////////
+PT(CollisionEntry) CollisionTube::
+test_intersection_from_sphere(const CollisionEntry &entry) const {
+  const CollisionSphere *sphere;
+  DCAST_INTO_R(sphere, entry.get_from(), 0);
+
+  LPoint3f from_a = sphere->get_center() * entry.get_wrt_space();
+  LPoint3f from_b = from_a;
+
+  if (entry.has_from_velocity()) {
+    from_a = (sphere->get_center() - entry.get_from_velocity()) * entry.get_wrt_space();
+  }
+
+  LVector3f from_direction = from_b - from_a;
+
+  LVector3f from_radius_v =
+    LVector3f(sphere->get_radius(), 0.0f, 0.0f) * entry.get_wrt_space();
+  float from_radius = length(from_radius_v);
+
+  double t1, t2;
+  if (!intersects_line(t1, t2, from_a, from_direction, from_radius)) {
+    // No intersection.
+    return NULL;
+  }
+
+  if (t2 < 0.0 || t1 > 1.0) {
+    // Both intersection points are before the start of the segment or
+    // after the end of the segment.
+    return NULL;
+  }
+
+  if (collide_cat.is_debug()) {
+    collide_cat.debug()
+      << "intersection detected from " << *entry.get_from_node() << " into "
+      << entry.get_into_node_path() << "\n";
+  }
+  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
+
+  LPoint3f into_intersection_point;
+  if (t1 < 0.0) {
+    // Point a is within the tube.  The first intersection point is
+    // point a itself.
+    into_intersection_point = from_a;
+  } else {
+    // Point a is outside the tube, and point b is either inside the
+    // tube or beyond it.  The first intersection point is at t1.
+    into_intersection_point = from_a + t1 * from_direction;
+  }
+  new_entry->set_into_intersection_point(into_intersection_point);
+
+  return new_entry;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::test_intersection_from_ray
+//       Access: Public, Virtual
+//  Description:
+////////////////////////////////////////////////////////////////////
+PT(CollisionEntry) CollisionTube::
+test_intersection_from_ray(const CollisionEntry &entry) const {
+  const CollisionRay *ray;
+  DCAST_INTO_R(ray, entry.get_from(), 0);
+
+  LPoint3f from_origin = ray->get_origin() * entry.get_wrt_space();
+  LVector3f from_direction = ray->get_direction() * entry.get_wrt_space();
+
+  double t1, t2;
+  if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
+    // No intersection.
+    return NULL;
+  }
+
+  if (t2 < 0.0) {
+    // Both intersection points are before the start of the ray.
+    return NULL;
+  }
+
+  if (collide_cat.is_debug()) {
+    collide_cat.debug()
+      << "intersection detected from " << *entry.get_from_node() << " into "
+      << entry.get_into_node_path() << "\n";
+  }
+  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
+
+  LPoint3f into_intersection_point;
+  if (t1 < 0.0) {
+    // Point a is within the tube.  The first intersection point is
+    // point a itself.
+    into_intersection_point = from_origin;
+  } else {
+    // Point a is outside the tube.  The first intersection point is
+    // at t1.
+    into_intersection_point = from_origin + t1 * from_direction;
+  }
+  new_entry->set_into_intersection_point(into_intersection_point);
+
+  return new_entry;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::test_intersection_from_segment
+//       Access: Public, Virtual
+//  Description:
+////////////////////////////////////////////////////////////////////
+PT(CollisionEntry) CollisionTube::
+test_intersection_from_segment(const CollisionEntry &entry) const {
+  const CollisionSegment *segment;
+  DCAST_INTO_R(segment, entry.get_from(), 0);
+
+  LPoint3f from_a = segment->get_point_a() * entry.get_wrt_space();
+  LPoint3f from_b = segment->get_point_b() * entry.get_wrt_space();
+  LVector3f from_direction = from_b - from_a;
+
+  double t1, t2;
+  if (!intersects_line(t1, t2, from_a, from_direction, 0.0f)) {
+    // No intersection.
+    return NULL;
+  }
+
+  if (t2 < 0.0 || t1 > 1.0) {
+    // Both intersection points are before the start of the segment or
+    // after the end of the segment.
+    return NULL;
+  }
+
+  if (collide_cat.is_debug()) {
+    collide_cat.debug()
+      << "intersection detected from " << *entry.get_from_node() << " into "
+      << entry.get_into_node_path() << "\n";
+  }
+  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
+
+  LPoint3f into_intersection_point;
+  if (t1 < 0.0) {
+    // Point a is within the tube.  The first intersection point is
+    // point a itself.
+    into_intersection_point = from_a;
+  } else {
+    // Point a is outside the tube, and point b is either inside the
+    // tube or beyond it.  The first intersection point is at t1.
+    into_intersection_point = from_a + t1 * from_direction;
+  }
+  new_entry->set_into_intersection_point(into_intersection_point);
+
+  return new_entry;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::fill_viz_geom
+//       Access: Protected, Virtual
+//  Description: Fills the _viz_geom GeomNode up with Geoms suitable
+//               for rendering this solid.
+////////////////////////////////////////////////////////////////////
+void CollisionTube::
+fill_viz_geom() {
+  if (collide_cat.is_debug()) {
+    collide_cat.debug()
+      << "Recomputing viz for " << *this << "\n";
+  }
+
+  // Generate the vertices such that we draw a tube with one endpoint
+  // at (0, 0, 0), and another at (0, length, 0).  Then we'll rotate
+  // and translate it into place with the appropriate look_at matrix.
+  LVector3f direction = (_b - _a);
+  float length = direction.length();
+
+  PTA_Vertexf verts;
+  PTA_int lengths;
+
+  // Generate the first endcap.
+  static const int num_slices = 8;
+  static const int num_rings = 4;
+  int ri, si;
+  for (ri = 0; ri < num_rings; ri++) {
+    for (si = 0; si <= num_slices; si++) {
+      verts.push_back(calc_sphere1_vertex(ri, si, num_rings, num_slices));
+      verts.push_back(calc_sphere1_vertex(ri + 1, si, num_rings, num_slices));
+    }
+    lengths.push_back((num_slices + 1) * 2);
+  }
+
+  // Now the cylinder sides.
+  for (si = 0; si <= num_slices; si++) {
+    verts.push_back(calc_sphere1_vertex(num_rings, si, num_rings, num_slices));
+    verts.push_back(calc_sphere2_vertex(num_rings, si, num_rings, num_slices,
+                                        length));
+  }
+  lengths.push_back((num_slices + 1) * 2);
+
+  // And the second endcap.
+  for (ri = num_rings - 1; ri >= 0; ri--) {
+    for (si = 0; si <= num_slices; si++) {
+      verts.push_back(calc_sphere2_vertex(ri + 1, si, num_rings, num_slices, length));
+      verts.push_back(calc_sphere2_vertex(ri, si, num_rings, num_slices, length));
+    }
+    lengths.push_back((num_slices + 1) * 2);
+  }
+
+  // Now transform the vertices to their actual location.
+  LMatrix4f mat;
+  look_at(mat, direction, LVector3f(0.0f, 0.0f, 1.0f), CS_zup_right);
+  mat.set_row(3, _a);
+
+  for (size_t i = 0; i < verts.size(); i++) {
+    verts[i] = verts[i] * mat;
+  }
+
+  GeomTristrip *tube = new GeomTristrip;
+  tube->set_coords(verts);
+  tube->set_num_prims(lengths.size());
+  tube->set_lengths(lengths);
+
+  _viz_geom->add_geom(tube, get_solid_viz_state());
+  _viz_geom->add_geom(tube, get_wireframe_viz_state());
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::recalc_internals
+//       Access: Private
+//  Description: Should be called internally to recompute the matrix
+//               and length when the properties of the tube have
+//               changed.
+////////////////////////////////////////////////////////////////////
+void CollisionTube::
+recalc_internals() {
+  LVector3f direction = (_b - _a);
+  _length = direction.length();
+
+  LMatrix4f mat;
+  look_at(mat, direction, LVector3f(0.0f, 0.0f, 1.0f), CS_zup_right);
+  mat.set_row(3, _a);
+  _inv_mat.invert_from(mat);
+
+  mark_viz_stale();
+  mark_bound_stale();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::calc_sphere1_vertex
+//       Access: Private
+//  Description: Calculates a particular vertex on the surface of the
+//               first endcap hemisphere, for use in generating the
+//               viz geometry.
+////////////////////////////////////////////////////////////////////
+Vertexf CollisionTube::
+calc_sphere1_vertex(int ri, int si, int num_rings, int num_slices) {
+  float r = (float)ri / (float)num_rings;
+  float s = (float)si / (float)num_slices;
+
+  // Find the point on the rim, based on the slice.
+  float theta = s * 2.0f * MathNumbers::pi_f;
+  float x_rim = cos(theta);
+  float z_rim = sin(theta);
+
+  // Now pull that point in towards the pole, based on the ring.
+  float phi = r * 0.5f * MathNumbers::pi_f;
+  float to_pole = sin(phi);
+
+  float x = _radius * x_rim * to_pole;
+  float y = -_radius * cos(phi);
+  float z = _radius * z_rim * to_pole;
+
+  return Vertexf(x, y, z);
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::calc_sphere2_vertex
+//       Access: Private
+//  Description: Calculates a particular vertex on the surface of the
+//               second endcap hemisphere, for use in generating the
+//               viz geometry.
+////////////////////////////////////////////////////////////////////
+Vertexf CollisionTube::
+calc_sphere2_vertex(int ri, int si, int num_rings, int num_slices,
+                    float length) {
+  float r = (float)ri / (float)num_rings;
+  float s = (float)si / (float)num_slices;
+
+  // Find the point on the rim, based on the slice.
+  float theta = s * 2.0f * MathNumbers::pi_f;
+  float x_rim = cos(theta);
+  float z_rim = sin(theta);
+
+  // Now pull that point in towards the pole, based on the ring.
+  float phi = r * 0.5f * MathNumbers::pi_f;
+  float to_pole = sin(phi);
+
+  float x = _radius * x_rim * to_pole;
+  float y = length + _radius * cos(phi);
+  float z = _radius * z_rim * to_pole;
+
+  return Vertexf(x, y, z);
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::intersects_line
+//       Access: Private
+//  Description: Determine the point(s) of intersection of a parametric
+//               line with the tube.  The line is infinite in both
+//               directions, and passes through "from" and from+delta.
+//               If the line does not intersect the tube, the
+//               function returns false, and t1 and t2 are undefined.
+//               If it does intersect the tube, it returns true, and
+//               t1 and t2 are set to the points along the equation
+//               from+t*delta that correspond to the two points of
+//               intersection.
+////////////////////////////////////////////////////////////////////
+bool CollisionTube::
+intersects_line(double &t1, double &t2,
+                LPoint3f from, LVector3f delta, float inflate_radius) const {
+  // Convert the line into our canonical coordinate space: the tube is
+  // aligned with the y axis.
+  from = from * _inv_mat;
+  delta = delta * _inv_mat;
+
+  float radius = _radius + inflate_radius;
+
+  // Now project the line into the X-Z plane to test for intersection
+  // with a 2-d circle around the origin.  The equation for this is
+  // very similar to the formula for the intersection of a line with a
+  // sphere; see CollisionSphere::intersects_line() for the complete
+  // derivation.  It's a little bit simpler because the circle is
+  // centered on the origin.
+  LVector2f from2(from[0], from[2]);
+  LVector2f delta2(delta[0], delta[2]);
+
+  double A = dot(delta2, delta2);
+
+  if (IS_NEARLY_ZERO(A)) {
+    // If the delta2 is 0, the line is perpendicular to the X-Z plane.
+    // The whole line intersects with the infinite cylinder if the
+    // point is within the circle.
+    if (from2.dot(from2) > radius * radius) {
+      // Nope, the 2-d point is outside the circle, so no
+      // intersection.
+      return false;
+    }
+
+    if (IS_NEARLY_ZERO(delta[1])) {
+      // Actually, the whole delta vector is 0, so the line is just a
+      // point.  In this case, (since we have already shown the point
+      // is within the infinite cylinder), we intersect if and only if
+      // the three-dimensional point is between the endcaps.
+      if (from[1] < -radius || from[1] > _length + radius) {
+        // Way out.
+        return false;
+      }
+      if (from[1] < 0.0f) {
+        // Possibly within the first endcap.
+        if (from.dot(from) > radius * radius) {
+          return false;
+        }
+      } else if (from[1] > _length) {
+        // Possibly within the second endcap.
+        from[1] -= _length;
+        if (from.dot(from) > radius * radius) {
+          return false;
+        }
+      }
+
+      // The point is within the tube!
+      t1 = t2 = 0.0;
+      return true;
+    }
+
+    // The 2-d point is within the circle, so compute our intersection
+    // points to include the entire vertical slice of the cylinder.
+    t1 = (-radius - from[1]) / delta[1];
+    t2 = (_length + radius - from[1]) / delta[1];
+
+  } else {
+    // The line is not perpendicular to the X-Z plane, so its
+    // projection into the plane is 2-d line.  Test that 2-d line for
+    // intersection with the circular projection of the cylinder.
+
+    double B = 2.0f * dot(delta2, from2);
+    double fc_d2 = dot(from2, from2);
+    double C = fc_d2 - radius * radius;
+    
+    double radical = B*B - 4.0*A*C;
+    
+    if (IS_NEARLY_ZERO(radical)) {
+      // Tangent.
+      t1 = t2 = -B / (2.0*A);
+      
+    } else if (radical < 0.0) {
+      // No real roots: no intersection with the line.
+      return false;
+      
+    } else {
+      double reciprocal_2A = 1.0 / (2.0 * A);
+      double sqrt_radical = sqrtf(radical);
+      t1 = ( -B - sqrt_radical ) * reciprocal_2A;
+      t2 = ( -B + sqrt_radical ) * reciprocal_2A;
+    }
+  }
+
+  // Now we need to verify that the intersection points fall within
+  // the length of the cylinder.
+  float t1_y = from[1] + t1 * delta[1];
+  float t2_y = from[1] + t2 * delta[1];
+
+  if (t1_y < -radius && t2_y < -radius) {
+    // Both points are way off the bottom of the tube; no
+    // intersection.
+    return false;
+  } else if (t1_y > _length + radius && t2_y > _length + radius) {
+    // Both points are way off the top of the tube; no intersection.
+    return false;
+  }
+
+  if (t1_y < 0.0f) {
+    // The starting point is off the bottom of the tube.  Test the
+    // line against the first endcap.
+    double t1a, t2a;
+    if (!sphere_intersects_line(t1a, t2a, 0.0f, from, delta, inflate_radius)) {
+      // If there's no intersection with the endcap, there can't be an
+      // intersection with the cylinder.
+      return false;
+    }
+    t1 = t1a;
+
+  } else if (t1_y > _length) {
+    // The starting point is off the top of the tube.  Test the
+    // line against the second endcap.
+    double t1b, t2b;
+    if (!sphere_intersects_line(t1b, t2b, _length, from, delta, inflate_radius)) {
+      // If there's no intersection with the endcap, there can't be an
+      // intersection with the cylinder.
+      return false;
+    }
+    t1 = t1b;
+  }
+
+  if (t2_y < 0.0f) {
+    // The ending point is off the bottom of the tube.  Test the
+    // line against the first endcap.
+    double t1a, t2a;
+    if (!sphere_intersects_line(t1a, t2a, 0.0f, from, delta, inflate_radius)) {
+      // If there's no intersection with the endcap, there can't be an
+      // intersection with the cylinder.
+      return false;
+    }
+    t2 = t2a;
+
+  } else if (t2_y > _length) {
+    // The ending point is off the top of the tube.  Test the
+    // line against the second endcap.
+    double t1b, t2b;
+    if (!sphere_intersects_line(t1b, t2b, _length, from, delta, inflate_radius)) {
+      // If there's no intersection with the endcap, there can't be an
+      // intersection with the cylinder.
+      return false;
+    }
+    t2 = t2b;
+  }
+
+  return true;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::sphere_intersects_line
+//       Access: Private
+//  Description: After confirming that the line intersects an infinite
+//               cylinder, test whether it intersects one or the other
+//               endcaps.  The y parameter specifies the center of the
+//               sphere (and hence the particular endcap.
+////////////////////////////////////////////////////////////////////
+bool CollisionTube::
+sphere_intersects_line(double &t1, double &t2, float center_y,
+                       const LPoint3f &from, const LVector3f &delta,
+                       float inflate_radius) const {
+  // See CollisionSphere::intersects_line() for a derivation of the
+  // formula here.
+  float radius = _radius + inflate_radius;
+
+  double A = dot(delta, delta);
+
+  nassertr(A != 0.0, false);
+
+  LVector3f fc = from;
+  fc[1] -= center_y;
+  double B = 2.0f* dot(delta, fc);
+  double fc_d2 = dot(fc, fc);
+  double C = fc_d2 - radius * radius;
+
+  double radical = B*B - 4.0*A*C;
+
+  if (IS_NEARLY_ZERO(radical)) {
+    // Tangent.
+    t1 = t2 = -B / (2.0 * A);
+    return true;
+
+  } else if (radical < 0.0) {
+    // No real roots: no intersection with the line.
+    return false;
+  }
+
+  double reciprocal_2A = 1.0 / (2.0 * A);
+  double sqrt_radical = sqrtf(radical);
+  t1 = ( -B - sqrt_radical ) * reciprocal_2A;
+  t2 = ( -B + sqrt_radical ) * reciprocal_2A;
+
+  return true;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::register_with_read_factory
+//       Access: Public, Static
+//  Description: Tells the BamReader how to create objects of type
+//               CollisionTube.
+////////////////////////////////////////////////////////////////////
+void CollisionTube::
+register_with_read_factory() {
+  BamReader::get_factory()->register_factory(get_class_type(), make_from_bam);
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::write_datagram
+//       Access: Public, Virtual
+//  Description: Writes the contents of this object to the datagram
+//               for shipping out to a Bam file.
+////////////////////////////////////////////////////////////////////
+void CollisionTube::
+write_datagram(BamWriter *manager, Datagram &dg) {
+  CollisionSolid::write_datagram(manager, dg);
+  _a.write_datagram(dg);
+  _b.write_datagram(dg);
+  dg.add_float32(_radius);
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::make_from_bam
+//       Access: Protected, Static
+//  Description: This function is called by the BamReader's factory
+//               when a new object of type CollisionTube is encountered
+//               in the Bam file.  It should create the CollisionTube
+//               and extract its information from the file.
+////////////////////////////////////////////////////////////////////
+TypedWritable *CollisionTube::
+make_from_bam(const FactoryParams &params) {
+  CollisionTube *node = new CollisionTube();
+  DatagramIterator scan;
+  BamReader *manager;
+
+  parse_params(params, scan, manager);
+  node->fillin(scan, manager);
+
+  return node;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionTube::fillin
+//       Access: Protected
+//  Description: This internal function is called by make_from_bam to
+//               read in all of the relevant data from the BamFile for
+//               the new CollisionTube.
+////////////////////////////////////////////////////////////////////
+void CollisionTube::
+fillin(DatagramIterator &scan, BamReader *manager) {
+  CollisionSolid::fillin(scan, manager);
+  _a.read_datagram(scan);
+  _b.read_datagram(scan);
+  _radius = scan.get_float32();
+  recalc_internals();
+}

panda/src/collide/collisionTube.h

@@ -0,0 +1,131 @@
+// Filename: collisionTube.h
+// Created by:  drose (25Sep03)
+//
+////////////////////////////////////////////////////////////////////
+//
+// PANDA 3D SOFTWARE
+// Copyright (c) 2001, Disney Enterprises, Inc.  All rights reserved
+//
+// All use of this software is subject to the terms of the Panda 3d
+// Software license.  You should have received a copy of this license
+// along with this source code; you will also find a current copy of
+// the license at http://www.panda3d.org/license.txt .
+//
+// To contact the maintainers of this program write to
+// panda3d@yahoogroups.com .
+//
+////////////////////////////////////////////////////////////////////
+
+#ifndef COLLISIONTUBE_H
+#define COLLISIONTUBE_H
+
+#include "pandabase.h"
+
+#include "collisionSolid.h"
+
+///////////////////////////////////////////////////////////////////
+//       Class : CollisionTube
+// Description : This implements a solid roughly in cylindrical shape.
+//               It's not called a CollisionCylinder because it's not
+//               a true cylinder; specifically, it has rounded ends
+//               instead of flat ends.  It looks more like a Contac
+//               pill.
+////////////////////////////////////////////////////////////////////
+class EXPCL_PANDA CollisionTube : public CollisionSolid {
+PUBLISHED:
+  INLINE CollisionTube(const LPoint3f &a, const LPoint3f &db,
+                       float radius);
+  INLINE CollisionTube(float ax, float ay, float az,
+                       float bx, float by, float bz,
+                       float radius);
+
+private:
+  INLINE CollisionTube();
+
+public:
+  INLINE CollisionTube(const CollisionTube &copy);
+  virtual CollisionSolid *make_copy();
+
+  virtual void xform(const LMatrix4f &mat);
+  virtual LPoint3f get_collision_origin() const;
+
+  virtual void output(ostream &out) const;
+
+PUBLISHED:
+  INLINE void set_point_a(const LPoint3f &a);
+  INLINE void set_point_a(float x, float y, float z);
+  INLINE const LPoint3f &get_point_a() const;
+
+  INLINE void set_point_b(const LPoint3f &b);
+  INLINE void set_point_b(float x, float y, float z);
+  INLINE const LPoint3f &get_point_b() const;
+
+  INLINE void set_radius(float radius);
+  INLINE float get_radius() const;
+
+protected:
+  virtual BoundingVolume *recompute_bound();
+
+protected:
+  virtual PT(CollisionEntry)
+  test_intersection_from_sphere(const CollisionEntry &entry) const;
+  virtual PT(CollisionEntry)
+  test_intersection_from_ray(const CollisionEntry &entry) const;
+  virtual PT(CollisionEntry)
+  test_intersection_from_segment(const CollisionEntry &entry) const;
+
+  virtual void fill_viz_geom();
+
+private:
+  void recalc_internals();
+
+  Vertexf calc_sphere1_vertex(int ri, int si, int num_rings, int num_slices);
+  Vertexf calc_sphere2_vertex(int ri, int si, int num_rings, int num_slices,
+                              float length);
+
+  bool intersects_line(double &t1, double &t2,
+                       LPoint3f from, LVector3f delta,
+                       float inflate_radius) const;
+  bool sphere_intersects_line(double &t1, double &t2, float center_y,
+                              const LPoint3f &from, const LVector3f &delta,
+                              float inflate_radius) const;
+
+private:
+  LPoint3f _a, _b;
+  float _radius;
+
+  // These are derived from the above.
+  LMatrix4f _inv_mat;
+  float _length;
+
+public:
+  static void register_with_read_factory();
+  virtual void write_datagram(BamWriter *manager, Datagram &dg);
+
+protected:
+  static TypedWritable *make_from_bam(const FactoryParams &params);
+  void fillin(DatagramIterator &scan, BamReader *manager);
+
+public:
+  static TypeHandle get_class_type() {
+    return _type_handle;
+  }
+  static void init_type() {
+    CollisionSolid::init_type();
+    register_type(_type_handle, "CollisionTube",
+                  CollisionSolid::get_class_type());
+  }
+  virtual TypeHandle get_type() const {
+    return get_class_type();
+  }
+  virtual TypeHandle force_init_type() {init_type(); return get_class_type();}
+
+private:
+  static TypeHandle _type_handle;
+};
+
+#include "collisionTube.I"
+
+#endif
+
+

panda/src/collide/collisionVisualizer.cxx

@@ -27,6 +27,8 @@
 #include "depthOffsetAttrib.h"
 #include "colorScaleAttrib.h"
 #include "transparencyAttrib.h"
+#include "geomLine.h"
+#include "geomSphere.h"
 
 
 #ifdef DO_COLLISION_RECORDING
@@ -155,7 +157,7 @@ cull_callback(CullTraverser *trav, CullTraverserData &data) {
 
     // Now draw all of the detected points.
     if (!viz_info._points.empty()) {
-      CPT(RenderState) line_state = RenderState::make_empty();
+      CPT(RenderState) empty_state = RenderState::make_empty();
 
       PTA_Colorf colors;
       colors.push_back(Colorf(1.0f, 0.0f, 0.0f, 1.0f));
@@ -164,19 +166,38 @@ cull_callback(CullTraverser *trav, CullTraverserData &data) {
       Points::const_iterator pi;
       for (pi = viz_info._points.begin(); pi != viz_info._points.end(); ++pi) {
         const CollisionPoint &point = (*pi);
-        PT(GeomLine) line = new GeomLine;
+
+        // Draw a tiny sphere at the collision point.
+        {
+          PT(GeomSphere) sphere = new GeomSphere;
+          PTA_Vertexf verts;
+          verts.push_back(point._point);
+          verts.push_back(point._point + LVector3f(0.1f, 0.0f, 0.0f));
+          sphere->set_coords(verts);
+          sphere->set_colors(colors, G_OVERALL);
+          sphere->set_num_prims(1);
+          CullableObject *object = 
+            new CullableObject(sphere, empty_state, xform_data._render_transform);
+          
+          trav->get_cull_handler()->record_object(object);
+        }
+
+        // Draw the normal vector at the collision point.
+        if (!point._normal.almost_equal(LVector3f::zero())) {
+          PT(GeomLine) line = new GeomLine;
         
-        PTA_Vertexf verts;
+          PTA_Vertexf verts;
-        verts.push_back(point._point);
+          verts.push_back(point._point);
-        verts.push_back(point._point + point._normal);
+          verts.push_back(point._point + point._normal);
-        line->set_coords(verts);
+          line->set_coords(verts);
-        line->set_colors(colors, G_PER_VERTEX);
+          line->set_colors(colors, G_PER_VERTEX);
-        line->set_num_prims(1);
+          line->set_num_prims(1);
-
+          
-        CullableObject *object = 
+          CullableObject *object = 
-          new CullableObject(line, line_state, xform_data._render_transform);
+            new CullableObject(line, empty_state, xform_data._render_transform);
-
+          
-        trav->get_cull_handler()->record_object(object);
+          trav->get_cull_handler()->record_object(object);
+        }
       }
     }
 
@@ -232,11 +253,14 @@ collision_tested(const CollisionEntry &entry, bool detected) {
   if (detected) {
     viz_info._solids[entry.get_into()]._detected_count++;
 
-    if (entry.has_into_intersection_point() &&
+    if (entry.has_into_intersection_point()) {
-        entry.has_into_surface_normal()) {
       CollisionPoint p;
       p._point = entry.get_into_intersection_point();
-      p._normal = entry.get_into_surface_normal();
+      if (entry.has_into_surface_normal()) {
+        p._normal = entry.get_into_surface_normal();
+      } else {
+        p._normal = LVector3f::zero();
+      }
       viz_info._points.push_back(p);
     }
 

panda/src/collide/config_collide.cxx

@@ -32,6 +32,7 @@
 #include "collisionSegment.h"
 #include "collisionSolid.h"
 #include "collisionSphere.h"
+#include "collisionTube.h"
 #include "collisionVisualizer.h"
 #include "dconfig.h"
 
@@ -77,6 +78,7 @@ init_libcollide() {
   CollisionSegment::init_type();
   CollisionSolid::init_type();
   CollisionSphere::init_type();
+  CollisionTube::init_type();
 
 #ifdef DO_COLLISION_RECORDING
   CollisionRecorder::init_type();