move set_velocity() etc. from CollisionNode to PandaNode

panda/src/collide/collisionNode.I

@@ -172,39 +172,3 @@ 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;
-}

panda/src/collide/collisionNode.cxx

@@ -44,7 +44,6 @@ CollisionNode(const string &name) :
   PandaNode(name),
   _from_collide_mask(CollideMask::all_on()),
   _into_collide_mask(CollideMask::all_on()),
-  _velocity(0.0f, 0.0f, 0.0f),
   _flags(0)
 {
   // CollisionNodes are hidden by default.
@@ -61,7 +60,6 @@ CollisionNode(const CollisionNode &copy) :
   PandaNode(copy),
   _from_collide_mask(copy._from_collide_mask),
   _into_collide_mask(copy._into_collide_mask),
-  _velocity(copy._velocity),
   _flags(copy._flags),
   _solids(copy._solids)
 {
@@ -254,31 +252,6 @@ 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

panda/src/collide/collisionNode.h

@@ -68,11 +68,6 @@ 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();
@@ -85,11 +80,9 @@ private:
   // this later.
   CollideMask _from_collide_mask;
   CollideMask _into_collide_mask;
-  LVector3f _velocity;
 
   enum Flags {
     F_collide_geom = 0x0001,
-    F_has_velocity = 0x0002,
     // Presently only 8 bits are written to the bam file.
   };
   int _flags;

panda/src/pgraph/nodePath.I

@@ -843,6 +843,59 @@ get_distance(const NodePath &other) const {
   return length(LVector3f(pos));
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: NodePath::set_velocity
+//       Access: Published
+//  Description: Sets the velocity of this node.  This is a delta from
+//               the position of this node in the previous frame,
+//               relative to the node's parent.  The value should not
+//               be transformed by the node's transformation (it is in
+//               the coordinate system of the node's parent).  This
+//               has meaning only to the collision subsystem.
+////////////////////////////////////////////////////////////////////
+INLINE void NodePath::
+set_velocity(const LVector3f &velocity) {
+  nassertv_always(!is_empty());
+  node()->set_velocity(velocity);
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: NodePath::clear_velocity
+//       Access: Published
+//  Description: Resets the velocity of this node to zero.  See
+//               set_velocity().
+////////////////////////////////////////////////////////////////////
+INLINE void NodePath::
+clear_velocity() {
+  nassertv_always(!is_empty());
+  node()->clear_velocity();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: NodePath::has_velocity
+//       Access: Published
+//  Description: Returns true if a nonzero velocity has been set on
+//               this node, false otherwise.
+////////////////////////////////////////////////////////////////////
+INLINE bool NodePath::
+has_velocity() const {
+  nassertr_always(!is_empty(), false);
+  return node()->has_velocity();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: NodePath::get_velocity
+//       Access: Published
+//  Description: Returns the velocity previously set on this node via
+//               set_velocity(), or LVector3f::zero() if no velocity
+//               has been set.
+////////////////////////////////////////////////////////////////////
+INLINE const LVector3f &NodePath::
+get_velocity() const {
+  nassertr_always(!is_empty(), LVector3f::zero());
+  return node()->get_velocity();
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: NodePath::set_color_scale
 //       Access: Published

panda/src/pgraph/nodePath.cxx

@@ -1502,6 +1502,107 @@ heads_up(const NodePath &other, const LPoint3f &point, const LVector3f &up) {
   set_quat(quat);
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: NodePath::get_velocity
+//       Access: Published
+//  Description: Returns the relative velocity to this node from the
+//               other node; i.e. the velocity of this node as seen
+//               from the other node.
+////////////////////////////////////////////////////////////////////
+LVector3f NodePath::
+get_velocity(const NodePath &other) const {
+  if (other.is_empty()) {
+    return get_net_velocity();
+  }
+  if (is_empty()) {
+    other.uncollapse_head();
+    CPT(TransformState) net_transform = TransformState::make_identity();
+    CPT(TransformState) parent_transform = net_transform;
+    LVector3f net_velocity = LVector3f::zero();
+    r_get_net_velocity(other._head, net_velocity, net_transform, parent_transform);
+    return -net_velocity;
+  }
+    
+  nassertr(verify_complete(), LVector3f::zero());
+  nassertr(other.verify_complete(), LVector3f::zero());
+
+  int a_count, b_count;
+  find_common_ancestor(*this, other, a_count, b_count);
+
+  // Now compute the net velocity of each node, by working down from
+  // the common ancestor.
+  CPT(TransformState) a_transform = TransformState::make_identity();
+  CPT(TransformState) a_parent_transform = a_transform;
+  LVector3f a_velocity = LVector3f::zero();
+  r_get_partial_velocity(_head, a_count, a_velocity, a_transform, 
+                         a_parent_transform);
+
+  CPT(TransformState) b_transform = TransformState::make_identity();
+  CPT(TransformState) b_parent_transform = b_transform;
+  LVector3f b_velocity = LVector3f::zero();
+  r_get_partial_velocity(other._head, b_count, b_velocity, b_transform, 
+                         b_parent_transform);
+
+  LVector3f net_velocity = a_velocity - b_velocity;
+  
+  // Finally, convert the net velocity back into the parent's
+  // coordinate space.
+  CPT(TransformState) a_parent_inverse = 
+    a_parent_transform->invert_compose(TransformState::make_identity());
+  return net_velocity * a_parent_inverse->get_mat();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: NodePath::set_velocity
+//       Access: Published
+//  Description: Sets the velocity of this node, relative to the other
+//               node.  This computes a new velocity that will have
+//               the indicated value when seen from the other node.
+////////////////////////////////////////////////////////////////////
+void NodePath::
+set_velocity(const NodePath &other, const LVector3f &velocity) {
+  nassertv(_error_type == ET_ok && other._error_type == ET_ok);
+  nassertv_always(!is_empty());
+
+  // First, we perform a wrt to the parent, to get the conversion.
+  LVector3f rel_velocity;
+  if (has_parent()) {
+    rel_velocity = other.get_velocity(get_parent());
+  } else {
+    rel_velocity = other.get_velocity(NodePath());
+  }
+
+  LVector3f new_velocity = velocity + rel_velocity;
+  set_velocity(new_velocity);
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: NodePath::get_net_velocity
+//       Access: Published
+//  Description: Returns the relative "velocity" of this node (as
+//               reported by get_velocity()) from the root of the
+//               scene graph, accumulating all velocity values set on
+//               ancestor nodes.  The result is returned in the
+//               coordinate space of the node's parent (the same
+//               coordinate space in which get_velocity() is
+//               returned).
+////////////////////////////////////////////////////////////////////
+LVector3f NodePath::
+get_net_velocity() const {
+  uncollapse_head();
+  CPT(TransformState) net_transform = TransformState::make_identity();
+  CPT(TransformState) parent_transform = net_transform;
+  LVector3f net_velocity = LVector3f::zero();
+  r_get_net_velocity(_head, net_velocity, net_transform, parent_transform);
+
+  // Convert the net velocity back from global coordinates into the
+  // parent's coordinate space.
+  CPT(TransformState) parent_inverse = 
+    parent_transform->invert_compose(TransformState::make_identity());
+  return net_velocity * parent_inverse->get_mat();
+}
+
+
 ////////////////////////////////////////////////////////////////////
 //     Function: NodePath::set_color
 //       Access: Published
@@ -3206,11 +3307,15 @@ uncollapse_head() const {
 //               that both have in common, if any.  Fills a_count and
 //               b_count with the number of nodes below the common
 //               node in each path.
+//
+//               The return value is the NodePathComponent of the node
+//               they have in common, or NULL if they have nothing in
+//               common.
 ////////////////////////////////////////////////////////////////////
-void NodePath::
+NodePathComponent *NodePath::
 find_common_ancestor(const NodePath &a, const NodePath &b,
                      int &a_count, int &b_count) {
-  nassertv(!a.is_empty() && !b.is_empty());
+  nassertr(!a.is_empty() && !b.is_empty(), NULL);
   a.uncollapse_head();
   b.uncollapse_head();
 
@@ -3221,12 +3326,12 @@ find_common_ancestor(const NodePath &a, const NodePath &b,
 
   // Shorten up the longer one until they are the same length.
   while (ac->get_length() > bc->get_length()) {
-    nassertv(ac != (NodePathComponent *)NULL);
+    nassertr(ac != (NodePathComponent *)NULL, NULL);
     ac = ac->get_next();
     a_count++;
   }
   while (bc->get_length() > ac->get_length()) {
-    nassertv(bc != (NodePathComponent *)NULL);
+    nassertr(bc != (NodePathComponent *)NULL, NULL);
     bc = bc->get_next();
     b_count++;
   }
@@ -3234,19 +3339,21 @@ find_common_ancestor(const NodePath &a, const NodePath &b,
   // Now shorten them both up until we reach the same component.
   while (ac != bc) {
     // These shouldn't go to NULL unless they both go there together. 
-    nassertv(ac != (NodePathComponent *)NULL);
-    nassertv(bc != (NodePathComponent *)NULL);
+    nassertr(ac != (NodePathComponent *)NULL, NULL);
+    nassertr(bc != (NodePathComponent *)NULL, NULL);
     ac = ac->get_next();
     a_count++;
     bc = bc->get_next();
     b_count++;
   }
+
+  return ac;
 }
 
 ////////////////////////////////////////////////////////////////////
 //     Function: NodePath::r_get_net_state
 //       Access: Private
-//  Description: Recursively determines the net state chnages to the
+//  Description: Recursively determines the net state changes to the
 //               indicated component node from the root of the graph.
 ////////////////////////////////////////////////////////////////////
 CPT(RenderState) NodePath::
@@ -3311,6 +3418,62 @@ r_get_partial_transform(NodePathComponent *comp, int n) const {
   }
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: NodePath::r_get_net_velocity
+//       Access: Private
+//  Description: Recursively determines the net velocity of the
+//               indicated node; the result is returned in the global
+//               coordinate space.
+////////////////////////////////////////////////////////////////////
+void NodePath::
+r_get_net_velocity(NodePathComponent *comp, LVector3f &net_vel, 
+                   CPT(TransformState) &net_transform,
+                   CPT(TransformState) &parent_net_transform) const {
+  if (comp != (NodePathComponent *)NULL) {
+    r_get_net_velocity(comp->get_next(), net_vel, net_transform, 
+                       parent_net_transform);
+
+    PandaNode *node = comp->get_node();
+    if (node->has_velocity()) {
+      LVector3f vel = node->get_velocity();
+      net_vel += vel * net_transform->get_mat();
+    }
+    
+    CPT(TransformState) transform = node->get_transform();
+    parent_net_transform = net_transform;
+    net_transform = net_transform->compose(transform);
+  }
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: NodePath::r_get_partial_velocity
+//       Access: Private
+//  Description: Recursively determines the net velocity of the
+//               indicated component node from the nth node above it.
+//               If n exceeds the length of the path, this returns the
+//               net velocity from the root of the graph.
+////////////////////////////////////////////////////////////////////
+void NodePath::
+r_get_partial_velocity(NodePathComponent *comp, int n, 
+                       LVector3f &net_vel,
+                       CPT(TransformState) &net_transform,
+                       CPT(TransformState) &parent_net_transform) const {
+  if (n != 0 && comp != (NodePathComponent *)NULL) {
+    r_get_partial_velocity(comp->get_next(), n - 1, net_vel, net_transform,
+                           parent_net_transform);
+    
+    PandaNode *node = comp->get_node();
+    if (node->has_velocity()) {
+      LVector3f vel = node->get_velocity();
+      net_vel += vel * net_transform->get_mat();
+    }
+    
+    CPT(TransformState) transform = node->get_transform();
+    parent_net_transform = net_transform;
+    net_transform = net_transform->compose(transform);
+  }
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: NodePath::find_matches
 //       Access: Private

panda/src/pgraph/nodePath.h

@@ -415,6 +415,16 @@ PUBLISHED:
 
   INLINE float get_distance(const NodePath &other) const;
 
+  INLINE void set_velocity(const LVector3f &velocity);
+  INLINE void clear_velocity();
+  INLINE bool has_velocity() const;
+  INLINE const LVector3f &get_velocity() const;
+
+  LVector3f get_velocity(const NodePath &other) const;
+  void set_velocity(const NodePath &other, const LVector3f &velocity);
+
+  LVector3f get_net_velocity() const;
+  
 
   // Methods that affect appearance of geometry: color, texture, etc.
   // These affect the state at the bottom level only.
@@ -571,14 +581,23 @@ PUBLISHED:
 
 private:
   void uncollapse_head() const;
-  static void find_common_ancestor(const NodePath &a, const NodePath &b,
-                                   int &a_count, int &b_count);
+  static NodePathComponent *
+  find_common_ancestor(const NodePath &a, const NodePath &b,
+                       int &a_count, int &b_count);
 
   CPT(RenderState) r_get_net_state(NodePathComponent *comp) const;
   CPT(RenderState) r_get_partial_state(NodePathComponent *comp, int n) const;
   CPT(TransformState) r_get_net_transform(NodePathComponent *comp) const;
   CPT(TransformState) r_get_partial_transform(NodePathComponent *comp, int n) const;
 
+  void r_get_net_velocity(NodePathComponent *comp, LVector3f &net_vel, 
+                          CPT(TransformState) &net_transform,
+                          CPT(TransformState) &parent_net_transform) const;
+  void r_get_partial_velocity(NodePathComponent *comp, int n, 
+                              LVector3f &net_vel, 
+                              CPT(TransformState) &net_transform,
+                              CPT(TransformState) &parent_net_transform) const;
+
   void find_matches(NodePathCollection &result,
                     const string &approx_path_str,
                     int max_matches) const;

panda/src/pgraph/pandaNode.I

@@ -119,6 +119,8 @@ CData() {
   _effects = RenderEffects::make_empty();
   _transform = TransformState::make_identity();
   _draw_mask = DrawMask::all_on();
+  _velocity = LVector3f::zero();
+  _has_velocity = false;
   _net_collide_mask = CollideMask::all_off();
   _fixed_internal_bound = false;
 }
@@ -139,6 +141,8 @@ CData(const PandaNode::CData &copy) :
   _transform(copy._transform),
   _tag_data(copy._tag_data),
   _draw_mask(copy._draw_mask),
+  _velocity(copy._velocity),
+  _has_velocity(copy._has_velocity),
   _fixed_internal_bound(copy._fixed_internal_bound)
 {
   _net_collide_mask = CollideMask::all_off();
@@ -780,6 +784,61 @@ get_net_collide_mask() const {
   return cdata->_net_collide_mask;
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: PandaNode::set_velocity
+//       Access: Published
+//  Description: Sets the velocity of this node.  This is a delta from
+//               the position of this node in the previous frame,
+//               relative to the node's parent.  The value should not
+//               be transformed by the node's transformation (it is in
+//               the coordinate system of the node's parent).  This
+//               has meaning only to the collision subsystem.
+////////////////////////////////////////////////////////////////////
+INLINE void PandaNode::
+set_velocity(const LVector3f &velocity) {
+  CDWriter cdata(_cycler);
+  cdata->_velocity = velocity;
+  cdata->_has_velocity = (!velocity.almost_equal(LVector3f::zero()));
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: PandaNode::clear_velocity
+//       Access: Published
+//  Description: Resets the velocity of this node to zero.  See
+//               set_velocity().
+////////////////////////////////////////////////////////////////////
+INLINE void PandaNode::
+clear_velocity() {
+  CDWriter cdata(_cycler);
+  cdata->_velocity = LVector3f::zero();
+  cdata->_has_velocity = false;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: PandaNode::has_velocity
+//       Access: Published
+//  Description: Returns true if a nonzero velocity has been set on
+//               this node, false otherwise.
+////////////////////////////////////////////////////////////////////
+INLINE bool PandaNode::
+has_velocity() const {
+  CDReader cdata(_cycler);
+  return cdata->_has_velocity;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: PandaNode::get_velocity
+//       Access: Published
+//  Description: Returns the velocity previously set on this node via
+//               set_velocity(), or LVector3f::zero() if no velocity
+//               has been set.
+////////////////////////////////////////////////////////////////////
+INLINE const LVector3f &PandaNode::
+get_velocity() const {
+  CDReader cdata(_cycler);
+  return cdata->_velocity;
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: PandaNode::set_bound
 //       Access: Published

panda/src/pgraph/pandaNode.cxx

@@ -1388,25 +1388,6 @@ as_light() {
   return NULL;
 }
 
-////////////////////////////////////////////////////////////////////
-//     Function: PandaNode::set_velocity
-//       Access: Published, 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

panda/src/pgraph/pandaNode.h

@@ -162,6 +162,11 @@ PUBLISHED:
 
   INLINE CollideMask get_net_collide_mask() const;
 
+  INLINE void set_velocity(const LVector3f &velocity);
+  INLINE void clear_velocity();
+  INLINE bool has_velocity() const;
+  INLINE const LVector3f &get_velocity() const;
+
   virtual void output(ostream &out) const;
   virtual void write(ostream &out, int indent_level) const;
 
@@ -186,7 +191,6 @@ PUBLISHED:
 
   virtual bool is_geom_node() const;
   virtual Light *as_light();
-  virtual void set_velocity(const LVector3f &vel);
 
 protected:
   // Inherited from BoundedObject
@@ -316,6 +320,9 @@ private:
     // This is the draw_mask of this particular node.
     DrawMask _draw_mask;
 
+    LVector3f _velocity;
+    bool _has_velocity;
+
     // This is the union of all into_collide_mask bits for any
     // CollisionNodes at and below this level.  It's conceptually
     // similar to a bounding volume--it represents the bounding volume