From b39fa23b8e39a290e1a08ec9867295d1cab77263 Mon Sep 17 00:00:00 2001
From: David Rose <david@ddrose.com>
Date: Tue, 11 Jun 2002 23:47:25 +0000
Subject: [PATCH] support velocity for collisions

---
 panda/src/collide/collisionEntry.I           | 37 ++++++++++
 panda/src/collide/collisionEntry.h           |  6 ++
 panda/src/collide/collisionHandlerPusher.cxx |  2 +-
 panda/src/collide/collisionLevelState.cxx    | 14 ++++
 panda/src/collide/collisionNode.I            | 66 +++++++++++++----
 panda/src/collide/collisionNode.cxx          | 78 +++++++++++++++++++-
 panda/src/collide/collisionNode.h            | 16 +++-
 panda/src/collide/collisionPolygon.cxx       | 50 ++++++++++++-
 panda/src/collide/collisionTraverser.cxx     | 15 ++++
 panda/src/collide/config_collide.cxx         |  5 ++
 panda/src/collide/config_collide.h           |  2 +
 panda/src/pgraph/pandaNode.cxx               | 19 +++++
 panda/src/pgraph/pandaNode.h                 |  1 +
 panda/src/tform/driveInterface.cxx           | 10 ++-
 panda/src/tform/driveInterface.h             |  3 +
 panda/src/tform/transform2sg.cxx             | 42 ++++++++++-
 panda/src/tform/transform2sg.h               |  5 ++
 17 files changed, 348 insertions(+), 23 deletions(-)

