improved pgraph collisions

panda/src/collide/qpcollisionLevelState.I

@@ -55,6 +55,17 @@ get_node_path() const {
   return _node_path.get_node_path();
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: qpCollisionLevelState::node
+//       Access: Public
+//  Description: Returns the PandaNode pointer of the node we have
+//               traversed to.
+////////////////////////////////////////////////////////////////////
+INLINE PandaNode *qpCollisionLevelState::
+node() const {
+  return _node_path.node();
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: qpCollisionLevelState::get_num_colliders
 //       Access: Public
@@ -212,7 +223,8 @@ omit_collider(int n) {
 ////////////////////////////////////////////////////////////////////
 //     Function: qpCollisionLevelState::get_mask
 //       Access: Private
-//  Description:
+//  Description: Returns a single bit associated with the nth
+//               collider.
 ////////////////////////////////////////////////////////////////////
 INLINE qpCollisionLevelState::ColliderMask qpCollisionLevelState::
 get_mask(int n) const {

panda/src/collide/qpcollisionLevelState.cxx

@@ -73,28 +73,85 @@ prepare_collider(const ColliderDef &def) {
 }
 
 ////////////////////////////////////////////////////////////////////
-//     Function: qpCollisionLevelState::xform
+//     Function: qpCollisionLevelState::any_in_bounds
 //       Access: Public
-//  Description:
+//  Description: Checks the bounding volume of the current node
+//               against each of our colliders.  Eliminates from the
+//               current collider list any that are outside of the
+//               bounding volume.  Returns true if any colliders
+//               remain, false if all of them fall outside this node's
+//               bounding volume.
 ////////////////////////////////////////////////////////////////////
-void qpCollisionLevelState::
-xform(const LMatrix4f &mat) {
-  BoundingVolumes new_bounds;
+bool qpCollisionLevelState::
+any_in_bounds() {
+  const BoundingVolume &node_bv = node()->get_bound();
+  if (node_bv.is_of_type(GeometricBoundingVolume::get_class_type())) {
+    const GeometricBoundingVolume *node_gbv;
+    DCAST_INTO_R(node_gbv, &node_bv, false);
+
+    int num_colliders = get_num_colliders();
+    for (int c = 0; c < num_colliders; c++) {
+      if (has_collider(c)) {
+        bool is_in = false;
+
+        // Don't even bother testing the bounding volume if there are
+        // no collide bits in common between our collider and this
+        // node.
+        CollideMask from_mask = get_node(c)->get_from_collide_mask();
+        if ((from_mask & node()->get_net_collide_mask()) != 0) {
+          // There are bits in common, so go ahead and try the
+          // bounding volume.
+          const GeometricBoundingVolume *col_gbv =
+            get_local_bound(c);
+          if (col_gbv != (GeometricBoundingVolume *)NULL) {
+            is_in = (node_gbv->contains(col_gbv) != 0);
+          }
+        }
 
-  int num_colliders = get_num_colliders();
-  new_bounds.reserve(num_colliders);
-  for (int c = 0; c < num_colliders; c++) {
-    if (!has_collider(c) ||
-        get_local_bound(c) == (GeometricBoundingVolume *)NULL) {
-      new_bounds.push_back((GeometricBoundingVolume *)NULL);
-    } else {
-      const GeometricBoundingVolume *old_bound = get_local_bound(c);
-      GeometricBoundingVolume *new_bound = 
-        DCAST(GeometricBoundingVolume, old_bound->make_copy());
-      new_bound->xform(mat);
-      new_bounds.push_back(new_bound);
+        if (!is_in) {
+          // This collider cannot intersect with any geometry at
+          // this node or below.
+          omit_collider(c);
+        }
+      }
     }
   }
 
-  _local_bounds = new_bounds;
+  return has_any_collider();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: qpCollisionLevelState::apply_transform
+//       Access: Public
+//  Description: Applies the inverse transform from the current node,
+//               if any, onto all the colliders in the level state.
+////////////////////////////////////////////////////////////////////
+void qpCollisionLevelState::
+apply_transform() {
+  const TransformState *node_transform = node()->get_transform();
+  if (!node_transform->is_identity()) {
+    CPT(TransformState) inv_transform = 
+      node_transform->invert_compose(TransformState::make_identity());
+    const LMatrix4f &mat = inv_transform->get_mat();
+
+    // Now build the new bounding volumes list.
+    BoundingVolumes new_bounds;
+
+    int num_colliders = get_num_colliders();
+    new_bounds.reserve(num_colliders);
+    for (int c = 0; c < num_colliders; c++) {
+      if (!has_collider(c) ||
+          get_local_bound(c) == (GeometricBoundingVolume *)NULL) {
+        new_bounds.push_back((GeometricBoundingVolume *)NULL);
+      } else {
+        const GeometricBoundingVolume *old_bound = get_local_bound(c);
+        GeometricBoundingVolume *new_bound = 
+          DCAST(GeometricBoundingVolume, old_bound->make_copy());
+        new_bound->xform(mat);
+        new_bounds.push_back(new_bound);
+      }
+    }
+    
+    _local_bounds = new_bounds;
+  }    
 }

panda/src/collide/qpcollisionLevelState.h

@@ -55,9 +55,12 @@ public:
   void clear();
   void reserve(int max_colliders);
   void prepare_collider(const ColliderDef &def);
-  void xform(const LMatrix4f &mat);
 
+  bool any_in_bounds();
+  void apply_transform();
+  
   INLINE qpNodePath get_node_path() const;
+  INLINE PandaNode *node() const;
 
   INLINE int get_num_colliders() const;
   INLINE bool has_collider(int n) const;
@@ -75,7 +78,7 @@ public:
 
   INLINE void omit_collider(int n);
 
-private:
+  //private:
   typedef int ColliderMask;
 
   INLINE ColliderMask get_mask(int n) const;

panda/src/collide/qpcollisionNode.I

@@ -25,8 +25,8 @@
 ////////////////////////////////////////////////////////////////////
 INLINE void qpCollisionNode::
 set_collide_mask(CollideMask mask) {
-  _from_collide_mask = mask;
-  _into_collide_mask = mask;
+  set_from_collide_mask(mask);
+  set_into_collide_mask(mask);
 }
 
 ////////////////////////////////////////////////////////////////////
@@ -55,6 +55,11 @@ set_from_collide_mask(CollideMask mask) {
 INLINE void qpCollisionNode::
 set_into_collide_mask(CollideMask mask) {
   _into_collide_mask = mask;
+
+  // We mark the bound stale when this changes, not because the actual
+  // bounding volume changes, but rather because we piggyback the
+  // computing of the _net_collide_mask on the bounding volume.
+  mark_bound_stale();
 }
 
 ////////////////////////////////////////////////////////////////////

panda/src/collide/qpcollisionNode.cxx

@@ -232,6 +232,23 @@ output(ostream &out) const {
 }
 
 
+////////////////////////////////////////////////////////////////////
+//     Function: qpCollisionNode::recompute_bound
+//       Access: Protected, Virtual
+//  Description: Recomputes the dynamic bounding volume for this
+//               object.  This is the bounding volume for the node and
+//               all of its children, and normally does not need to be
+//               specialized beyond PandaNode; we specialize this
+//               function just so we can piggyback on it the
+//               setting the _net_collide_mask bits.
+////////////////////////////////////////////////////////////////////
+BoundingVolume *qpCollisionNode::
+recompute_bound() {
+  BoundingVolume *result = PandaNode::recompute_bound();
+  add_net_collide_mask(get_into_collide_mask());
+  return result;
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: qpCollisionNode::recompute_internal_bound
 //       Access: Protected, Virtual

panda/src/collide/qpcollisionNode.h

@@ -69,6 +69,7 @@ PUBLISHED:
   INLINE int add_solid(CollisionSolid *solid);
 
 protected:
+  virtual BoundingVolume *recompute_bound();
   virtual BoundingVolume *recompute_internal_bound();
 
 private:

panda/src/collide/qpcollisionTraverser.cxx

@@ -27,6 +27,7 @@
 #include "geom.h"
 #include "qpnodePath.h"
 #include "pStatTimer.h"
+#include "indent.h"
 
 #ifndef CPPPARSER
 PStatCollector qpCollisionTraverser::_collisions_pcollector("App:Collisions");
@@ -236,7 +237,7 @@ traverse(const qpNodePath &root) {
     (*hi).first->begin_group();
   }
 
-  r_traverse(root.node(), level_state);
+  r_traverse(level_state);
 
   hi = _handlers.begin();
   while (hi != _handlers.end()) {
@@ -339,7 +340,13 @@ prepare_colliders(qpCollisionLevelState &level_state) {
 //  Description:
 ////////////////////////////////////////////////////////////////////
 void qpCollisionTraverser::
-r_traverse(PandaNode *node, qpCollisionLevelState &level_state) {
+r_traverse(qpCollisionLevelState &level_state) {
+  if (!level_state.any_in_bounds()) {
+    return;
+  }
+  level_state.apply_transform();
+
+  PandaNode *node = level_state.node();
   if (node->is_exact_type(qpCollisionNode::get_class_type())) {
     level_state.reached_collision_node();
 
@@ -420,66 +427,11 @@ r_traverse(PandaNode *node, qpCollisionLevelState &level_state) {
 
   int num_children = node->get_num_children();
   for (int i = 0; i < num_children; i++) {
-    PandaNode *child_node = node->get_child(i);
-    qpCollisionLevelState next_state(level_state, child_node);
-    r_traverse(child_node, next_state);
+    qpCollisionLevelState next_state(level_state, node->get_child(i));
+    r_traverse(next_state);
   }
 }
 
-/*
-////////////////////////////////////////////////////////////////////
-//     Function: qpCollisionTraverser::forward_arc
-//       Access: Public
-//  Description:
-////////////////////////////////////////////////////////////////////
-bool qpCollisionTraverser::
-forward_arc(NodeRelation *arc, NullTransitionWrapper &,
-            NullTransitionWrapper &, NullTransitionWrapper &,
-            qpCollisionLevelState &level_state) {
-  // Check the bounding volume on the arc against each of our
-  // colliders.
-  const BoundingVolume &arc_bv = arc->get_bound();
-  if (arc_bv.is_of_type(GeometricBoundingVolume::get_class_type())) {
-    const GeometricBoundingVolume *arc_gbv;
-    DCAST_INTO_R(arc_gbv, &arc_bv, false);
-
-    int num_colliders = level_state.get_num_colliders();
-    for (int c = 0; c < num_colliders; c++) {
-      if (level_state.has_collider(c)) {
-        const GeometricBoundingVolume *col_gbv =
-          level_state.get_local_bound(c);
-        if (col_gbv != (GeometricBoundingVolume *)NULL) {
-          if (arc_gbv->contains(col_gbv) == 0) {
-            // This collider certainly does not intersect with any
-            // geometry at this arc or below.
-            level_state.omit_collider(c);
-          }
-        }
-      }
-    }
-  }
-
-  if (!level_state.has_any_collider()) {
-    // No colliders intersect with the geometry at this arc or below,
-    // so don't bother traversing them.
-    return false;
-  }
-
-  TransformState *tt;
-  if (get_transition_into(tt, arc)) {
-    // There's a transform on the arc; apply it to our colliders'
-    // bounding volumes.
-    LMatrix4f inv_mat;
-    inv_mat.invert_from(tt->get_matrix());
-    level_state.xform(inv_mat);
-  }
-
-  level_state.forward_arc(arc);
-
-  return true;
-}
-*/
-
 ////////////////////////////////////////////////////////////////////
 //     Function: qpCollisionTraverser::compare_collider_to_node
 //       Access: Private

panda/src/collide/qpcollisionTraverser.h

@@ -63,7 +63,7 @@ PUBLISHED:
 private:
   void prepare_colliders(qpCollisionLevelState &state);
 
-  void r_traverse(PandaNode *node, qpCollisionLevelState &level_state);
+  void r_traverse(qpCollisionLevelState &level_state);
 
   void compare_collider_to_node(qpCollisionEntry &entry,
                                 const GeometricBoundingVolume *from_node_gbv,

panda/src/pgraph/pandaNode.I

@@ -119,6 +119,7 @@ CData() {
   _effects = RenderEffects::make_empty();
   _transform = TransformState::make_identity();
   _draw_mask = DrawMask::all_on();
+  _net_collide_mask = CollideMask::all_off();
 }
 
 ////////////////////////////////////////////////////////////////////
@@ -137,6 +138,7 @@ CData(const PandaNode::CData &copy) :
   _transform(copy._transform),
   _draw_mask(copy._draw_mask)
 {
+  _net_collide_mask = CollideMask::all_off();
 }
 
 ////////////////////////////////////////////////////////////////////
@@ -692,6 +694,20 @@ get_draw_mask() const {
   return cdata->_draw_mask;
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: PandaNode::get_net_collide_mask
+//       Access: Published
+//  Description: Returns the union of all into_collide_mask() values
+//               set at CollisionNodes at this level and below.
+////////////////////////////////////////////////////////////////////
+INLINE CollideMask PandaNode::
+get_net_collide_mask() const {
+  // Call get_bound() first to ensure the mask is recomputed.
+  BoundedObject::get_bound();
+  CDReader cdata(_cycler);
+  return cdata->_net_collide_mask;
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: PandaNode::set_bound
 //       Access: Published
@@ -757,6 +773,19 @@ changed_internal_bound() {
   BoundedObject::mark_bound_stale();
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: PandaNode::add_net_collide_mask
+//       Access: Protected
+//  Description: Adds the indicated bits into the net_collide_mask for
+//               this node.  This is normally called only by
+//               CollisionNode::recompute_bound().
+////////////////////////////////////////////////////////////////////
+INLINE void PandaNode::
+add_net_collide_mask(CollideMask mask) {
+  CDWriter cdata(_cycler);
+  cdata->_net_collide_mask |= mask;
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: PandaNode::get_children
 //       Access: Public

panda/src/pgraph/pandaNode.cxx

@@ -1179,6 +1179,10 @@ recompute_bound() {
   BoundingVolume *bound = BoundedObject::recompute_bound();
   nassertr(bound != (BoundingVolume*)NULL, bound);
 
+  // Also, recompute the net_collide_mask bits while we do this.
+  CDWriter cdata(_cycler);
+  cdata->_net_collide_mask = CollideMask::all_off();
+
   // Now actually compute the bounding volume by putting it around all
   // of our child bounding volumes.
   pvector<const BoundingVolume *> child_volumes;
@@ -1186,13 +1190,13 @@ recompute_bound() {
   // It goes around this node's internal bounding volume . . .
   child_volumes.push_back(internal_bound);
 
-  CDReader cdata(_cycler);
   Down::const_iterator di;
   for (di = cdata->_down.begin(); di != cdata->_down.end(); ++di) {
     // . . . plus each node's external bounding volume.
     PandaNode *child = (*di).get_child();
     const BoundingVolume &child_bound = child->get_bound();
     child_volumes.push_back(&child_bound);
+    cdata->_net_collide_mask |= child->get_net_collide_mask();
   }
 
   const BoundingVolume **child_begin = &child_volumes[0];

panda/src/pgraph/pandaNode.h

@@ -31,6 +31,7 @@
 #include "drawMask.h"
 #include "typedWritable.h"
 #include "boundedObject.h"
+#include "collideMask.h"
 #include "namable.h"
 #include "referenceCount.h"
 #include "luse.h"
@@ -135,6 +136,8 @@ PUBLISHED:
   INLINE void set_draw_mask(DrawMask mask);
   INLINE DrawMask get_draw_mask() const;
 
+  INLINE CollideMask get_net_collide_mask() const;
+
   virtual void output(ostream &out) const;
   virtual void write(ostream &out, int indent_level) const;
 
@@ -170,6 +173,7 @@ protected:
   INLINE void changed_internal_bound();
   virtual void parents_changed();
   virtual void children_changed();
+  INLINE void add_net_collide_mask(CollideMask mask);
 
   typedef pmap<PandaNode *, PandaNode *> InstanceMap;
   virtual PT(PandaNode) r_copy_subgraph(InstanceMap &inst_map) const;
@@ -270,7 +274,17 @@ private:
     CPT(RenderState) _state;
     CPT(RenderEffects) _effects;
     CPT(TransformState) _transform;
+
+    // This is the draw_mask of this particular node.
     DrawMask _draw_mask;
+
+    // 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
+    // of this node in the space of collision bits--and it needs to be
+    // updated for the same reasons the bounding volume needs to be
+    // updated.  So we update them together.
+    CollideMask _net_collide_mask;
   };
 
   PipelineCycler<CData> _cycler;

panda/src/pgraph/qpnodePath.cxx

@@ -2406,21 +2406,7 @@ hide_bounds() {
 PT(BoundingVolume) qpNodePath::
 get_bounds() const {
   nassertr_always(!is_empty(), new BoundingSphere);
-
-  PandaNode *this_node = node();
-  PT(BoundingVolume) bv = this_node->get_bound().make_copy();
-  if (bv->is_of_type(GeometricBoundingVolume::get_class_type()) &&
-      !this_node->get_transform()->is_identity()) {
-
-    // The bounding volume has already been transformed by the node's
-    // matrix.  We'd rather return a bounding volume in the node's
-    // space, so we have to untransform it now.  Ick.
-    GeometricBoundingVolume *gbv = DCAST(GeometricBoundingVolume, bv);
-    const LMatrix4f &mat = get_parent().get_mat(*this);
-    gbv->xform(mat);
-  }
-
-  return bv;
+  return node()->get_bound().make_copy();
 }
 
 ////////////////////////////////////////////////////////////////////

panda/src/pgraph/workingNodePath.cxx

@@ -19,6 +19,21 @@
 #include "workingNodePath.h"
 
 
+////////////////////////////////////////////////////////////////////
+//     Function: WorkingNodePath::get_num_nodes
+//       Access: Public
+//  Description: Returns the number of nodes in the path from the root
+//               to the current node.
+////////////////////////////////////////////////////////////////////
+int WorkingNodePath::
+get_num_nodes() const {
+  if (_next == (WorkingNodePath *)NULL) {
+    return _start->get_length();
+  }
+
+  return _next->get_num_nodes() + 1;
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: WorkingNodePath::r_get_node_path
 //       Access: Private

panda/src/pgraph/workingNodePath.h

@@ -55,6 +55,8 @@ public:
   INLINE qpNodePath get_node_path() const;
   INLINE PandaNode *node() const;
 
+  int get_num_nodes() const;
+
 private:
   PT(qpNodePathComponent) r_get_node_path() const;