Added CollisionBox courtesy CMU ETC student Amith Tudur

panda/src/collide/collide_composite2.cxx

@@ -11,3 +11,4 @@
 #include "collisionTraverser.cxx"
 #include "collisionTube.cxx"
 #include "collisionVisualizer.cxx"
+#include "collisionBox.cxx"

panda/src/collide/collisionBox.I

@@ -0,0 +1,362 @@
+// Filename: collisionBox.I
+// Created by:  amith tudur (31Jul09)
+//
+////////////////////////////////////////////////////////////////////
+//
+// PANDA 3D SOFTWARE
+// Copyright (c) Carnegie Mellon University.  All rights reserved.
+//
+// All use of this software is subject to the terms of the revised BSD
+// license.  You should have received a copy of this license along
+// with this source code in a file named "LICENSE."
+//
+////////////////////////////////////////////////////////////////////
+
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::Constructor
+//       Access: Public
+//  Description: Create the Box by giving a Center and distances of
+//               of each of the sides of box from the Center.
+////////////////////////////////////////////////////////////////////
+INLINE CollisionBox::
+CollisionBox(const LPoint3f &center, float x, float y, float z) :
+  _center(center), _x(x), _y(y), _z(z)
+{
+  _min = LPoint3f(_center.get_x() - _x, _center.get_y() - _y, _center.get_z() - _z);
+  _max = LPoint3f(_center.get_x() + _x, _center.get_y() + _y, _center.get_z() + _z); 
+  _radius = sqrt(_x*_x + _y*_y + _z*_z);
+  for(int v = 0; v < 8; v++)
+    _vertex[v] = get_point_aabb(v);
+  for(int p = 0; p < 6; p++)
+    _planes[p] = set_plane(p);
+  setup_box();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::Constructor
+//       Access: Public
+//  Description: Create the Box by Specifying the Diagonal Points
+////////////////////////////////////////////////////////////////////
+INLINE CollisionBox::
+CollisionBox(const LPoint3f &min, const LPoint3f &max) :
+  _min(min), _max(max)
+{
+  _center = (_min + _max) / 2;
+  _x = _center.get_x() - _min.get_x();
+  _y = _center.get_y() - _min.get_y();
+  _z = _center.get_z() - _min.get_z();
+  _radius = sqrt(_x*_x + _y*_y + _z*_z);
+  for(int v = 0; v < 8; v++)
+    _vertex[v] = get_point_aabb(v);
+  for(int p = 0; p < 6; p++)
+    _planes[p] = set_plane(p);
+  setup_box();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::Constructor
+//       Access: Public
+//  Description: Center as three separate co-ordinate points
+////////////////////////////////////////////////////////////////////
+INLINE CollisionBox::
+CollisionBox(float cx, float cy, float cz, float x, float y, float z) 
+{
+  CollisionBox(LPoint3f(cx, cy, cz), x, y, z);
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::Default constructor
+//       Access: Protected
+//  Description: Creates an invalid Box.  Only used when reading
+//               from a bam file.
+////////////////////////////////////////////////////////////////////
+INLINE CollisionBox::
+CollisionBox() {
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::Copy Constructor
+//       Access: Public
+//  Description:
+////////////////////////////////////////////////////////////////////
+INLINE CollisionBox::
+CollisionBox(const CollisionBox &copy) :
+  CollisionSolid(copy),
+  _center(copy._center),
+  _x( copy._x ),
+  _y( copy._y ),
+  _z( copy._z ),
+  _min( copy._min),
+  _max( copy._max),
+  _radius( copy._radius )
+{
+  for(int v = 0; v < 8; v++)
+    _vertex[v] = copy._vertex[v];
+  for(int p = 0; p < 6; p++)
+    _planes[p] = copy._planes[p];
+  setup_box();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::flush_level
+//       Access: Public, Static
+//  Description: Flushes the PStatCollectors used during traversal.
+////////////////////////////////////////////////////////////////////
+INLINE void CollisionBox::
+flush_level() {
+  _volume_pcollector.flush_level();
+  _test_pcollector.flush_level();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::set_center
+//       Access: Published
+//  Description:
+////////////////////////////////////////////////////////////////////
+INLINE void CollisionBox::
+set_center(const LPoint3f &center) {
+  _center = center;
+  mark_internal_bounds_stale();
+  mark_viz_stale();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::set_center
+//       Access: Published
+//  Description:
+////////////////////////////////////////////////////////////////////
+INLINE void CollisionBox::
+set_center(float x, float y, float z) {
+  set_center(LPoint3f(x, y, z));
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::get_center
+//       Access: Published
+//  Description:
+////////////////////////////////////////////////////////////////////
+INLINE const LPoint3f &CollisionBox::
+get_center() const {
+  return _center;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::get_radius
+//       Access: Published
+//  Description:
+////////////////////////////////////////////////////////////////////
+INLINE float CollisionBox::
+get_radius() const {
+  return _radius;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::get_num_points
+//       Access: Published
+//  Description: Returns 8: the number of vertices of a rectangular solid.
+////////////////////////////////////////////////////////////////////
+INLINE_MATHUTIL int CollisionBox::
+get_num_points() const {
+  return 8;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::get_point
+//       Access: Published
+//  Description: Returns the nth vertex of the OBB.
+////////////////////////////////////////////////////////////////////
+INLINE_MATHUTIL LPoint3f CollisionBox::
+get_point(int n) const {
+  nassertr(n >= 0 && n < 8, LPoint3f::zero());
+  return _vertex[n];
+}
+
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::get_point_aabb
+//       Access: Published
+//  Description: Returns the nth vertex of the Axis Aligned Bounding Box.
+////////////////////////////////////////////////////////////////////
+INLINE_MATHUTIL LPoint3f CollisionBox::
+get_point_aabb(int n) const {
+  nassertr(n >= 0 && n < 8, LPoint3f::zero());
+  
+  // We do some trickery assuming that _min and _max are consecutive
+  // in memory.
+  const LPoint3f *a = &_min;
+  return LPoint3f(a[(n>>2)&1][0], a[(n>>1)&1][1], a[(n)&1][2]);
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::get_num_planes
+//       Access: Published
+//  Description: Returns 6: the number of faces of a rectangular solid.
+////////////////////////////////////////////////////////////////////
+INLINE_MATHUTIL int CollisionBox::
+get_num_planes() const {
+  return 6;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::get_plane
+//       Access: Published
+//  Description: Returns the nth face of the rectangular solid.
+////////////////////////////////////////////////////////////////////
+INLINE_MATHUTIL Planef CollisionBox::
+get_plane(int n) const {
+  nassertr(n >= 0 && n < 6, Planef());
+  return _planes[n];
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::set_plane
+//       Access: Published
+//  Description: Creates the nth face of the rectangular solid.
+////////////////////////////////////////////////////////////////////
+INLINE_MATHUTIL Planef CollisionBox::
+set_plane(int n) const {
+  nassertr(n >= 0 && n < 6, Planef());
+  return Planef(get_point(plane_def[n][0]),
+                get_point(plane_def[n][1]),
+                get_point(plane_def[n][2]));
+}
+
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::is_right
+//       Access: Private, Static
+//  Description: Returns true if the 2-d v1 is to the right of v2.
+////////////////////////////////////////////////////////////////////
+INLINE bool CollisionBox::
+is_right(const LVector2f &v1, const LVector2f &v2) {
+  return (v1[0] * v2[1] - v1[1] * v2[0]) > 1.0e-6f;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::dist_to_line
+//       Access: Private, Static
+//  Description: Returns the linear distance of p to the line defined
+//               by f and f+v, where v is a normalized vector.  The
+//               result is negative if p is left of the line, positive
+//               if it is right of the line.
+////////////////////////////////////////////////////////////////////
+INLINE float CollisionBox::
+dist_to_line(const LPoint2f &p,
+             const LPoint2f &f, const LVector2f &v) {
+  LVector2f v1 = (p - f);
+  return (v1[0] * v[1] - v1[1] * v[0]);
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::to_2d
+//       Access: Private
+//  Description: Assuming the indicated point in 3-d space lies within
+//               the polygon's plane, returns the corresponding point
+//               in the polygon's 2-d definition space.
+////////////////////////////////////////////////////////////////////
+INLINE LPoint2f CollisionBox::
+to_2d(const LVecBase3f &point3d, int plane) const {
+  LPoint3f point = LPoint3f(point3d) * _to_2d_mat[plane];
+  return LPoint2f(point[0], point[2]);
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::calc_to_3d_mat
+//       Access: Private
+//  Description: Fills the indicated matrix with the appropriate
+//               rotation transform to move points from the 2-d plane
+//               into the 3-d (X, 0, Z) plane.
+////////////////////////////////////////////////////////////////////
+INLINE void CollisionBox::
+calc_to_3d_mat(LMatrix4f &to_3d_mat,int plane) const {
+  // We have to be explicit about the coordinate system--we
+  // specifically mean CS_zup_right, because that points the forward
+  // vector down the Y axis and moves the coords in (X, 0, Z).  We
+  // want this effect regardless of the user's coordinate system of
+  // choice.
+
+  // The up vector, on the other hand, is completely arbitrary.
+
+  look_at(to_3d_mat, -get_plane(plane).get_normal(), 
+          LVector3f(0.0f, 0.0f, 1.0f), CS_zup_right);
+  to_3d_mat.set_row(3, get_plane(plane).get_point());
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::rederive_to_3d_mat
+//       Access: Private
+//  Description: Fills the indicated matrix with the appropriate
+//               rotation transform to move points from the 2-d plane
+//               into the 3-d (X, 0, Z) plane.
+//
+//               This is essentially similar to calc_to_3d_mat, except
+//               that the matrix is rederived from whatever is stored
+//               in _to_2d_mat, guaranteeing that it will match
+//               whatever algorithm produced that one, even if it was
+//               produced on a different machine with different
+//               numerical precision.
+////////////////////////////////////////////////////////////////////
+INLINE void CollisionBox::
+rederive_to_3d_mat(LMatrix4f &to_3d_mat, int plane) const {
+  to_3d_mat.invert_from(_to_2d_mat[plane]);
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::to_3d
+//       Access: Private, Static
+//  Description: Extrude the indicated point in the polygon's 2-d
+//               definition space back into 3-d coordinates.
+////////////////////////////////////////////////////////////////////
+INLINE LPoint3f CollisionBox::
+to_3d(const LVecBase2f &point2d, const LMatrix4f &to_3d_mat) {
+  return LPoint3f(point2d[0], 0.0f, point2d[1]) * to_3d_mat;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::PointDef::Constructor
+//       Access: Public
+//  Description: 
+////////////////////////////////////////////////////////////////////
+INLINE CollisionBox::PointDef::
+PointDef(const LPoint2f &p, const LVector2f &v) : _p(p), _v(v) {
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::PointDef::Constructor
+//       Access: Public
+//  Description: 
+////////////////////////////////////////////////////////////////////
+INLINE CollisionBox::PointDef::
+PointDef(float x, float y) : _p(x, y), _v(0.0f, 0.0f) {
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::PointDef::Copy Constructor
+//       Access: Public
+//  Description: 
+////////////////////////////////////////////////////////////////////
+INLINE CollisionBox::PointDef::
+PointDef(const CollisionBox::PointDef &copy) : _p(copy._p), _v(copy._v) {
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::PointDef::Copy Assignment Operator
+//       Access: Public
+//  Description: 
+////////////////////////////////////////////////////////////////////
+INLINE void CollisionBox::PointDef::
+operator = (const CollisionBox::PointDef &copy) {
+  _p = copy._p;
+  _v = copy._v;
+}
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::get_plane_points
+//       Access: Public
+//  Description: returns the points that form the nth plane
+////////////////////////////////////////////////////////////////////
+
+INLINE CollisionBox::Points CollisionBox::
+get_plane_points( int n ){
+	return _points[n];
+}

panda/src/collide/collisionBox.cxx

@@ -0,0 +1,1044 @@
+// Filename: collisionBox.cxx
+// Created by:  amith tudur (31Jul09)
+//
+////////////////////////////////////////////////////////////////////
+//
+// PANDA 3D SOFTWARE
+// Copyright (c) Carnegie Mellon University.  All rights reserved.
+//
+// All use of this software is subject to the terms of the revised BSD
+// license.  You should have received a copy of this license along
+// with this source code in a file named "LICENSE."
+//
+////////////////////////////////////////////////////////////////////
+
+
+#include "collisionDSSolid.h"
+#include "collisionBox.h"
+#include "collisionLine.h"
+#include "collisionRay.h"
+#include "collisionSegment.h"
+#include "collisionHandler.h"
+#include "collisionEntry.h"
+#include "config_collide.h"
+#include "boundingBox.h"
+#include "datagram.h"
+#include "datagramIterator.h"
+#include "bamReader.h"
+#include "bamWriter.h"
+#include "nearly_zero.h"
+#include "cmath.h"
+#include "mathNumbers.h"
+#include "geom.h"
+#include "geomTristrips.h"
+#include "geomVertexWriter.h"
+#include "config_mathutil.h"
+#include "dcast.h"
+
+#include <math.h>
+
+PStatCollector CollisionBox::_volume_pcollector(
+  "Collision Volumes:CollisionBox");
+PStatCollector CollisionBox::_test_pcollector(
+  "Collision Tests:CollisionBox");
+TypeHandle CollisionBox::_type_handle;
+
+const int CollisionBox::plane_def[6][4] = {
+  {0, 4, 5, 1},
+  {4, 6, 7, 5},
+  {6, 2, 3, 7},
+  {2, 0, 1, 3},
+  {1, 5, 7, 3},
+  {2, 6, 4, 0},
+};
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::make_copy
+//       Access: Public, Virtual
+//  Description:
+////////////////////////////////////////////////////////////////////
+CollisionSolid *CollisionBox::
+make_copy() {
+  return new CollisionBox(*this);
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::setup_box
+//       Access: Public, Virtual
+//  Description: Compute parameters for each of the box's sides
+////////////////////////////////////////////////////////////////////
+void CollisionBox::
+setup_box(){
+  for(int plane = 0; plane < 6; plane++) {
+    LPoint3f array[4];
+    array[0] = get_point(plane_def[plane][0]);
+    array[1] = get_point(plane_def[plane][1]);
+    array[2] = get_point(plane_def[plane][2]);
+    array[3] = get_point(plane_def[plane][3]);
+    setup_points(array, array+4, plane);
+	}
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::setup_points
+//       Access: Private
+//  Description: Computes the plane and 2d projection of points that
+//               make up this side          
+////////////////////////////////////////////////////////////////////
+void CollisionBox::
+setup_points(const LPoint3f *begin, const LPoint3f *end, int plane) {
+  int num_points = end - begin;
+  nassertv(num_points >= 3);
+
+  _points[plane].clear();
+
+  // Construct a matrix that rotates the points from the (X,0,Z) plane
+  // into the 3-d plane.
+  LMatrix4f to_3d_mat;
+  calc_to_3d_mat(to_3d_mat, plane);
+
+  // And the inverse matrix rotates points from 3-d space into the 2-d
+  // plane.
+  _to_2d_mat[plane].invert_from(to_3d_mat);
+
+  // Now project all of the points onto the 2-d plane.
+
+  const LPoint3f *pi;
+  for (pi = begin; pi != end; ++pi) {
+    LPoint3f point = (*pi) * _to_2d_mat[plane];
+    _points[plane].push_back(PointDef(point[0], point[2]));
+  }
+
+  nassertv(_points[plane].size() >= 3);
+
+#ifndef NDEBUG
+  /*
+  // Now make sure the points define a convex polygon.
+  if (is_concave()) {
+    collide_cat.error() << "Invalid concave CollisionPolygon defined:\n";
+    const LPoint3f *pi;
+    for (pi = begin; pi != end; ++pi) {
+      collide_cat.error(false) << "  " << (*pi) << "\n";
+    }
+    collide_cat.error(false)
+      << "  normal " << normal << " with length " << normal.length() << "\n";
+    _points.clear();
+  }
+  */
+#endif
+
+  compute_vectors(_points[plane]);
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::test_intersection
+//       Access: Public, Virtual
+//  Description: First Dispatch point for box as a FROM object
+////////////////////////////////////////////////////////////////////
+PT(CollisionEntry) CollisionBox::
+test_intersection(const CollisionEntry &entry) const {
+  return entry.get_into()->test_intersection_from_box(entry);
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::xform
+//       Access: Public, Virtual
+//  Description: Transforms the solid by the indicated matrix.
+////////////////////////////////////////////////////////////////////
+void CollisionBox::
+xform(const LMatrix4f &mat) {
+  _center = _center * mat;
+  for(int v = 0; v < 8; v++)
+	   _vertex[v] = _vertex[v] * mat;
+  for(int p = 0; p < 6 ; p++)
+	   _planes[p] = set_plane(p);
+  _x = _vertex[0].get_x()-_center.get_x(); 
+  _y = _vertex[0].get_y()-_center.get_y();
+  _z = _vertex[0].get_z()-_center.get_z();
+  _radius = sqrt( _x*_x + _y*_y + _z*_z );
+  setup_box();
+  mark_viz_stale();
+  mark_internal_bounds_stale();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::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 CollisionBox::
+get_collision_origin() const {
+	return _center;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::get_min
+//       Access: Public, Virtual
+//  Description: 
+////////////////////////////////////////////////////////////////////
+LPoint3f CollisionBox::
+get_min() const {
+  return _min;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::get_max
+//       Access: Public, Virtual
+//  Description: 
+////////////////////////////////////////////////////////////////////
+LPoint3f CollisionBox::
+get_max() const {
+  return _max;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::get_approx_center
+//       Access: Public, Virtual
+//  Description: 
+////////////////////////////////////////////////////////////////////
+LPoint3f CollisionBox::
+get_approx_center() const {
+  return (_min + _max) * 0.5f;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::get_volume_pcollector
+//       Access: Public, Virtual
+//  Description: Returns a PStatCollector that is used to count the
+//               number of bounding volume tests made against a solid
+//               of this type in a given frame.
+////////////////////////////////////////////////////////////////////
+PStatCollector &CollisionBox::
+get_volume_pcollector() {
+  return _volume_pcollector;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::get_test_pcollector
+//       Access: Public, Virtual
+//  Description: Returns a PStatCollector that is used to count the
+//               number of intersection tests made against a solid
+//               of this type in a given frame.
+////////////////////////////////////////////////////////////////////
+PStatCollector &CollisionBox::
+get_test_pcollector() {
+  return _test_pcollector;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::output
+//       Access: Public, Virtual
+//  Description:
+////////////////////////////////////////////////////////////////////
+void CollisionBox::
+output(ostream &out) const {
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::compute_internal_bounds
+//       Access: Protected, Virtual
+//  Description: Sphere is chosen as the Bounding Volume type for 
+//               speed and efficiency
+////////////////////////////////////////////////////////////////////
+PT(BoundingVolume) CollisionBox::
+compute_internal_bounds() const {
+  return new BoundingSphere(_center, _radius);
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::test_intersection_from_sphere
+//       Access: Public, Virtual
+//  Description: Double dispatch point for sphere as FROM object
+////////////////////////////////////////////////////////////////////
+PT(CollisionEntry) CollisionBox::
+test_intersection_from_sphere(const CollisionEntry &entry) const {
+
+  const CollisionSphere *sphere;
+  DCAST_INTO_R(sphere, entry.get_from(), 0);
+
+  CPT(TransformState) wrt_space = entry.get_wrt_space();
+  CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
+
+  const LMatrix4f &wrt_mat = wrt_space->get_mat();
+
+  LPoint3f orig_center = sphere->get_center() * wrt_mat;
+  LPoint3f from_center = orig_center;
+  bool moved_from_center = false;
+  float t = 1.0f;
+  LPoint3f contact_point(from_center);
+  float actual_t = 1.0f;
+
+  LVector3f from_radius_v =
+    LVector3f(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
+  float from_radius_2 = from_radius_v.length_squared();
+  float from_radius = csqrt(from_radius_2);
+
+  int ip;
+  float max_dist,dist;
+  bool intersect;
+  Planef plane;
+  LVector3f normal;
+  
+  for(ip = 0, intersect = false; ip < 6 && !intersect; ip++) {
+    plane = get_plane( ip );
+    if (_points[ip].size() < 3) {
+      continue;
+    }
+    if (wrt_prev_space != wrt_space) {
+		    // If we have a delta between the previous position and the
+		    // current position, we use that to determine some more properties
+		    // of the collision.
+		    LPoint3f b = from_center;
+		    LPoint3f a = sphere->get_center() * wrt_prev_space->get_mat();
+		    LVector3f delta = b - a;
+
+		    // First, there is no collision if the "from" object is definitely
+		    // moving in the same direction as the plane's normal.
+		    float dot = delta.dot(plane.get_normal());
+		    if (dot > 0.1f) {
+		      continue; // no intersection
+		    }
+
+		    if (IS_NEARLY_ZERO(dot)) {
+		      // If we're moving parallel to the plane, the sphere is tested
+		      // at its final point.  Leave it as it is.
+
+		    } else {
+		      // Otherwise, we're moving into the plane; the sphere is tested
+		      // at the point along its path that is closest to intersecting
+		      // the plane.  This may be the actual intersection point, or it
+		      // may be the starting point or the final point.
+		      // dot is equal to the (negative) magnitude of 'delta' along the
+		      // direction of the plane normal
+		      // t = ratio of (distance from start pos to plane) to (distance
+		      // from start pos to end pos), along axis of plane normal
+		      float dist_to_p = plane.dist_to_plane(a);
+		      t = (dist_to_p / -dot);
+    	      
+		      // also compute the actual contact point and time of contact
+		      // for handlers that need it
+		      actual_t = ((dist_to_p - from_radius) / -dot);
+		      actual_t = min(1.0f, max(0.0f, actual_t));
+		      contact_point = a + (actual_t * delta);
+
+		      if (t >= 1.0f) {
+			    // Leave it where it is.
+
+		      } else if (t < 0.0f) {
+             from_center = a;
+             moved_from_center = true;
+		      } else {
+			          from_center = a + t * delta;
+			          moved_from_center = true;
+		      }
+		    }
+	   }
+
+    normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : plane.get_normal();
+	   
+    #ifndef NDEBUG
+    /*if (!IS_THRESHOLD_EQUAL(normal.length_squared(), 1.0f, 0.001), NULL) {
+      std::cout
+      << "polygon within " << entry.get_into_node_path()
+      << " has normal " << normal << " of length " << normal.length()
+      << "\n";
+      normal.normalize();
+    }*/
+    #endif
+
+	  // The nearest point within the plane to our center is the
+	  // intersection of the line (center, center - normal) with the plane.
+	  
+    if (!plane.intersects_line(dist, from_center, -(plane.get_normal()))) {
+		    // No intersection with plane?  This means the plane's effective
+		    // normal was within the plane itself.  A useless polygon.
+		    continue;
+	   }
+
+    if (dist > from_radius || dist < -from_radius) {
+      // No intersection with the plane.
+      continue;
+    }
+
+    LPoint2f p = to_2d(from_center - dist * plane.get_normal(), ip);
+    float edge_dist = 0.0f;
+
+    const ClipPlaneAttrib *cpa = entry.get_into_clip_planes();
+    if (cpa != (ClipPlaneAttrib *)NULL) {
+      // We have a clip plane; apply it.
+      Points new_points;
+      if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform(),ip)) {
+        // All points are behind the clip plane; just do the default
+        // test.
+        edge_dist = dist_to_polygon(p, _points[ip]);
+      } else if (new_points.empty()) {
+        // The polygon is completely clipped.
+        continue;
+      } else {
+        // Test against the clipped polygon.
+        edge_dist = dist_to_polygon(p, new_points);
+      }
+    } else {
+      // No clip plane is in effect.  Do the default test. 
+      edge_dist = dist_to_polygon(p, _points[ip]);
+    }
+
+    // Now we have edge_dist, which is the distance from the sphere
+    // center to the nearest edge of the polygon, within the polygon's
+    // plane. edge_dist<0 means the point is within the polygon.
+    if(edge_dist < 0) {
+      intersect = true;
+      continue;
+    }
+
+    if((edge_dist > 0) && 
+      ((edge_dist * edge_dist + dist * dist) > from_radius_2)) {
+      // No intersection; the circle is outside the polygon.
+      continue;
+    }
+
+    // The sphere appears to intersect the polygon.  If the edge is less
+    // than from_radius away, the sphere may be resting on an edge of
+    // the polygon.  Determine how far the center of the sphere must
+    // remain from the plane, based on its distance from the nearest
+    // edge.
+
+    max_dist = from_radius;
+    if (edge_dist >= 0.0f) {
+      float max_dist_2 = max(from_radius_2 - edge_dist * edge_dist, 0.0f);
+      max_dist = csqrt(max_dist_2);
+    }
+
+    if (dist > max_dist) {
+      // There's no intersection: the sphere is hanging off the edge.
+      continue;
+    }
+    intersect = true;
+  }
+  if( !intersect )
+	  return NULL;
+
+  if (collide_cat.is_debug()) {
+    collide_cat.debug()
+      << "intersection detected from " << entry.get_from_node_path()
+      << " into " << entry.get_into_node_path() << "\n";
+  }
+  
+  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
+
+  float into_depth = max_dist - dist;
+  if (moved_from_center) {
+    // We have to base the depth of intersection on the sphere's final
+    // resting point, not the point from which we tested the
+    // intersection.
+    float orig_dist;
+    plane.intersects_line(orig_dist, orig_center, -normal);
+    into_depth = max_dist - orig_dist;
+  }
+
+  new_entry->set_surface_normal(normal);
+  new_entry->set_surface_point(from_center - normal * dist);
+  new_entry->set_interior_point(from_center - normal * (dist + into_depth));
+  new_entry->set_contact_pos(contact_point);
+  new_entry->set_contact_normal(plane.get_normal());
+  new_entry->set_t(actual_t);
+
+  return new_entry;
+}
+
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::test_intersection_from_ray
+//       Access: Public, Virtual
+//  Description: Double dispatch point for ray as a FROM object
+////////////////////////////////////////////////////////////////////
+PT(CollisionEntry) CollisionBox::
+test_intersection_from_ray(const CollisionEntry &entry) const {
+  const CollisionRay *ray;
+  DCAST_INTO_R(ray, entry.get_from(), 0);
+  const LMatrix4f &wrt_mat = entry.get_wrt_mat();
+
+  LPoint3f from_origin = ray->get_origin() * wrt_mat;
+  LVector3f from_direction = ray->get_direction() * wrt_mat;
+
+  int i, j;
+  float t, near_t;
+  bool intersect;
+  Planef plane;
+  Planef near_plane; 
+
+  //Returns the details about the first plane of the box that the ray
+  //intersects.
+  for(i = 0, intersect = false, t = 0, j = 0; i < 6 && j < 2; i++) {
+    plane = get_plane(i);
+
+    if (!plane.intersects_line(t, from_origin, from_direction)) {
+      // No intersection.  The ray is parallel to the plane.
+      continue;
+    }
+
+    if (t < 0.0f) {
+      // The intersection point is before the start of the ray, and so
+      // the ray is entirely in front of the plane.
+      continue;
+    }
+    LPoint3f plane_point = from_origin + t * from_direction;
+    LPoint2f p = to_2d(plane_point, i);
+
+    if (!point_is_inside(p, _points[i])){
+      continue;
+    }
+    intersect = true;
+    if(j) {
+      if(t < near_t) {
+        near_plane = plane;
+        near_t = t;
+      }
+    }
+    else {
+      near_plane = plane;
+      near_t = t;
+    }
+    ++j;
+  }
+	 
+
+  if(!intersect) {
+	   //No intersection with ANY of the box's planes has been detected
+	   return NULL;
+  }
+
+  if (collide_cat.is_debug()) {
+    collide_cat.debug()
+      << "intersection detected from " << entry.get_from_node_path()
+      << " into " << entry.get_into_node_path() << "\n";
+  }
+
+  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
+
+  LPoint3f into_intersection_point = from_origin + near_t * from_direction;
+
+  LVector3f normal =
+    (has_effective_normal() && ray->get_respect_effective_normal()) 
+    ? get_effective_normal() : near_plane.get_normal();
+
+  new_entry->set_surface_normal(normal);
+  new_entry->set_surface_point(into_intersection_point);
+
+  return new_entry;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::test_intersection_from_box
+//       Access: Public, Virtual
+//  Description: Double dispatch point for box as a FROM object
+//               NOT Implemented.
+////////////////////////////////////////////////////////////////////
+PT(CollisionEntry) CollisionBox::
+test_intersection_from_box(const CollisionEntry &entry) const {
+  return NULL;
+}
+
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::fill_viz_geom
+//       Access: Protected, Virtual
+//  Description: Fills the _viz_geom GeomNode up with Geoms suitable
+//               for rendering this solid.
+////////////////////////////////////////////////////////////////////
+void CollisionBox::
+fill_viz_geom() {
+  if (collide_cat.is_debug()) {
+    collide_cat.debug()
+      << "Recomputing viz for " << *this << "\n";
+  }
+
+  PT(GeomVertexData) vdata = new GeomVertexData
+    ("collision", GeomVertexFormat::get_v3(),
+     Geom::UH_static);
+  GeomVertexWriter vertex(vdata, InternalName::get_vertex());
+  
+  for(int i = 0; i < 6; i++) {
+    for(int j = 0; j < 4; j++)
+      vertex.add_data3f(get_point(plane_def[i][j]));
+
+    PT(GeomTrifans) body = new GeomTrifans(Geom::UH_static);
+    body->add_consecutive_vertices(i*4, 4);
+    body->close_primitive();
+
+    PT(Geom) geom = new Geom(vdata);
+    geom->add_primitive(body);
+
+    _viz_geom->add_geom(geom, get_solid_viz_state());
+    _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state());
+  }
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::apply_clip_plane
+//       Access: Private
+//  Description: Clips the polygon by all of the clip planes named in
+//               the clip plane attribute and fills new_points up with
+//               the resulting points.
+//
+//               The return value is true if the set of points is
+//               unmodified (all points are behind all the clip
+//               planes), or false otherwise.
+////////////////////////////////////////////////////////////////////
+bool CollisionBox::
+apply_clip_plane(CollisionBox::Points &new_points, 
+                 const ClipPlaneAttrib *cpa,
+                 const TransformState *net_transform, int plane_no) const {
+  bool all_in = true;
+
+  int num_planes = cpa->get_num_on_planes();
+  bool first_plane = true;
+
+  for (int i = 0; i < num_planes; i++) {
+    NodePath plane_path = cpa->get_on_plane(0);
+    PlaneNode *plane_node = DCAST(PlaneNode, plane_path.node());
+    if ((plane_node->get_clip_effect() & PlaneNode::CE_collision) != 0) {
+      CPT(TransformState) new_transform = 
+        net_transform->invert_compose(plane_path.get_net_transform());
+      
+      Planef plane = plane_node->get_plane() * new_transform->get_mat();
+      if (first_plane) {
+        first_plane = false;
+        if (!clip_polygon(new_points, _points[plane_no], plane, plane_no)) {
+          all_in = false;
+        }
+      } else {
+        Points last_points;
+        last_points.swap(new_points);
+        if (!clip_polygon(new_points, last_points, plane, plane_no)) {
+          all_in = false;
+        }
+      }
+    }
+  }
+
+  if (!all_in) {
+    compute_vectors(new_points);
+  }
+
+  return all_in;
+}
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::clip_polygon
+//       Access: Private
+//  Description: Clips the source_points of the polygon by the
+//               indicated clipping plane, and modifies new_points to
+//               reflect the new set of clipped points (but does not
+//               compute the vectors in new_points).
+//
+//               The return value is true if the set of points is
+//               unmodified (all points are behind the clip plane), or
+//               false otherwise.
+////////////////////////////////////////////////////////////////////
+bool CollisionBox::
+clip_polygon(CollisionBox::Points &new_points, 
+             const CollisionBox::Points &source_points,
+             const Planef &plane, int plane_no) const {
+  new_points.clear();
+  if (source_points.empty()) {
+    return true;
+  }
+
+  LPoint3f from3d;
+  LVector3f delta3d;
+  if (!plane.intersects_plane(from3d, delta3d, get_plane(plane_no))) {
+    // The clipping plane is parallel to the polygon.  The polygon is
+    // either all in or all out.
+    if (plane.dist_to_plane(get_plane(plane_no).get_point()) < 0.0) {
+      // A point within the polygon is behind the clipping plane: the
+      // polygon is all in.
+      new_points = source_points;
+      return true;
+    }
+    return false;
+  }
+
+  // Project the line of intersection into the 2-d plane.  Now we have
+  // a 2-d clipping line.
+  LPoint2f from2d = to_2d(from3d,plane_no);
+  LVector2f delta2d = to_2d(delta3d,plane_no);
+
+  float a = -delta2d[1];
+  float b = delta2d[0];
+  float c = from2d[0] * delta2d[1] - from2d[1] * delta2d[0];
+
+  // Now walk through the points.  Any point on the left of our line
+  // gets removed, and the line segment clipped at the point of
+  // intersection.
+
+  // We might increase the number of vertices by as many as 1, if the
+  // plane clips off exactly one corner.  (We might also decrease the
+  // number of vertices, or keep them the same number.)
+  new_points.reserve(source_points.size() + 1);
+
+  LPoint2f last_point = source_points.back()._p;
+  bool last_is_in = !is_right(last_point - from2d, delta2d);
+  bool all_in = last_is_in;
+  Points::const_iterator pi;
+  for (pi = source_points.begin(); pi != source_points.end(); ++pi) {
+    const LPoint2f &this_point = (*pi)._p;
+    bool this_is_in = !is_right(this_point - from2d, delta2d);
+
+    // There appears to be a compiler bug in gcc 4.0: we need to
+    // extract this comparison outside of the if statement.
+    bool crossed_over = (this_is_in != last_is_in);
+    if (crossed_over) {
+      // We have just crossed over the clipping line.  Find the point
+      // of intersection.
+      LVector2f d = this_point - last_point;
+      float denom = (a * d[0] + b * d[1]);
+      if (denom != 0.0) {
+        float t = -(a * last_point[0] + b * last_point[1] + c) / denom;
+        LPoint2f p = last_point + t * d;
+
+        new_points.push_back(PointDef(p[0], p[1]));
+        last_is_in = this_is_in;
+      }
+    } 
+
+    if (this_is_in) {
+      // We are behind the clipping line.  Keep the point.
+      new_points.push_back(PointDef(this_point[0], this_point[1]));
+    } else {
+      all_in = false;
+    }
+
+    last_point = this_point;
+  }
+
+  return all_in;
+}
+
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::
+//       Access: Private
+//  Description: Returns the linear distance from the 2-d point to the
+//               nearest part of the polygon defined by the points
+//               vector.  The result is negative if the point is
+//               within the polygon.
+////////////////////////////////////////////////////////////////////
+float CollisionBox::
+dist_to_polygon(const LPoint2f &p, const CollisionBox::Points &points) const {
+
+  // We know that that the polygon is convex and is defined with the
+  // points in counterclockwise order.  Therefore, we simply compare
+  // the signed distance to each line segment; we ignore any negative
+  // values, and take the minimum of all the positive values.  
+
+  // If all values are negative, the point is within the polygon; we
+  // therefore return an arbitrary negative result.
+  
+  bool got_dist = false;
+  float best_dist = -1.0f;
+
+  size_t num_points = points.size();
+  for (size_t i = 0; i < num_points - 1; ++i) {
+    float d = dist_to_line_segment(p, points[i]._p, points[i + 1]._p,
+                                   points[i]._v);
+    if (d >= 0.0f) {
+      if (!got_dist || d < best_dist) {
+        best_dist = d;
+        got_dist = true;
+      }
+    }
+  }
+
+  float d = dist_to_line_segment(p, points[num_points - 1]._p, points[0]._p,
+                                 points[num_points - 1]._v);
+  if (d >= 0.0f) {
+    if (!got_dist || d < best_dist) {
+      best_dist = d;
+      got_dist = true;
+    }
+  }
+
+  return best_dist;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::dist_to_line_segment
+//       Access: Private, Static
+//  Description: Returns the linear distance of p to the line segment
+//               defined by f and t, where v = (t - f).normalize().
+//               The result is negative if p is left of the line,
+//               positive if it is right of the line.  If the result
+//               is positive, it is constrained by endpoints of the
+//               line segment (i.e. the result might be larger than it
+//               would be for a straight distance-to-line test).  If
+//               the result is negative, we don't bother.
+////////////////////////////////////////////////////////////////////
+float CollisionBox::
+dist_to_line_segment(const LPoint2f &p,
+                     const LPoint2f &f, const LPoint2f &t,
+                     const LVector2f &v) {
+  LVector2f v1 = (p - f);
+  float d = (v1[0] * v[1] - v1[1] * v[0]);
+  if (d < 0.0f) {
+    return d;
+  }
+
+  // Compute the nearest point on the line.
+  LPoint2f q = p + LVector2f(-v[1], v[0]) * d;
+
+  // Now constrain that point to the line segment.
+  if (v[0] > 0.0f) {
+    // X+
+    if (v[1] > 0.0f) {
+      // Y+
+      if (v[0] > v[1]) {
+        // X-dominant.
+        if (q[0] < f[0]) {
+          return (p - f).length();
+        } if (q[0] > t[0]) {
+          return (p - t).length();
+        } else {
+          return d;
+        }
+      } else {
+        // Y-dominant.
+        if (q[1] < f[1]) {
+          return (p - f).length();
+        } if (q[1] > t[1]) {
+          return (p - t).length();
+        } else {
+          return d;
+        }
+      }
+    } else {
+      // Y-
+      if (v[0] > -v[1]) {
+        // X-dominant.
+        if (q[0] < f[0]) {
+          return (p - f).length();
+        } if (q[0] > t[0]) {
+          return (p - t).length();
+        } else {
+          return d;
+        }
+      } else {
+        // Y-dominant.
+        if (q[1] > f[1]) {
+          return (p - f).length();
+        } if (q[1] < t[1]) {
+          return (p - t).length();
+        } else {
+          return d;
+        }
+      }
+    }
+  } else {
+    // X-
+    if (v[1] > 0.0f) {
+      // Y+
+      if (-v[0] > v[1]) {
+        // X-dominant.
+        if (q[0] > f[0]) {
+          return (p - f).length();
+        } if (q[0] < t[0]) {
+          return (p - t).length();
+        } else {
+          return d;
+        }
+      } else {
+        // Y-dominant.
+        if (q[1] < f[1]) {
+          return (p - f).length();
+        } if (q[1] > t[1]) {
+          return (p - t).length();
+        } else {
+          return d;
+        }
+      }
+    } else {
+      // Y-
+      if (-v[0] > -v[1]) {
+        // X-dominant.
+        if (q[0] > f[0]) {
+          return (p - f).length();
+        } if (q[0] < t[0]) {
+          return (p - t).length();
+        } else {
+          return d;
+        }
+      } else {
+        // Y-dominant.
+        if (q[1] > f[1]) {
+          return (p - f).length();
+        } if (q[1] < t[1]) {
+          return (p - t).length();
+        } else {
+          return d;
+        }
+      }
+    }
+  }
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::point_is_inside
+//       Access: Private
+//  Description: Returns true if the indicated point is within the
+//               polygon's 2-d space, false otherwise.
+////////////////////////////////////////////////////////////////////
+bool CollisionBox::
+point_is_inside(const LPoint2f &p, const CollisionBox::Points &points) const {
+  // We insist that the polygon be convex.  This makes things a bit simpler.
+  // In the case of a convex polygon, defined with points in counterclockwise
+  // order, a point is interior to the polygon iff the point is not right of
+  // each of the edges.
+  for (int i = 0; i < (int)points.size() - 1; i++) {
+    if (is_right(p - points[i]._p, points[i+1]._p - points[i]._p)) {
+      return false;
+    }
+  }
+  if (is_right(p - points[points.size() - 1]._p, 
+	  points[0]._p - points[points.size() - 1]._p)) {
+    return false;
+  }
+
+  return true;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::compute_point
+//       Access: Protected
+//  Description: Returns a point on the surface of the sphere.
+//               latitude and longitude range from 0.0 to 1.0.  This
+//               is used by fill_viz_geom() to create a visible
+//               representation of the sphere.
+////////////////////////////////////////////////////////////////////
+Vertexf CollisionBox::
+compute_point(float latitude, float longitude) const {
+  float s1, c1;
+  csincos(latitude * MathNumbers::pi_f, &s1, &c1);
+
+  float s2, c2;
+  csincos(longitude * 2.0f * MathNumbers::pi_f, &s2, &c2);
+
+  Vertexf p(s1 * c2, s1 * s2, c1);
+  return p * get_radius() + get_center();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::compute_vectors
+//       Access: Private, Static
+//  Description: Now that the _p members of the given points array
+//               have been computed, go back and compute all of the _v
+//               members.
+////////////////////////////////////////////////////////////////////
+void CollisionBox::
+compute_vectors(Points &points) {
+  size_t num_points = points.size();
+  for (size_t i = 0; i < num_points; i++) {
+    points[i]._v = points[(i + 1) % num_points]._p - points[i]._p;
+    points[i]._v.normalize();
+  }
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::register_with_read_factory
+//       Access: Public, Static
+//  Description: Factory method to generate a CollisionBox object
+////////////////////////////////////////////////////////////////////
+void CollisionBox::
+register_with_read_factory() {
+  BamReader::get_factory()->register_factory(get_class_type(), make_CollisionBox);
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::write_datagram
+//       Access: Public
+//  Description: Function to write the important information in
+//               the particular object to a Datagram
+////////////////////////////////////////////////////////////////////
+void CollisionBox::
+write_datagram(BamWriter *manager, Datagram &me) {
+  CollisionSolid::write_datagram(manager, me);
+  _center.write_datagram(me);
+  _min.write_datagram(me);
+  _max.write_datagram(me);
+  for(int i=0; i < 8; i++) {
+    _vertex[i].write_datagram(me);
+  }
+  me.add_float32(_radius);
+  me.add_float32(_x);
+  me.add_float32(_y);
+  me.add_float32(_z);
+  for(int i=0; i < 6; i++) {
+    _planes[i].write_datagram(me);
+  }
+  for(int i=0; i < 6; i++) {
+    _to_2d_mat[i].write_datagram(me);
+  }  
+  for(int i=0; i < 6; i++) {  
+    me.add_uint16(_points[i].size());
+    for (size_t j = 0; j < _points[i].size(); j++) {
+      _points[i][j]._p.write_datagram(me);
+      _points[i][j]._v.write_datagram(me);
+    }
+  }
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::make_CollisionBox
+//       Access: Protected
+//  Description: Factory method to generate a CollisionBox object
+////////////////////////////////////////////////////////////////////
+TypedWritable *CollisionBox::
+make_CollisionBox(const FactoryParams &params) {
+  CollisionBox *me = new CollisionBox;
+  DatagramIterator scan;
+  BamReader *manager;
+
+  parse_params(params, scan, manager);
+  me->fillin(scan, manager);
+  return me;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionBox::fillin
+//       Access: Protected
+//  Description: Function that reads out of the datagram (or asks
+//               manager to read) all of the data that is needed to
+//               re-create this object and stores it in the appropiate
+//               place
+////////////////////////////////////////////////////////////////////
+void CollisionBox::
+fillin(DatagramIterator& scan, BamReader* manager) {
+  CollisionSolid::fillin(scan, manager);
+  _center.read_datagram(scan);
+  _min.read_datagram(scan);
+  _max.read_datagram(scan);
+  for(int i=0; i < 8; i++) {
+    _vertex[i].read_datagram(scan);
+  }
+  _radius = scan.get_float32();
+  _x = scan.get_float32();
+  _y = scan.get_float32();
+  _z = scan.get_float32();
+  for(int i=0; i < 6; i++) {
+    _planes[i].read_datagram(scan);
+  }
+  for(int i=0; i < 6; i++) {
+    _to_2d_mat[i].read_datagram(scan);
+  }
+  for(int i=0; i < 6; i++) {
+    size_t size = scan.get_uint16();
+    for (size_t j = 0; j < size; j++) {
+      LPoint2f p;
+      LVector2f v;
+      p.read_datagram(scan);
+      v.read_datagram(scan);
+      _points[i].push_back(PointDef(p, v));
+    }
+  }
+}

panda/src/collide/collisionBox.h

@@ -0,0 +1,175 @@
+// Filename: collisionBox.h
+// Created by:  amith tudur( 31Jul09 )
+//
+////////////////////////////////////////////////////////////////////
+//
+// PANDA 3D SOFTWARE
+// Copyright (c) Carnegie Mellon University.  All rights reserved.
+//
+// All use of this software is subject to the terms of the revised BSD
+// license.  You should have received a copy of this license along
+// with this source code in a file named "LICENSE."
+//
+////////////////////////////////////////////////////////////////////
+
+#ifndef COLLISIONBOX_H
+#define COLLISIONBOX_H
+
+#include "pandabase.h"
+#include "collisionSolid.h"
+#include "parabola.h"
+#include "plane.h"
+#include "look_at.h"
+#include "clipPlaneAttrib.h"
+
+////////////////////////////////////////////////////////////////////
+//       Class : CollisionBox
+// Description : A cuboid collision volume or object.
+////////////////////////////////////////////////////////////////////
+class EXPCL_PANDA_COLLIDE CollisionBox : public CollisionSolid {
+PUBLISHED:
+  INLINE CollisionBox(const LPoint3f &center, 
+					   float x, float y, float z);
+  INLINE CollisionBox(float cx, float cy,	float cz,
+        float x,  float y,	float z);
+  INLINE CollisionBox(const LPoint3f &min, const LPoint3f &max);
+
+  virtual LPoint3f get_collision_origin() const;
+
+protected:
+  INLINE CollisionBox();
+
+public:
+  INLINE CollisionBox(const CollisionBox &copy);
+  virtual CollisionSolid *make_copy();
+
+  virtual PT(CollisionEntry)
+  test_intersection(const CollisionEntry &entry) const;
+  virtual void xform(const LMatrix4f &mat);
+
+  virtual PStatCollector &get_volume_pcollector();
+  virtual PStatCollector &get_test_pcollector();
+
+  virtual void output(ostream &out) const;
+  
+  virtual LPoint3f get_approx_center() const;
+  virtual LPoint3f get_min() const;
+  virtual LPoint3f get_max() const;
+
+  INLINE static void flush_level();
+  void setup_box();
+
+PUBLISHED:
+  INLINE_MATHUTIL int get_num_points() const;
+  INLINE_MATHUTIL LPoint3f get_point_aabb(int n) const;
+  INLINE_MATHUTIL LPoint3f get_point(int n) const;
+  INLINE_MATHUTIL int get_num_planes() const;
+  INLINE_MATHUTIL Planef set_plane(int n) const;
+  INLINE_MATHUTIL Planef get_plane(int n) const;
+  INLINE void set_center(const LPoint3f &center);
+  INLINE void set_center(float x, float y, float z);
+  INLINE const LPoint3f &get_center() const;
+  INLINE float get_radius() const;
+
+protected:
+  virtual PT(BoundingVolume) compute_internal_bounds() const;
+  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_box(const CollisionEntry &entry) const;
+  
+  virtual void fill_viz_geom();
+
+protected:
+  Vertexf compute_point(float latitude, float longitude) const;
+
+private:
+  LPoint3f _center;
+  LPoint3f _min;
+  LPoint3f _max;
+  float _x, _y, _z, _radius;
+  LPoint3f _vertex[8]; // Each of the Eight Vertices of the Box
+  Planef _planes[6]; //Points to each of the six sides of the Box
+  
+  static const int plane_def[6][4];
+  
+  static PStatCollector _volume_pcollector;
+  static PStatCollector _test_pcollector;
+
+private:
+  INLINE static bool is_right(const LVector2f &v1, const LVector2f &v2);
+  INLINE static float dist_to_line(const LPoint2f &p,
+                                   const LPoint2f &f, const LVector2f &v);
+  static float dist_to_line_segment(const LPoint2f &p,
+                                    const LPoint2f &f, const LPoint2f &t,
+                                    const LVector2f &v);
+  
+public:
+  class PointDef {
+  public:
+    INLINE PointDef(const LPoint2f &p, const LVector2f &v);
+    INLINE PointDef(float x, float y);
+    INLINE PointDef(const PointDef &copy);
+    INLINE void operator = (const PointDef &copy);
+
+    LPoint2f _p;  // the point in 2-d space
+    LVector2f _v; // the normalized vector to the next point
+  };
+  typedef pvector<PointDef> Points;
+
+  static void compute_vectors(Points &points);
+  void draw_polygon(GeomNode *viz_geom_node, GeomNode *bounds_viz_geom_node,
+                    const Points &points) const;
+
+  bool point_is_inside(const LPoint2f &p, const Points &points) const;
+  float dist_to_polygon(const LPoint2f &p, const Points &points) const;
+
+  void setup_points(const LPoint3f *begin, const LPoint3f *end, int plane);
+  INLINE LPoint2f to_2d(const LVecBase3f &point3d, int plane) const;
+  INLINE void calc_to_3d_mat(LMatrix4f &to_3d_mat, int plane) const;
+  INLINE void rederive_to_3d_mat(LMatrix4f &to_3d_mat, int plane) const;
+  INLINE static LPoint3f to_3d(const LVecBase2f &point2d, const LMatrix4f &to_3d_mat);
+  LPoint3f legacy_to_3d(const LVecBase2f &point2d, int axis) const;
+  bool clip_polygon(Points &new_points, const Points &source_points,
+                    const Planef &plane,int plane_no) const;
+  bool apply_clip_plane(Points &new_points, const ClipPlaneAttrib *cpa,
+                        const TransformState *net_transform, int plane_no) const;
+
+private:
+  Points _points[6]; // one set of points for each of the six planes that make up the box
+  LMatrix4f _to_2d_mat[6]; 
+
+public:
+  INLINE Points get_plane_points( int n );
+
+public:
+  static void register_with_read_factory();
+  virtual void write_datagram(BamWriter *manager, Datagram &me);
+
+protected:
+  static TypedWritable *make_CollisionBox(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, "CollisionBox",
+                  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 "collisionBox.I"
+
+#endif /* COLLISIONBOX_H */

panda/src/collide/collisionSolid.cxx

@@ -19,6 +19,7 @@
 #include "collisionRay.h"
 #include "collisionSegment.h"
 #include "collisionParabola.h"
+#include "collisionBox.h"
 #include "collisionEntry.h"
 #include "boundingSphere.h"
 #include "datagram.h"
@@ -309,6 +310,21 @@ test_intersection_from_parabola(const CollisionEntry &) const {
   return NULL;
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionSolid::test_intersection_from_box
+//       Access: Protected, Virtual
+//  Description: This is part of the double-dispatch implementation of
+//               test_intersection().  It is called when the "from"
+//               object is a box.
+////////////////////////////////////////////////////////////////////
+PT(CollisionEntry) CollisionSolid::
+test_intersection_from_box(const CollisionEntry &) const {
+  report_undefined_intersection_test(CollisionBox::get_class_type(),
+                                     get_type());
+  return NULL;
+}
+
+
 #ifndef NDEBUG
 class CollisionSolidUndefinedPair {
 public:

panda/src/collide/collisionSolid.h

@@ -110,6 +110,8 @@ protected:
   test_intersection_from_segment(const CollisionEntry &entry) const;
   virtual PT(CollisionEntry)
   test_intersection_from_parabola(const CollisionEntry &entry) const;
+  virtual PT(CollisionEntry)
+  test_intersection_from_box(const CollisionEntry &entry) const;
 
   static void report_undefined_intersection_test(TypeHandle from_type,
                                                  TypeHandle into_type);
@@ -178,6 +180,7 @@ private:
   friend class CollisionSegment;
   friend class CollisionParabola;
   friend class CollisionHandlerFluidPusher;
+  friend class CollisionBox;
 };
 
 INLINE ostream &operator << (ostream &out, const CollisionSolid &cs) {

panda/src/collide/collisionSphere.cxx

@@ -456,6 +456,132 @@ test_intersection_from_line(const CollisionEntry &entry) const {
   return new_entry;
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionSphere::test_intersection_from_box
+//       Access: Public, Virtual
+//  Description: Double dispatch point for box as a FROM object
+////////////////////////////////////////////////////////////////////
+PT(CollisionEntry) CollisionSphere::
+test_intersection_from_box(const CollisionEntry &entry) const {
+  const CollisionBox *box;
+  DCAST_INTO_R(box, entry.get_from(), 0);
+
+  CPT(TransformState) wrt_space = entry.get_wrt_space();
+  CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
+
+  const LMatrix4f &wrt_mat = wrt_space->get_mat();
+
+  CollisionBox local_b( *box );
+  local_b.xform( wrt_mat );
+
+  LPoint3f from_center = local_b.get_center();
+
+  LPoint3f orig_center = get_center();
+  LPoint3f to_center = orig_center;
+  bool moved_from_center = false;
+  float t = 1.0f;
+  LPoint3f contact_point(from_center);
+  float actual_t = 1.0f;
+
+  float to_radius = get_radius();
+  float to_radius_2 = to_radius * to_radius;
+
+  int ip;
+  float max_dist,dist;
+  bool intersect;
+  Planef plane;
+  LVector3f normal;
+
+  for( ip = 0, intersect=false; ip < 6 && !intersect; ip++ ){
+    plane = local_b.get_plane( ip );
+    if (local_b.get_plane_points(ip).size() < 3) {
+      continue;
+    }
+    normal = (has_effective_normal() && box->get_respect_effective_normal()) ? get_effective_normal() : plane.get_normal();
+    
+    #ifndef NDEBUG
+    /*
+    if (!IS_THRESHOLD_EQUAL(normal.length_squared(), 1.0f, 0.001), NULL) {
+      collide_cat.info()
+      << "polygon being collided with " << entry.get_into_node_path()
+      << " has normal " << normal << " of length " << normal.length()
+      << "\n";
+      normal.normalize();
+    }
+    */
+    #endif
+
+    // The nearest point within the plane to our center is the
+    // intersection of the line (center, center - normal) with the plane.
+
+    if (!plane.intersects_line(dist, to_center, -(plane.get_normal()))) {
+      // No intersection with plane?  This means the plane's effective
+      // normal was within the plane itself.  A useless polygon.
+      continue;
+    }
+
+    if (dist > to_radius || dist < -to_radius) {
+      // No intersection with the plane.
+      continue;
+    }
+
+    LPoint2f p = local_b.to_2d(to_center - dist * plane.get_normal(), ip);
+    float edge_dist = 0.0f;
+
+    edge_dist = local_b.dist_to_polygon(p, local_b.get_plane_points(ip));
+
+    if(edge_dist < 0) {
+      intersect = true;
+      continue;
+    }
+
+    if((edge_dist > 0) && 
+      ((edge_dist * edge_dist + dist * dist) > to_radius_2)) {
+      // No intersection; the circle is outside the polygon.
+      continue;
+    }
+
+    // The sphere appears to intersect the polygon.  If the edge is less
+    // than to_radius away, the sphere may be resting on an edge of
+    // the polygon.  Determine how far the center of the sphere must
+    // remain from the plane, based on its distance from the nearest
+    // edge.
+
+    max_dist = to_radius;
+    if (edge_dist >= 0.0f) {
+      float max_dist_2 = max(to_radius_2 - edge_dist * edge_dist, 0.0f);
+      max_dist = csqrt(max_dist_2);
+    }
+
+    if (dist > max_dist) {
+      // There's no intersection: the sphere is hanging off the edge.
+      continue;
+    }
+    intersect = true;
+  }
+  if( !intersect )
+    return NULL;
+
+  if (collide_cat.is_debug()) {
+    collide_cat.debug()
+      << "intersection detected from " << entry.get_from_node_path()
+      << " into " << entry.get_into_node_path() << "\n";
+  }
+
+  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
+
+  float into_depth = max_dist - dist;
+
+  new_entry->set_surface_normal(normal);
+  new_entry->set_surface_point(to_center - normal * dist);
+  new_entry->set_interior_point(to_center - normal * (dist + into_depth));
+  new_entry->set_contact_pos(contact_point);
+  new_entry->set_contact_normal(plane.get_normal());
+  new_entry->set_t(actual_t);
+
+  return new_entry;
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionSphere::test_intersection_from_ray
 //       Access: Public, Virtual

panda/src/collide/collisionSphere.h

@@ -18,6 +18,7 @@
 #include "pandabase.h"
 #include "collisionSolid.h"
 #include "parabola.h"
+#include "collisionBox.h"
 
 ////////////////////////////////////////////////////////////////////
 //       Class : CollisionSphere
@@ -72,6 +73,8 @@ protected:
   test_intersection_from_segment(const CollisionEntry &entry) const;
   virtual PT(CollisionEntry)
   test_intersection_from_parabola(const CollisionEntry &entry) const;
+  virtual PT(CollisionEntry)
+  test_intersection_from_box(const CollisionEntry &entry) const;
 
   virtual void fill_viz_geom();
 

panda/src/collide/collisionTraverser.cxx

@@ -20,6 +20,7 @@
 #include "collisionRecorder.h"
 #include "collisionVisualizer.h"
 #include "collisionSphere.h"
+#include "collisionBox.h"
 #include "collisionTube.h"
 #include "collisionPolygon.h"
 #include "collisionPlane.h"
@@ -370,6 +371,7 @@ traverse(const NodePath &root) {
   CollisionTube::flush_level();
   CollisionPolygon::flush_level();
   CollisionPlane::flush_level();
+  CollisionBox::flush_level();
 }
 
 #ifdef DO_COLLISION_RECORDING

panda/src/collide/config_collide.cxx

@@ -39,6 +39,7 @@
 #include "collisionSphere.h"
 #include "collisionTraverser.h"
 #include "collisionTube.h"
+#include "collisionBox.h"
 #include "collisionVisualizer.h"
 #include "dconfig.h"
 
@@ -142,6 +143,7 @@ init_libcollide() {
   CollisionSphere::init_type();
   CollisionTraverser::init_type();
   CollisionTube::init_type();
+  CollisionBox::init_type();
 
 #ifdef DO_COLLISION_RECORDING
   CollisionRecorder::init_type();
@@ -160,4 +162,5 @@ init_libcollide() {
   CollisionSegment::register_with_read_factory();
   CollisionSphere::register_with_read_factory();
   CollisionTube::register_with_read_factory();
+  CollisionBox::register_with_read_factory();
 }