diff --git a/panda/src/collide/collisionEntry.I b/panda/src/collide/collisionEntry.I
index 9ab3026cb4..f0ca2dc0e3 100644
--- a/panda/src/collide/collisionEntry.I
+++ b/panda/src/collide/collisionEntry.I
@@ -156,6 +156,43 @@ get_inv_wrt_space() const {
   return _inv_wrt_space;
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionEntry::set_from_velocity
+//       Access: Public
+//  Description: Indicates the velocity associated with the "from"
+//               object, in the object's coordinate space.
+////////////////////////////////////////////////////////////////////
+INLINE void CollisionEntry::
+set_from_velocity(const LVector3f &vel) {
+  _from_velocity = vel;
+  _flags |= F_has_from_velocity;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionEntry::has_from_velocity
+//       Access: Public
+//  Description: Returns true if the "from" object had an indicated
+//               velocity, false otherwise.
+////////////////////////////////////////////////////////////////////
+INLINE bool CollisionEntry::
+has_from_velocity() const {
+  return (_flags & F_has_from_velocity) != 0;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionEntry::get_from_velocity
+//       Access: Public
+//  Description: Returns the instantaneous velocity of the "from"
+//               object, in its own coordinate space.  This represents
+//               the delta between its current position and its
+//               position last frame.
+////////////////////////////////////////////////////////////////////
+INLINE const LVector3f &CollisionEntry::
+get_from_velocity() const {
+  nassertr(has_from_velocity(), _from_velocity);
+  return _from_velocity;
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionEntry::set_into_intersection_point
 //       Access: Public
diff --git a/panda/src/collide/collisionEntry.h b/panda/src/collide/collisionEntry.h
index 8e45cc7fa1..325e508c0e 100644
--- a/panda/src/collide/collisionEntry.h
+++ b/panda/src/collide/collisionEntry.h
@@ -63,6 +63,10 @@ PUBLISHED:
   INLINE const LMatrix4f &get_wrt_space() const;
   INLINE const LMatrix4f &get_inv_wrt_space() const;
 
+  INLINE void set_from_velocity(const LVector3f &vel);
+  INLINE bool has_from_velocity() const;
+  INLINE const LVector3f &get_from_velocity() const;
+
   INLINE void set_into_intersection_point(const LPoint3f &point);
   INLINE bool has_into_intersection_point() const;
   INLINE const LPoint3f &get_into_intersection_point() const;
@@ -94,10 +98,12 @@ private:
     F_has_into_intersection_point = 0x0001,
     F_has_into_surface_normal     = 0x0002,
     F_has_into_depth              = 0x0004,
+    F_has_from_velocity           = 0x0008,
   };
 
   int _flags;
 
+  LVector3f _from_velocity;
   LPoint3f _into_intersection_point;
   LVector3f _into_surface_normal;
   float _into_depth;
diff --git a/panda/src/collide/collisionHandlerPusher.cxx b/panda/src/collide/collisionHandlerPusher.cxx
index 72471e5244..1a83088cb5 100644
--- a/panda/src/collide/collisionHandlerPusher.cxx
+++ b/panda/src/collide/collisionHandlerPusher.cxx
@@ -132,7 +132,7 @@ handle_entries() {
                 normal[2] = 0.0f;
               }
               // Just to be on the safe size, we normalize the normal
-              // vector, even though it really out to be unit-length
+              // vector, even though it really ought to be unit-length
               // already (unless we just forced it horizontal, above).
               normal.normalize();
 
diff --git a/panda/src/collide/collisionLevelState.cxx b/panda/src/collide/collisionLevelState.cxx
index 9e1826ec00..ca5d1680c3 100644
--- a/panda/src/collide/collisionLevelState.cxx
+++ b/panda/src/collide/collisionLevelState.cxx
@@ -63,6 +63,20 @@ prepare_collider(const ColliderDef &def) {
   } else {
     GeometricBoundingVolume *gbv;
     DCAST_INTO_V(gbv, bv.make_copy());
+
+    if (def._node->has_velocity()) {
+      // If the node has a velocity, we have to include the starting
+      // point in the volume as well.
+
+      // Strictly speaking, we should actually transform gbv backward
+      // by get_velocity(), and extend by *that* volume, instead of
+      // just extending by the single point at -get_velocity().
+      // However, assuming the solids within a moving CollisionNode
+      // tend to be near the origin, this will generally produce the
+      // same results, and is much easier to compute.
+      gbv->extend_by(LPoint3f(-def._node->get_velocity()));
+    }
+
     gbv->xform(def._space);
     _local_bounds.push_back(gbv);
   }
diff --git a/panda/src/collide/collisionNode.I b/panda/src/collide/collisionNode.I
index 16abb08199..6ab58baca5 100644
--- a/panda/src/collide/collisionNode.I
+++ b/panda/src/collide/collisionNode.I
@@ -19,7 +19,7 @@
 
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionNode::set_collide_mask
-//       Access: Public
+//       Access: Published
 //  Description: Simultaneously sets both the "from" and "into"
 //               CollideMask values to the same thing.
 ////////////////////////////////////////////////////////////////////
@@ -31,7 +31,7 @@ set_collide_mask(CollideMask mask) {
 
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionNode::set_from_collide_mask
-//       Access: Public
+//       Access: Published
 //  Description: Sets the "from" CollideMask.  In order for a
 //               collision to be detected from this object into
 //               another object, the intersection of this object's
@@ -45,7 +45,7 @@ set_from_collide_mask(CollideMask mask) {
 
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionNode::set_into_collide_mask
-//       Access: Public
+//       Access: Published
 //  Description: Sets the "into" CollideMask.  In order for a
 //               collision to be detected from another object into
 //               this object, the intersection of the other object's
@@ -64,7 +64,7 @@ set_into_collide_mask(CollideMask mask) {
 
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionNode::get_from_collide_mask
-//       Access: Public
+//       Access: Published
 //  Description: Returns the current "from" CollideMask.  In order for
 //               a collision to be detected from this object into
 //               another object, the intersection of this object's
@@ -78,7 +78,7 @@ get_from_collide_mask() const {
 
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionNode::get_into_collide_mask
-//       Access: Public
+//       Access: Published
 //  Description: Returns the current "into" CollideMask.  In order for
 //               a collision to be detected from another object into
 //               this object, the intersection of the other object's
@@ -92,7 +92,7 @@ get_into_collide_mask() const {
 
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionNode::set_collide_geom
-//       Access: Public
+//       Access: Published
 //  Description: Sets the state of the "collide geom" flag for this
 //               CollisionNode.  Normally, this is false; when this is
 //               set true, the CollisionSolids in this node will test
@@ -107,23 +107,27 @@ get_into_collide_mask() const {
 ////////////////////////////////////////////////////////////////////
 INLINE void CollisionNode::
 set_collide_geom(bool flag) {
-  _collide_geom = flag;
+  if (flag) {
+    _flags |= F_collide_geom;
+  } else {
+    _flags &= ~F_collide_geom;
+  }
 }
 
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionNode::get_collide_geom
-//       Access: Public
+//       Access: Published
 //  Description: Returns the current state of the collide_geom flag.
 //               See set_collide_geom().
 ////////////////////////////////////////////////////////////////////
 INLINE bool CollisionNode::
 get_collide_geom() const {
-  return _collide_geom;
+  return (_flags & F_collide_geom) != 0;
 }
 
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionNode::get_num_solids
-//       Access: Public
+//       Access: Published
 //  Description:
 ////////////////////////////////////////////////////////////////////
 INLINE int CollisionNode::
@@ -133,7 +137,7 @@ get_num_solids() const {
 
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionNode::get_solid
-//       Access: Public
+//       Access: Published
 //  Description:
 ////////////////////////////////////////////////////////////////////
 INLINE CollisionSolid *CollisionNode::
@@ -144,7 +148,7 @@ get_solid(int n) const {
 
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionNode::remove_solid
-//       Access: Public
+//       Access: Published
 //  Description: Removes the solid with the indicated index.  This
 //               will shift all subsequent indices down by one.
 ////////////////////////////////////////////////////////////////////
@@ -157,7 +161,7 @@ remove_solid(int n) {
 
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionNode::add_solid
-//       Access: Public
+//       Access: Published
 //  Description: Adds the indicated solid to the node.  Returns the
 //               index of the new solid within the node's list of
 //               solids.
@@ -168,3 +172,39 @@ add_solid(CollisionSolid *solid) {
   mark_bound_stale();
   return _solids.size() - 1;
 }
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionNode::clear_velocity
+//       Access: Published
+//  Description: Removes the velocity information associated with the
+//               node.  See set_velocity().
+////////////////////////////////////////////////////////////////////
+INLINE void CollisionNode::
+clear_velocity() {
+  _flags &= ~F_has_velocity;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionNode::has_velocity
+//       Access: Published
+//  Description: Returns true if the node has an associated velocity,
+//               false otherwise.  See set_velocity().
+////////////////////////////////////////////////////////////////////
+INLINE bool CollisionNode::
+has_velocity() const {
+  return (_flags & F_has_velocity) != 0;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionNode::get_velocity
+//       Access: Published
+//  Description: Returns the instantaneous velocity of the node, in
+//               its own coordinate space.  This represents the delta
+//               between its current position and its position last
+//               frame.  See set_velocity().
+////////////////////////////////////////////////////////////////////
+INLINE const LVector3f &CollisionNode::
+get_velocity() const {
+  nassertr(has_velocity(), _velocity);
+  return _velocity;
+}
diff --git a/panda/src/collide/collisionNode.cxx b/panda/src/collide/collisionNode.cxx
index f0f35497cb..edfd79c21c 100644
--- a/panda/src/collide/collisionNode.cxx
+++ b/panda/src/collide/collisionNode.cxx
@@ -22,6 +22,10 @@
 #include "geomNode.h"
 #include "cullTraverserData.h"
 #include "cullTraverser.h"
+#include "renderState.h"
+#include "transformState.h"
+#include "colorScaleAttrib.h"
+#include "transparencyAttrib.h"
 #include "datagram.h"
 #include "datagramIterator.h"
 #include "bamReader.h"
@@ -40,7 +44,8 @@ CollisionNode(const string &name) :
   PandaNode(name),
   _from_collide_mask(CollideMask::all_on()),
   _into_collide_mask(CollideMask::all_on()),
-  _collide_geom(false)
+  _velocity(0.0f, 0.0f, 0.0f),
+  _flags(0)
 {
   // CollisionNodes are hidden by default.
   set_draw_mask(DrawMask::all_off());
@@ -56,7 +61,8 @@ CollisionNode(const CollisionNode &copy) :
   PandaNode(copy),
   _from_collide_mask(copy._from_collide_mask),
   _into_collide_mask(copy._into_collide_mask),
-  _collide_geom(copy._collide_geom),
+  _velocity(copy._velocity),
+  _flags(copy._flags),
   _solids(copy._solids)
 {
 }
@@ -212,6 +218,22 @@ cull_callback(CullTraverser *trav, CullTraverserData &data) {
     trav->traverse(next_data);
   }
 
+  if (has_velocity()) {
+    // If we have a velocity, also draw the previous frame's position,
+    // ghosted.
+    CPT(TransformState) transform = TransformState::make_pos(-get_velocity());
+    for (si = _solids.begin(); si != _solids.end(); ++si) {
+      CollisionSolid *solid = (*si);
+      PandaNode *node = solid->get_viz();
+      CullTraverserData next_data(data, node);
+
+      next_data._render_transform = 
+        next_data._render_transform->compose(transform);
+      next_data._state = get_last_pos_state();
+      trav->traverse(next_data);
+    }
+  }
+
   // Now carry on to render our child nodes.
   return true;
 }
@@ -232,6 +254,31 @@ output(ostream &out) const {
 }
 
 
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionNode::set_velocity
+//       Access: Published, Virtual
+//  Description: Indicates the instantaneous velocity of the node.
+//               This is only meaningful for nodes that represent
+//               "colliders", that is, nodes added to a
+//               CollisionTraverser to be tested for collision into
+//               other objects.
+//
+//               The velocity vector represents the delta from this
+//               node's position last frame to its current position.
+//               The collision traverser automatically clears it after
+//               it has performed the traversal.
+//
+//               This velocity information is optional and, if
+//               available, is used by the collision traverser to help
+//               determine which walls the collider is actually
+//               intersecting with, and which it is simply passing by.
+////////////////////////////////////////////////////////////////////
+void CollisionNode::
+set_velocity(const LVector3f &vel) {
+  _velocity = vel;
+  _flags |= F_has_velocity;
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionNode::recompute_bound
 //       Access: Protected, Virtual
@@ -293,6 +340,29 @@ recompute_internal_bound() {
   return bound;
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: CollisionNode::get_last_pos_state
+//       Access: Protected
+//  Description: Returns a RenderState for rendering the ghosted
+//               collision solid that represents the previous frame's
+//               position, for those collision nodes that indicate a
+//               velocity.
+////////////////////////////////////////////////////////////////////
+CPT(RenderState) CollisionNode::
+get_last_pos_state() {
+  // Once someone asks for this pointer, we hold its reference count
+  // and never free it.
+  static CPT(RenderState) state = (const RenderState *)NULL;
+  if (state == (const RenderState *)NULL) {
+    state = RenderState::make
+      (ColorScaleAttrib::make(LVecBase4f(1.0f, 1.0f, 1.0f, 0.5f)),
+       TransparencyAttrib::make(TransparencyAttrib::M_alpha));
+  }
+
+  return state;
+}
+
+
 ////////////////////////////////////////////////////////////////////
 //     Function: CollisionNode::register_with_read_factory
 //       Access: Public, Static
@@ -322,7 +392,7 @@ write_datagram(BamWriter *manager, Datagram &dg) {
 
   dg.add_uint32(_from_collide_mask.get_word());
   dg.add_uint32(_into_collide_mask.get_word());
-  dg.add_bool(_collide_geom);
+  dg.add_uint8(_flags);
 }
 
 ////////////////////////////////////////////////////////////////////
@@ -387,5 +457,5 @@ fillin(DatagramIterator &scan, BamReader *manager) {
 
   _from_collide_mask.set_word(scan.get_uint32());
   _into_collide_mask.set_word(scan.get_uint32());
-  _collide_geom = scan.get_bool();
+  _flags = scan.get_uint8();
 }
diff --git a/panda/src/collide/collisionNode.h b/panda/src/collide/collisionNode.h
index cf1ae4cdbf..ac07806bf4 100644
--- a/panda/src/collide/collisionNode.h
+++ b/panda/src/collide/collisionNode.h
@@ -68,17 +68,31 @@ PUBLISHED:
   INLINE void remove_solid(int n);
   INLINE int add_solid(CollisionSolid *solid);
 
+  virtual void set_velocity(const LVector3f &vel);
+  INLINE void clear_velocity();
+  INLINE bool has_velocity() const;
+  INLINE const LVector3f &get_velocity() const;
+
 protected:
   virtual BoundingVolume *recompute_bound();
   virtual BoundingVolume *recompute_internal_bound();
 
 private:
+  CPT(RenderState) get_last_pos_state();
+
   // This data is not cycled, for now.  We assume the collision
   // traversal will take place in App only.  Perhaps we will revisit
   // this later.
   CollideMask _from_collide_mask;
   CollideMask _into_collide_mask;
-  bool _collide_geom;
+  LVector3f _velocity;
+
+  enum Flags {
+    F_collide_geom = 0x0001,
+    F_has_velocity = 0x0002,
+    // Presently only 8 bits are written to the bam file.
+  };
+  int _flags;
 
   typedef pvector< PT(CollisionSolid) > Solids;
   Solids _solids;
diff --git a/panda/src/collide/collisionPolygon.cxx b/panda/src/collide/collisionPolygon.cxx
index 787aecfe82..e9d71b50de 100644
--- a/panda/src/collide/collisionPolygon.cxx
+++ b/panda/src/collide/collisionPolygon.cxx
@@ -240,7 +240,48 @@ test_intersection_from_sphere(CollisionHandler *record,
   const CollisionSphere *sphere;
   DCAST_INTO_R(sphere, entry.get_from(), 0);
 
-  LPoint3f from_center = sphere->get_center() * entry.get_wrt_space();
+  LPoint3f orig_center = sphere->get_center() * entry.get_wrt_space();
+  LPoint3f from_center = orig_center;
+  bool moved_from_center = false;
+
+  if (entry.has_from_velocity()) {
+    // If we have a velocity indication, we use that to determine some
+    // more properties of the collision.
+    LPoint3f b = from_center;
+    LPoint3f a = (sphere->get_center() - entry.get_from_velocity()) * entry.get_wrt_space();
+
+    LVector3f delta = b - a;
+    // First, there is no collision if the "from" object is moving in
+    // the same direction as the plane's normal.
+    float dot = delta.dot(get_normal());
+    if (dot > 0.0f) {
+      return 0;
+    }
+
+    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.
+      float t = -(dist_to_plane(a) / dot);
+      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;
+      }
+    }
+  }
+
   LVector3f from_radius_v =
     LVector3f(sphere->get_radius(), 0.0f, 0.0f) * entry.get_wrt_space();
   float from_radius = length(from_radius_v);
@@ -325,9 +366,16 @@ test_intersection_from_sphere(CollisionHandler *record,
 
   LVector3f into_normal = get_normal() * entry.get_inv_wrt_space();
   float into_depth = from_radius - 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.
+    into_depth = from_radius - dist_to_plane(orig_center);
+  }
 
   new_entry->set_into_surface_normal(into_normal);
   new_entry->set_into_depth(into_depth);
+  new_entry->set_into_intersection_point(from_center);
 
   record->add_entry(new_entry);
   return 1;
diff --git a/panda/src/collide/collisionTraverser.cxx b/panda/src/collide/collisionTraverser.cxx
index a1a581356b..b93164e1a1 100644
--- a/panda/src/collide/collisionTraverser.cxx
+++ b/panda/src/collide/collisionTraverser.cxx
@@ -251,6 +251,17 @@ traverse(const NodePath &root) {
       ++hi;
     }
   }
+
+  if (auto_clear_velocity) {
+    // Clear all the velocities for next time.
+    OrderedColliders::iterator ci;
+    for (ci = _ordered_colliders.begin(); 
+         ci != _ordered_colliders.end(); 
+         ++ci) {
+      CollisionNode *node = (*ci);
+      node->clear_velocity();
+    }
+  }
 }
 
 ////////////////////////////////////////////////////////////////////
@@ -373,6 +384,10 @@ r_traverse(CollisionLevelState &level_state) {
           entry._from = level_state.get_collider(c);
           entry._from_space = level_state.get_space(c);
 
+          if (entry._from_node->has_velocity()) {
+            entry.set_from_velocity(entry._from_node->get_velocity());
+          }
+
           NodePath root;
           const LMatrix4f &into_space_inv = 
             root.get_mat(entry._into_node_path);
diff --git a/panda/src/collide/config_collide.cxx b/panda/src/collide/config_collide.cxx
index c6722dd712..10f91be8f2 100644
--- a/panda/src/collide/config_collide.cxx
+++ b/panda/src/collide/config_collide.cxx
@@ -40,6 +40,11 @@ ConfigureFn(config_collide) {
   init_libcollide();
 }
 
+// Set this false to stop the CollisionTraverser from automatically
+// clearing the velocity associated with its CollisionNodes after it
+// has traversed them.  Mainly useful for visualizing the velocities.
+const bool auto_clear_velocity = config_collide.GetBool("auto-clear-velocity", true);
+
 ////////////////////////////////////////////////////////////////////
 //     Function: init_libcollide
 //  Description: Initializes the library.  This must be called at
diff --git a/panda/src/collide/config_collide.h b/panda/src/collide/config_collide.h
index 88363f743b..3541a770f8 100644
--- a/panda/src/collide/config_collide.h
+++ b/panda/src/collide/config_collide.h
@@ -24,6 +24,8 @@
 
 NotifyCategoryDecl(collide, EXPCL_PANDA, EXPTP_PANDA);
 
+extern const bool auto_clear_velocity;
+
 extern EXPCL_PANDA void init_libcollide();
 
 #endif
diff --git a/panda/src/pgraph/pandaNode.cxx b/panda/src/pgraph/pandaNode.cxx
index f4c2e4f9ee..076fc68180 100644
--- a/panda/src/pgraph/pandaNode.cxx
+++ b/panda/src/pgraph/pandaNode.cxx
@@ -1167,6 +1167,25 @@ as_light() {
   return NULL;
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: PandaNode::set_velocity
+//       Access: Public, Virtual
+//  Description: Indicates the instantaneous velocity of this node.
+//               This function is meaningless to most kinds of nodes;
+//               it is implemented only for CollisionNodes and is
+//               intended to inform the collision system of velocity.
+//
+//               It is defined at this level only as an abstract
+//               interface to allow setting the velocity of a
+//               collision node without having to link with, or know
+//               anything about, the collision system.
+//
+//               See CollisionNode::set_velocity().
+////////////////////////////////////////////////////////////////////
+void PandaNode::
+set_velocity(const LVector3f &) {
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: PandaNode::propagate_stale_bound
 //       Access: Protected, Virtual
diff --git a/panda/src/pgraph/pandaNode.h b/panda/src/pgraph/pandaNode.h
index 45c876d8c1..113e49db59 100644
--- a/panda/src/pgraph/pandaNode.h
+++ b/panda/src/pgraph/pandaNode.h
@@ -166,6 +166,7 @@ PUBLISHED:
 public:
   virtual bool is_geom_node() const;
   virtual Light *as_light();
+  virtual void set_velocity(const LVector3f &vel);
 
 protected:
   // Inherited from BoundedObject
diff --git a/panda/src/tform/driveInterface.cxx b/panda/src/tform/driveInterface.cxx
index ccc49d65ac..0752f73408 100644
--- a/panda/src/tform/driveInterface.cxx
+++ b/panda/src/tform/driveInterface.cxx
@@ -112,8 +112,10 @@ DriveInterface(const string &name) :
   _button_events_input = define_input("button_events", ButtonEventList::get_class_type());
 
   _transform_output = define_output("transform", EventStoreMat4::get_class_type());
+  _velocity_output = define_output("velocity", EventStoreVec3::get_class_type());
 
   _transform = new EventStoreMat4(LMatrix4f::ident_mat());
+  _velocity = new EventStoreVec3(LVector3f::zero());
 
   _forward_speed = drive_forward_speed;
   _reverse_speed = drive_reverse_speed;
@@ -239,10 +241,12 @@ get_mat() const {
 void DriveInterface::
 force_dgraph() {
   _transform->set_value(_mat);
+  _velocity->set_value(_vel);
 
   DataNodeTransmit output;
   output.reserve(get_num_outputs());
   output.set_data(_transform_output, EventParameter(_transform));
+  output.set_data(_velocity_output, EventParameter(_velocity));
 
   DataGraphTraverser dg_trav;
   dg_trav.traverse_below(this, output);
@@ -356,6 +360,7 @@ apply(double x, double y, bool any_button) {
   }
 
   if (_speed == 0.0f && _rot_speed == 0.0f) {
+    _vel.set(0.0f, 0.0f, 0.0f);
     return;
   }
 
@@ -371,7 +376,8 @@ apply(double x, double y, bool any_button) {
     LMatrix3f::rotate_mat_normaxis(_hpr[0], LVector3f::up(_cs), _cs);
 
   // Take a step in the direction of our previous heading.
-  LVector3f step = distance * (LVector3f::forward(_cs) * rot_mat);
+  _vel = LVector3f::forward(_cs) * distance;
+  LVector3f step = (_vel * rot_mat);
 
   // To prevent upward drift due to numerical errors, force the
   // vertical component of our step to zero (it should be pretty near
@@ -503,5 +509,7 @@ do_transmit_data(const DataNodeTransmit &input, DataNodeTransmit &output) {
 
   apply(x, y, _mods.is_any_down());
   _transform->set_value(_mat);
+  _velocity->set_value(_vel);
   output.set_data(_transform_output, EventParameter(_transform));
+  output.set_data(_velocity_output, EventParameter(_velocity));
 }
diff --git a/panda/src/tform/driveInterface.h b/panda/src/tform/driveInterface.h
index 1499b39b73..2d15fa61f5 100644
--- a/panda/src/tform/driveInterface.h
+++ b/panda/src/tform/driveInterface.h
@@ -133,6 +133,7 @@ private:
   LPoint3f _xyz;
   LVecBase3f _hpr;
   LMatrix4f _mat;
+  LVector3f _vel;
   float _force_roll;
   bool _is_force_roll;
   CoordinateSystem _cs;
@@ -173,8 +174,10 @@ private:
 
   // outputs
   int _transform_output;
+  int _velocity_output;
 
   PT(EventStoreMat4) _transform;
+  PT(EventStoreVec3) _velocity;
 
 public:
   static TypeHandle get_class_type() {
diff --git a/panda/src/tform/transform2sg.cxx b/panda/src/tform/transform2sg.cxx
index 99f8373d13..61068bd20d 100644
--- a/panda/src/tform/transform2sg.cxx
+++ b/panda/src/tform/transform2sg.cxx
@@ -32,14 +32,16 @@ Transform2SG(const string &name) :
   DataNode(name)
 {
   _transform_input = define_input("transform", EventStoreMat4::get_class_type());
+  _velocity_input = define_input("velocity", EventStoreVec3::get_class_type());
 
   _node = NULL;
+  _velocity_node = NULL;
 }
 
 ////////////////////////////////////////////////////////////////////
 //     Function: Transform2SG::set_node
 //       Access: Public
-//  Description: Sets the node that this node will adjust.
+//  Description: Sets the node that this object will adjust.
 ////////////////////////////////////////////////////////////////////
 void Transform2SG::
 set_node(PandaNode *node) {
@@ -49,7 +51,7 @@ set_node(PandaNode *node) {
 ////////////////////////////////////////////////////////////////////
 //     Function: Transform2SG::get_node
 //       Access: Public
-//  Description: Returns the node that this node will adjust, or NULL
+//  Description: Returns the node that this object will adjust, or NULL
 //               if the node has not yet been set.
 ////////////////////////////////////////////////////////////////////
 PandaNode *Transform2SG::
@@ -57,6 +59,33 @@ get_node() const {
   return _node;
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: Transform2SG::set_velocity_node
+//       Access: Public
+//  Description: Sets the node that this object will assign the
+//               computed velocity to.  Normally this is a
+//               CollisionNode parented below the node indicated by
+//               set_node().  Setting this node allows the collision
+//               system to track the velocity imparted to the
+//               CollisionNode by the data graph object that set its
+//               transform, if that data is available.
+////////////////////////////////////////////////////////////////////
+void Transform2SG::
+set_velocity_node(PandaNode *node) {
+  _velocity_node = node;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: Transform2SG::get_velocity_node
+//       Access: Public
+//  Description: Returns the node that this object will assign the
+//               computed velocity to.  See set_velocity_node().
+////////////////////////////////////////////////////////////////////
+PandaNode *Transform2SG::
+get_velocity_node() const {
+  return _velocity_node;
+}
+
 
 ////////////////////////////////////////////////////////////////////
 //     Function: Transform2SG::do_transmit_data
@@ -81,4 +110,13 @@ do_transmit_data(const DataNodeTransmit &input, DataNodeTransmit &) {
       _node->set_transform(TransformState::make_mat(mat));
     }
   }
+
+  if (input.has_data(_velocity_input)) {
+    const EventStoreVec3 *velocity;
+    DCAST_INTO_V(velocity, input.get_data(_velocity_input).get_ptr());
+    LVector3f vel = velocity->get_value();
+    if (_velocity_node != (PandaNode *)NULL) {
+      _velocity_node->set_velocity(vel);
+    }
+  }
 }
diff --git a/panda/src/tform/transform2sg.h b/panda/src/tform/transform2sg.h
index 074746e6b6..df71c452b4 100644
--- a/panda/src/tform/transform2sg.h
+++ b/panda/src/tform/transform2sg.h
@@ -38,8 +38,12 @@ PUBLISHED:
   void set_node(PandaNode *node);
   PandaNode *get_node() const;
 
+  void set_velocity_node(PandaNode *node);
+  PandaNode *get_velocity_node() const;
+
 private:
   PandaNode *_node;
+  PandaNode *_velocity_node;
 
 protected:
   // Inherited from DataNode
@@ -49,6 +53,7 @@ protected:
 private:
   // inputs
   int _transform_input;
+  int _velocity_input;
 
 public:
   static TypeHandle get_class_type() {