Panda3D

collisionTraverser.cxx

00001 // Filename: collisionTraverser.cxx
00002 // Created by:  drose (16Mar02)
00003 //
00004 ////////////////////////////////////////////////////////////////////
00005 //
00006 // PANDA 3D SOFTWARE
00007 // Copyright (c) Carnegie Mellon University.  All rights reserved.
00008 //
00009 // All use of this software is subject to the terms of the revised BSD
00010 // license.  You should have received a copy of this license along
00011 // with this source code in a file named "LICENSE."
00012 //
00013 ////////////////////////////////////////////////////////////////////
00014 
00015 #include "collisionTraverser.h"
00016 #include "collisionNode.h"
00017 #include "collisionEntry.h"
00018 #include "collisionPolygon.h"
00019 #include "collisionGeom.h"
00020 #include "collisionRecorder.h"
00021 #include "collisionVisualizer.h"
00022 #include "collisionSphere.h"
00023 #include "collisionBox.h"
00024 #include "collisionTube.h"
00025 #include "collisionPolygon.h"
00026 #include "collisionPlane.h"
00027 #include "config_collide.h"
00028 #include "boundingSphere.h"
00029 #include "transformState.h"
00030 #include "geomNode.h"
00031 #include "geom.h"
00032 #include "geom.h"
00033 #include "geomTriangles.h"
00034 #include "geomVertexReader.h"
00035 #include "lodNode.h"
00036 #include "nodePath.h"
00037 #include "pStatTimer.h"
00038 #include "indent.h"
00039 
00040 #include <algorithm>
00041 
00042 PStatCollector CollisionTraverser::_collisions_pcollector("App:Collisions");
00043 
00044 PStatCollector CollisionTraverser::_cnode_volume_pcollector("Collision Volumes:CollisionNode");
00045 PStatCollector CollisionTraverser::_gnode_volume_pcollector("Collision Volumes:GeomNode");
00046 PStatCollector CollisionTraverser::_geom_volume_pcollector("Collision Volumes:Geom");
00047 
00048 TypeHandle CollisionTraverser::_type_handle;
00049 
00050 // This function object class is used in prepare_colliders(), below.
00051 class SortByColliderSort {
00052 public:
00053   SortByColliderSort(const CollisionTraverser &trav) :
00054     _trav(trav) 
00055   {
00056   }
00057 
00058   inline bool operator () (int a, int b) const {
00059     const CollisionTraverser::OrderedColliderDef &ocd_a = _trav._ordered_colliders[a];
00060     const CollisionTraverser::OrderedColliderDef &ocd_b = _trav._ordered_colliders[b];
00061     return DCAST(CollisionNode, ocd_a._node_path.node())->get_collider_sort() < DCAST(CollisionNode, ocd_b._node_path.node())->get_collider_sort();
00062   }
00063 
00064   const CollisionTraverser &_trav;
00065 };
00066 
00067 ////////////////////////////////////////////////////////////////////
00068 //     Function: CollisionTraverser::Constructor
00069 //       Access: Published
00070 //  Description:
00071 ////////////////////////////////////////////////////////////////////
00072 CollisionTraverser::
00073 CollisionTraverser(const string &name) : 
00074   Namable(name),
00075   _this_pcollector(_collisions_pcollector, name)
00076 {
00077   _respect_prev_transform = respect_prev_transform;
00078   #ifdef DO_COLLISION_RECORDING
00079   _recorder = (CollisionRecorder *)NULL;
00080   #endif
00081 }
00082 
00083 ////////////////////////////////////////////////////////////////////
00084 //     Function: CollisionTraverser::Destructor
00085 //       Access: Published
00086 //  Description:
00087 ////////////////////////////////////////////////////////////////////
00088 CollisionTraverser::
00089 ~CollisionTraverser() {
00090   #ifdef DO_COLLISION_RECORDING
00091   clear_recorder();
00092   #endif
00093 }
00094 
00095 ////////////////////////////////////////////////////////////////////
00096 //     Function: CollisionTraverser::add_collider
00097 //       Access: Published
00098 //  Description: Adds a new CollisionNode, representing an object that
00099 //               will be tested for collisions into other objects,
00100 //               along with the handler that will serve each detected
00101 //               collision.  Each CollisionNode may be served by only
00102 //               one handler at a time, but a given handler may serve
00103 //               many CollisionNodes.
00104 //
00105 //               The handler that serves a particular node may be
00106 //               changed from time to time by calling add_collider()
00107 //               again on the same node.
00108 ////////////////////////////////////////////////////////////////////
00109 void CollisionTraverser::
00110 add_collider(const NodePath &collider, CollisionHandler *handler) {
00111   nassertv(_ordered_colliders.size() == _colliders.size());
00112   nassertv(!collider.is_empty() && collider.node()->is_of_type(CollisionNode::get_class_type()));
00113   nassertv(handler != (CollisionHandler *)NULL);
00114 
00115   Colliders::iterator ci = _colliders.find(collider);
00116   if (ci != _colliders.end()) {
00117     // We already knew about this collider.
00118     if ((*ci).second != handler) {
00119       // Change the handler.
00120       PT(CollisionHandler) old_handler = (*ci).second;
00121       (*ci).second = handler;
00122 
00123       // Now update our own reference counts within our handler set.
00124       Handlers::iterator hi = _handlers.find(old_handler);
00125       nassertv(hi != _handlers.end());
00126       (*hi).second--;
00127       nassertv((*hi).second >= 0);
00128       if ((*hi).second == 0) {
00129         _handlers.erase(hi);
00130       }
00131 
00132       hi = _handlers.find(handler);
00133       if (hi == _handlers.end()) {
00134         _handlers.insert(Handlers::value_type(handler, 1));
00135       } else {
00136         ++(*hi).second;
00137       }
00138     }
00139 
00140   } else {
00141     // We hadn't already known about this collider.
00142     _colliders.insert(Colliders::value_type(collider, handler));
00143     OrderedColliderDef ocdef;
00144     ocdef._node_path = collider;
00145     ocdef._in_graph = true;
00146     _ordered_colliders.push_back(ocdef);
00147 
00148     Handlers::iterator hi = _handlers.find(handler);
00149     if (hi == _handlers.end()) {
00150       _handlers.insert(Handlers::value_type(handler, 1));
00151     } else {
00152       ++(*hi).second;
00153     }
00154   }
00155 
00156   nassertv(_ordered_colliders.size() == _colliders.size());
00157 }
00158 
00159 ////////////////////////////////////////////////////////////////////
00160 //     Function: CollisionTraverser::remove_collider
00161 //       Access: Published
00162 //  Description: Removes the collider (and its associated handler)
00163 //               from the set of CollisionNodes that will be tested
00164 //               each frame for collisions into other objects.
00165 //               Returns true if the definition was found and removed,
00166 //               false if it wasn't present to begin with.
00167 ////////////////////////////////////////////////////////////////////
00168 bool CollisionTraverser::
00169 remove_collider(const NodePath &collider) {
00170   nassertr(_ordered_colliders.size() == _colliders.size(), false);
00171 
00172   Colliders::iterator ci = _colliders.find(collider);
00173   if (ci == _colliders.end()) {
00174     // We didn't know about this node.
00175     return false;
00176   }
00177 
00178   CollisionHandler *handler = (*ci).second;
00179 
00180   // Update the set of handlers.
00181   Handlers::iterator hi = _handlers.find(handler);
00182   nassertr(hi != _handlers.end(), false);
00183   (*hi).second--;
00184   nassertr((*hi).second >= 0, false);
00185   if ((*hi).second == 0) {
00186     _handlers.erase(hi);
00187   }
00188 
00189   _colliders.erase(ci);
00190 
00191   OrderedColliders::iterator oci;
00192   oci = _ordered_colliders.begin();
00193   while (oci != _ordered_colliders.end() &&
00194          (*oci)._node_path != collider) {
00195     ++oci;
00196   }
00197 
00198   nassertr(oci != _ordered_colliders.end(), false);
00199   _ordered_colliders.erase(oci);
00200 
00201   nassertr(_ordered_colliders.size() == _colliders.size(), false);
00202   return true;
00203 }
00204 
00205 ////////////////////////////////////////////////////////////////////
00206 //     Function: CollisionTraverser::has_collider
00207 //       Access: Published
00208 //  Description: Returns true if the indicated node is current in the
00209 //               set of nodes that will be tested each frame for
00210 //               collisions into other objects.
00211 ////////////////////////////////////////////////////////////////////
00212 bool CollisionTraverser::
00213 has_collider(const NodePath &collider) const {
00214   Colliders::const_iterator ci = _colliders.find(collider);
00215   return (ci != _colliders.end());
00216 }
00217 
00218 ////////////////////////////////////////////////////////////////////
00219 //     Function: CollisionTraverser::get_num_colliders
00220 //       Access: Published
00221 //  Description: Returns the number of CollisionNodes that have been
00222 //               added to the traverser via add_collider().
00223 ////////////////////////////////////////////////////////////////////
00224 int CollisionTraverser::
00225 get_num_colliders() const {
00226   nassertr(_ordered_colliders.size() == _colliders.size(), 0);
00227   return _ordered_colliders.size();
00228 }
00229 
00230 ////////////////////////////////////////////////////////////////////
00231 //     Function: CollisionTraverser::get_collider
00232 //       Access: Published
00233 //  Description: Returns the nth CollisionNode that has been
00234 //               added to the traverser via add_collider().
00235 ////////////////////////////////////////////////////////////////////
00236 NodePath CollisionTraverser::
00237 get_collider(int n) const {
00238   nassertr(_ordered_colliders.size() == _colliders.size(), NULL);
00239   nassertr(n >= 0 && n < (int)_ordered_colliders.size(), NULL);
00240   return _ordered_colliders[n]._node_path;
00241 }
00242 
00243 ////////////////////////////////////////////////////////////////////
00244 //     Function: CollisionTraverser::get_handler
00245 //       Access: Published
00246 //  Description: Returns the handler that is currently assigned to
00247 //               serve the indicated collision node, or NULL if the
00248 //               node is not on the traverser's set of active nodes.
00249 ////////////////////////////////////////////////////////////////////
00250 CollisionHandler *CollisionTraverser::
00251 get_handler(const NodePath &collider) const {
00252   Colliders::const_iterator ci = _colliders.find(collider);
00253   if (ci != _colliders.end()) {
00254     return (*ci).second;
00255   }
00256   return NULL;
00257 }
00258 
00259 ////////////////////////////////////////////////////////////////////
00260 //     Function: CollisionTraverser::clear_colliders
00261 //       Access: Published
00262 //  Description: Completely empties the set of collision nodes and
00263 //               their associated handlers.
00264 ////////////////////////////////////////////////////////////////////
00265 void CollisionTraverser::
00266 clear_colliders() {
00267   _colliders.clear();
00268   _ordered_colliders.clear();
00269   _handlers.clear();
00270 }
00271 
00272 ////////////////////////////////////////////////////////////////////
00273 //     Function: CollisionTraverser::traverse
00274 //       Access: Published
00275 //  Description:
00276 ////////////////////////////////////////////////////////////////////
00277 void CollisionTraverser::
00278 traverse(const NodePath &root) {
00279   PStatTimer timer(_this_pcollector);
00280 
00281   #ifdef DO_COLLISION_RECORDING
00282   if (has_recorder()) {
00283     get_recorder()->begin_traversal();
00284   }
00285   #endif  // DO_COLLISION_RECORDING
00286   
00287   Handlers::iterator hi;
00288   for (hi = _handlers.begin(); hi != _handlers.end(); ++hi) {
00289     if ((*hi).first->wants_all_potential_collidees()) {
00290       (*hi).first->set_root(root);
00291     }
00292     (*hi).first->begin_group();
00293   }
00294 
00295   bool traversal_done = false;
00296   if ((int)_colliders.size() <= CollisionLevelStateSingle::get_max_colliders() ||
00297       !allow_collider_multiple) {
00298     // Use the single-word-at-a-time traverser, which might need to make
00299     // lots of passes.
00300     LevelStatesSingle level_states;
00301     prepare_colliders_single(level_states, root);
00302 
00303     if (level_states.size() == 1 || !allow_collider_multiple) {
00304       traversal_done = true;
00305 
00306       // Make a number of passes, one for each group of 32 Colliders (or
00307       // whatever number of bits we have available in CurrentMask).
00308       for (size_t pass = 0; pass < level_states.size(); ++pass) {
00309 #ifdef DO_PSTATS
00310         PStatTimer pass_timer(get_pass_collector(pass));
00311 #endif
00312         r_traverse_single(level_states[pass], pass);
00313       }
00314     }
00315   }
00316 
00317   if (!traversal_done &&
00318       (int)_colliders.size() <= CollisionLevelStateDouble::get_max_colliders()) {
00319     // Try the double-word-at-a-time traverser.
00320     LevelStatesDouble level_states;
00321     prepare_colliders_double(level_states, root);
00322 
00323     if (level_states.size() == 1) {
00324       traversal_done = true;
00325 
00326       for (size_t pass = 0; pass < level_states.size(); ++pass) {
00327 #ifdef DO_PSTATS
00328         PStatTimer pass_timer(get_pass_collector(pass));
00329 #endif
00330         r_traverse_double(level_states[pass], pass);
00331       }
00332     }
00333   }
00334 
00335   if (!traversal_done) {
00336     // OK, do the quad-word-at-a-time traverser.
00337     LevelStatesQuad level_states;
00338     prepare_colliders_quad(level_states, root);
00339 
00340     traversal_done = true;
00341 
00342     for (size_t pass = 0; pass < level_states.size(); ++pass) {
00343 #ifdef DO_PSTATS
00344       PStatTimer pass_timer(get_pass_collector(pass));
00345 #endif
00346       r_traverse_quad(level_states[pass], pass);
00347     }
00348   }
00349 
00350   hi = _handlers.begin();
00351   while (hi != _handlers.end()) {
00352     if (!(*hi).first->end_group()) {
00353       // This handler wants to remove itself from the traversal list.
00354       hi = remove_handler(hi);
00355     } else {
00356       ++hi;
00357     }
00358   }
00359 
00360   #ifdef DO_COLLISION_RECORDING
00361   if (has_recorder()) {
00362     get_recorder()->end_traversal();
00363   }
00364   #endif  // DO_COLLISION_RECORDING
00365 
00366   CollisionLevelStateBase::_node_volume_pcollector.flush_level();
00367   _cnode_volume_pcollector.flush_level();
00368   _gnode_volume_pcollector.flush_level();
00369   _geom_volume_pcollector.flush_level();
00370 
00371   CollisionSphere::flush_level();
00372   CollisionTube::flush_level();
00373   CollisionPolygon::flush_level();
00374   CollisionPlane::flush_level();
00375   CollisionBox::flush_level();
00376 }
00377 
00378 #ifdef DO_COLLISION_RECORDING
00379 ////////////////////////////////////////////////////////////////////
00380 //     Function: CollisionTraverser::set_recorder
00381 //       Access: Published
00382 //  Description: Uses the indicated CollisionRecorder object to start
00383 //               recording the intersection tests made by each
00384 //               subsequent call to traverse() on this object.  A
00385 //               particular CollisionRecorder object can only record
00386 //               one traverser at a time; if this object has already
00387 //               been assigned to another traverser, that assignment
00388 //               is broken.
00389 //
00390 //               This is intended to be used in a debugging mode to
00391 //               try to determine what work is being performed by the
00392 //               collision traversal.  Usually, attaching a recorder
00393 //               will impose significant runtime overhead.
00394 //
00395 //               This does not transfer ownership of the
00396 //               CollisionRecorder pointer; maintenance of that
00397 //               remains the caller's responsibility.  If the
00398 //               CollisionRecorder is destructed, it will cleanly
00399 //               remove itself from the traverser.
00400 ////////////////////////////////////////////////////////////////////
00401 void CollisionTraverser::
00402 set_recorder(CollisionRecorder *recorder) {
00403   if (recorder != _recorder) {
00404     // Remove the old recorder, if any.
00405     if (_recorder != (CollisionRecorder *)NULL) {
00406       nassertv(_recorder->_trav == this);
00407       _recorder->_trav = (CollisionTraverser *)NULL;
00408     }
00409     
00410     _recorder = recorder;
00411     
00412     // Tell the new recorder about his new owner.
00413     if (_recorder != (CollisionRecorder *)NULL) {
00414       nassertv(_recorder->_trav != this);
00415       if (_recorder->_trav != (CollisionTraverser *)NULL) {
00416         _recorder->_trav->clear_recorder();
00417       }
00418       nassertv(_recorder->_trav == (CollisionTraverser *)NULL);
00419       _recorder->_trav = this;
00420     }
00421   }
00422 }
00423 
00424 ////////////////////////////////////////////////////////////////////
00425 //     Function: CollisionTraverser::show_collisions
00426 //       Access: Published
00427 //  Description: This is a high-level function to create a
00428 //               CollisionVisualizer object to render the collision
00429 //               tests performed by this traverser.  The supplied root
00430 //               should be any node in the scene graph; typically, the
00431 //               top node (e.g. render).  The CollisionVisualizer will
00432 //               be attached to this node.
00433 ////////////////////////////////////////////////////////////////////
00434 CollisionVisualizer *CollisionTraverser::
00435 show_collisions(const NodePath &root) {
00436   hide_collisions();
00437   CollisionVisualizer *viz = new CollisionVisualizer("show_collisions");
00438   _collision_visualizer_np = root.attach_new_node(viz);
00439   set_recorder(viz);
00440   return viz;
00441 }
00442 
00443 ////////////////////////////////////////////////////////////////////
00444 //     Function: CollisionTraverser::hide_collisions
00445 //       Access: Published
00446 //  Description: Undoes the effect of a previous call to
00447 //               show_collisions().
00448 ////////////////////////////////////////////////////////////////////
00449 void CollisionTraverser::
00450 hide_collisions() {
00451   if (!_collision_visualizer_np.is_empty()) {
00452     _collision_visualizer_np.remove_node();
00453   }
00454   clear_recorder();
00455 }
00456 
00457 #endif  // DO_COLLISION_RECORDING
00458 
00459 ////////////////////////////////////////////////////////////////////
00460 //     Function: CollisionTraverser::output
00461 //       Access: Published
00462 //  Description:
00463 ////////////////////////////////////////////////////////////////////
00464 void CollisionTraverser::
00465 output(ostream &out) const {
00466   out << "CollisionTraverser, " << _colliders.size()
00467       << " colliders and " << _handlers.size() << " handlers.\n";
00468 }
00469 
00470 ////////////////////////////////////////////////////////////////////
00471 //     Function: CollisionTraverser::write
00472 //       Access: Published
00473 //  Description:
00474 ////////////////////////////////////////////////////////////////////
00475 void CollisionTraverser::
00476 write(ostream &out, int indent_level) const {
00477   indent(out, indent_level)
00478     << "CollisionTraverser, " << _colliders.size()
00479     << " colliders and " << _handlers.size() << " handlers:\n";
00480 
00481   OrderedColliders::const_iterator oci;
00482   for (oci = _ordered_colliders.begin(); 
00483        oci != _ordered_colliders.end(); 
00484        ++oci) {
00485     NodePath cnode_path = (*oci)._node_path;
00486     bool in_graph = (*oci)._in_graph;
00487     
00488     Colliders::const_iterator ci;
00489     ci = _colliders.find(cnode_path);
00490     nassertv(ci != _colliders.end());
00491 
00492     CollisionHandler *handler = (*ci).second;
00493     nassertv(handler != (CollisionHandler *)NULL);
00494     
00495     indent(out, indent_level + 2)
00496       << cnode_path;
00497     if (in_graph) {
00498       out << " handled by " << handler->get_type() << "\n";
00499     } else {
00500       out << " ignored\n";
00501     }
00502 
00503     if (!cnode_path.is_empty() && cnode_path.node()->is_of_type(
00504         CollisionNode::get_class_type())) {
00505       CollisionNode *cnode = DCAST(CollisionNode, cnode_path.node());
00506       
00507       int num_solids = cnode->get_num_solids();
00508       for (int i = 0; i < num_solids; ++i) {
00509         cnode->get_solid(i)->write(out, indent_level + 4);
00510       }
00511     }
00512   }
00513 }
00514 
00515 ////////////////////////////////////////////////////////////////////
00516 //     Function: CollisionTraverser::prepare_colliders_single
00517 //       Access: Private
00518 //  Description: Fills up the set of LevelStates corresponding to the
00519 //               active colliders in use.
00520 //
00521 //               This flavor uses a CollisionLevelStateSingle, which is
00522 //               limited to a certain number of colliders per pass
00523 //               (typically 32).
00524 ////////////////////////////////////////////////////////////////////
00525 void CollisionTraverser::
00526 prepare_colliders_single(CollisionTraverser::LevelStatesSingle &level_states, 
00527                          const NodePath &root) {
00528   int num_colliders = _colliders.size();
00529   int max_colliders = CollisionLevelStateSingle::get_max_colliders();
00530 
00531   CollisionLevelStateSingle level_state(root);
00532   // This reserve() call is only correct if there is exactly one solid
00533   // per collider added to the traverser, which is the normal case.
00534   // If there is more than one solid in any of the colliders, this
00535   // reserve() call won't reserve enough, but the code is otherwise
00536   // correct.
00537   level_state.reserve(min(num_colliders, max_colliders));
00538 
00539   // Create an indirect index array to walk through the colliders in
00540   // sorted order, without affect the actual collider order.
00541   int *indirect = (int *)alloca(sizeof(int) * num_colliders);
00542   int i;
00543   for (i = 0; i < num_colliders; ++i) {
00544     indirect[i] = i;
00545   }
00546   sort(indirect, indirect + num_colliders, SortByColliderSort(*this));
00547 
00548   int num_remaining_colliders = num_colliders;
00549   for (i = 0; i < num_colliders; ++i) {
00550     OrderedColliderDef &ocd = _ordered_colliders[indirect[i]];
00551     NodePath cnode_path = ocd._node_path;
00552 
00553     if (!cnode_path.is_same_graph(root)) {
00554       if (ocd._in_graph) {
00555         // Only report this warning once.
00556         collide_cat.info()
00557           << "Collider " << cnode_path
00558           << " is not in scene graph.  Ignoring.\n";
00559         ocd._in_graph = false;
00560       }
00561 
00562     } else {
00563       ocd._in_graph = true;
00564       CollisionNode *cnode = DCAST(CollisionNode, cnode_path.node());
00565       
00566       CollisionLevelStateSingle::ColliderDef def;
00567       def._node = cnode;
00568       def._node_path = cnode_path;
00569       
00570       int num_solids = cnode->get_num_solids();
00571       for (int s = 0; s < num_solids; ++s) {
00572         CPT(CollisionSolid) collider = cnode->get_solid(s);
00573         def._collider = collider;
00574         level_state.prepare_collider(def, root);
00575 
00576         if (level_state.get_num_colliders() == max_colliders) {
00577           // That's the limit.  Save off this level state and make a
00578           // new one.
00579           level_states.push_back(level_state);
00580           level_state.clear();
00581           level_state.reserve(min(num_remaining_colliders, max_colliders));
00582         }
00583       }
00584     }
00585 
00586     --num_remaining_colliders;
00587     nassertv(num_remaining_colliders >= 0);
00588   }
00589 
00590   if (level_state.get_num_colliders() != 0) {
00591     level_states.push_back(level_state);
00592   }
00593   nassertv(num_remaining_colliders == 0);
00594 }
00595 
00596 ////////////////////////////////////////////////////////////////////
00597 //     Function: CollisionTraverser::r_traverse_single
00598 //       Access: Private
00599 //  Description:
00600 ////////////////////////////////////////////////////////////////////
00601 void CollisionTraverser::
00602 r_traverse_single(CollisionLevelStateSingle &level_state, size_t pass) {
00603   if (!level_state.any_in_bounds()) {
00604     return;
00605   }
00606   if (!level_state.apply_transform()) {
00607     return;
00608   }
00609 
00610   PandaNode *node = level_state.node();
00611   if (node->is_exact_type(CollisionNode::get_class_type())) {
00612     CollisionNode *cnode;
00613     DCAST_INTO_V(cnode, node);
00614     CPT(BoundingVolume) node_bv = cnode->get_bounds();
00615     const GeometricBoundingVolume *node_gbv = NULL;
00616     if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
00617       DCAST_INTO_V(node_gbv, node_bv);
00618     }
00619 
00620     CollisionEntry entry;
00621     entry._into_node = cnode;
00622     entry._into_node_path = level_state.get_node_path();
00623     if (_respect_prev_transform) {
00624       entry._flags |= CollisionEntry::F_respect_prev_transform;
00625     }
00626 
00627     int num_colliders = level_state.get_num_colliders();
00628     for (int c = 0; c < num_colliders; ++c) {
00629       if (level_state.has_collider(c)) {
00630         entry._from_node = level_state.get_collider_node(c);
00631 
00632         if ((entry._from_node->get_from_collide_mask() &
00633              cnode->get_into_collide_mask()) != 0) {
00634           #ifdef DO_PSTATS
00635           //PStatTimer collide_timer(_solid_collide_collectors[pass]);
00636           #endif
00637           entry._from_node_path = level_state.get_collider_node_path(c);
00638           entry._from = level_state.get_collider(c);
00639 
00640           compare_collider_to_node(
00641               entry, 
00642               level_state.get_parent_bound(c),
00643               level_state.get_local_bound(c),
00644               node_gbv);
00645         }
00646       }
00647     }
00648 
00649   } else if (node->is_geom_node()) {
00650     #ifndef NDEBUG
00651     if (collide_cat.is_spam()) {
00652       collide_cat.spam()
00653         << "Reached " << *node << "\n";
00654     }
00655     #endif
00656     
00657     GeomNode *gnode;
00658     DCAST_INTO_V(gnode, node);
00659     CPT(BoundingVolume) node_bv = gnode->get_bounds();
00660     const GeometricBoundingVolume *node_gbv = NULL;
00661     if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
00662       DCAST_INTO_V(node_gbv, node_bv);
00663     }
00664 
00665     CollisionEntry entry;
00666     entry._into_node = gnode;
00667     entry._into_node_path = level_state.get_node_path();
00668     if (_respect_prev_transform) {
00669       entry._flags |= CollisionEntry::F_respect_prev_transform;
00670     }
00671 
00672     int num_colliders = level_state.get_num_colliders();
00673     for (int c = 0; c < num_colliders; ++c) {
00674       if (level_state.has_collider(c)) {
00675         entry._from_node = level_state.get_collider_node(c);
00676 
00677         if ((entry._from_node->get_from_collide_mask() &
00678              gnode->get_into_collide_mask()) != 0) {
00679           #ifdef DO_PSTATS
00680           //PStatTimer collide_timer(_solid_collide_collectors[pass]);
00681           #endif
00682           entry._from_node_path = level_state.get_collider_node_path(c);
00683           entry._from = level_state.get_collider(c);
00684 
00685           compare_collider_to_geom_node(
00686               entry, 
00687               level_state.get_parent_bound(c),
00688               level_state.get_local_bound(c),
00689               node_gbv);
00690         }
00691       }
00692     }
00693   }
00694 
00695   if (node->has_single_child_visibility()) {
00696     // If it's a switch node or sequence node, visit just the one
00697     // visible child.
00698     int index = node->get_visible_child();
00699     if (index >= 0 && index < node->get_num_children()) {
00700       CollisionLevelStateSingle next_state(level_state, node->get_child(index));
00701       r_traverse_single(next_state, pass);
00702     }
00703 
00704   } else if (node->is_lod_node()) {
00705     // If it's an LODNode, visit the lowest level of detail with all
00706     // bits, allowing collision with geometry under the lowest level
00707     // of default; and visit all other levels without
00708     // GeomNode::get_default_collide_mask(), allowing only collision
00709     // with CollisionNodes and special geometry under higher levels of
00710     // detail.
00711     int index = DCAST(LODNode, node)->get_lowest_switch();
00712     PandaNode::Children children = node->get_children();
00713     int num_children = children.get_num_children();
00714     for (int i = 0; i < num_children; ++i) {
00715       CollisionLevelStateSingle next_state(level_state, children.get_child(i));
00716       if (i != index) {
00717         next_state.set_include_mask(next_state.get_include_mask() &
00718           ~GeomNode::get_default_collide_mask());
00719       }
00720       r_traverse_single(next_state, pass);
00721     }
00722 
00723   } else {
00724     // Otherwise, visit all the children.
00725     PandaNode::Children children = node->get_children();
00726     int num_children = children.get_num_children();
00727     for (int i = 0; i < num_children; ++i) {
00728       CollisionLevelStateSingle next_state(level_state, children.get_child(i));
00729       r_traverse_single(next_state, pass);
00730     }
00731   }
00732 }
00733 
00734 ////////////////////////////////////////////////////////////////////
00735 //     Function: CollisionTraverser::prepare_colliders_double
00736 //       Access: Private
00737 //  Description: Fills up the set of LevelStates corresponding to the
00738 //               active colliders in use.
00739 //
00740 //               This flavor uses a CollisionLevelStateDouble, which is
00741 //               limited to a certain number of colliders per pass
00742 //               (typically 32).
00743 ////////////////////////////////////////////////////////////////////
00744 void CollisionTraverser::
00745 prepare_colliders_double(CollisionTraverser::LevelStatesDouble &level_states, 
00746                          const NodePath &root) {
00747   int num_colliders = _colliders.size();
00748   int max_colliders = CollisionLevelStateDouble::get_max_colliders();
00749 
00750   CollisionLevelStateDouble level_state(root);
00751   // This reserve() call is only correct if there is exactly one solid
00752   // per collider added to the traverser, which is the normal case.
00753   // If there is more than one solid in any of the colliders, this
00754   // reserve() call won't reserve enough, but the code is otherwise
00755   // correct.
00756   level_state.reserve(min(num_colliders, max_colliders));
00757 
00758   // Create an indirect index array to walk through the colliders in
00759   // sorted order, without affect the actual collider order.
00760   int *indirect = (int *)alloca(sizeof(int) * num_colliders);
00761   int i;
00762   for (i = 0; i < num_colliders; ++i) {
00763     indirect[i] = i;
00764   }
00765   sort(indirect, indirect + num_colliders, SortByColliderSort(*this));
00766 
00767   int num_remaining_colliders = num_colliders;
00768   for (i = 0; i < num_colliders; ++i) {
00769     OrderedColliderDef &ocd = _ordered_colliders[indirect[i]];
00770     NodePath cnode_path = ocd._node_path;
00771 
00772     if (!cnode_path.is_same_graph(root)) {
00773       if (ocd._in_graph) {
00774         // Only report this warning once.
00775         collide_cat.info()
00776           << "Collider " << cnode_path
00777           << " is not in scene graph.  Ignoring.\n";
00778         ocd._in_graph = false;
00779       }
00780 
00781     } else {
00782       ocd._in_graph = true;
00783       CollisionNode *cnode = DCAST(CollisionNode, cnode_path.node());
00784       
00785       CollisionLevelStateDouble::ColliderDef def;
00786       def._node = cnode;
00787       def._node_path = cnode_path;
00788       
00789       int num_solids = cnode->get_num_solids();
00790       for (int s = 0; s < num_solids; ++s) {
00791         CPT(CollisionSolid) collider = cnode->get_solid(s);
00792         def._collider = collider;
00793         level_state.prepare_collider(def, root);
00794 
00795         if (level_state.get_num_colliders() == max_colliders) {
00796           // That's the limit.  Save off this level state and make a
00797           // new one.
00798           level_states.push_back(level_state);
00799           level_state.clear();
00800           level_state.reserve(min(num_remaining_colliders, max_colliders));
00801         }
00802       }
00803     }
00804 
00805     --num_remaining_colliders;
00806     nassertv(num_remaining_colliders >= 0);
00807   }
00808 
00809   if (level_state.get_num_colliders() != 0) {
00810     level_states.push_back(level_state);
00811   }
00812   nassertv(num_remaining_colliders == 0);
00813 }
00814 
00815 ////////////////////////////////////////////////////////////////////
00816 //     Function: CollisionTraverser::r_traverse_double
00817 //       Access: Private
00818 //  Description:
00819 ////////////////////////////////////////////////////////////////////
00820 void CollisionTraverser::
00821 r_traverse_double(CollisionLevelStateDouble &level_state, size_t pass) {
00822   if (!level_state.any_in_bounds()) {
00823     return;
00824   }
00825   if (!level_state.apply_transform()) {
00826     return;
00827   }
00828 
00829   PandaNode *node = level_state.node();
00830   if (node->is_exact_type(CollisionNode::get_class_type())) {
00831     CollisionNode *cnode;
00832     DCAST_INTO_V(cnode, node);
00833     CPT(BoundingVolume) node_bv = cnode->get_bounds();
00834     const GeometricBoundingVolume *node_gbv = NULL;
00835     if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
00836       DCAST_INTO_V(node_gbv, node_bv);
00837     }
00838 
00839     CollisionEntry entry;
00840     entry._into_node = cnode;
00841     entry._into_node_path = level_state.get_node_path();
00842     if (_respect_prev_transform) {
00843       entry._flags |= CollisionEntry::F_respect_prev_transform;
00844     }
00845 
00846     int num_colliders = level_state.get_num_colliders();
00847     for (int c = 0; c < num_colliders; ++c) {
00848       if (level_state.has_collider(c)) {
00849         entry._from_node = level_state.get_collider_node(c);
00850 
00851         if ((entry._from_node->get_from_collide_mask() &
00852              cnode->get_into_collide_mask()) != 0) {
00853           #ifdef DO_PSTATS
00854           //PStatTimer collide_timer(_solid_collide_collectors[pass]);
00855           #endif
00856           entry._from_node_path = level_state.get_collider_node_path(c);
00857           entry._from = level_state.get_collider(c);
00858 
00859           compare_collider_to_node(
00860               entry, 
00861               level_state.get_parent_bound(c),
00862               level_state.get_local_bound(c),
00863               node_gbv);
00864         }
00865       }
00866     }
00867 
00868   } else if (node->is_geom_node()) {
00869     #ifndef NDEBUG
00870     if (collide_cat.is_spam()) {
00871       collide_cat.spam()
00872         << "Reached " << *node << "\n";
00873     }
00874     #endif
00875     
00876     GeomNode *gnode;
00877     DCAST_INTO_V(gnode, node);
00878     CPT(BoundingVolume) node_bv = gnode->get_bounds();
00879     const GeometricBoundingVolume *node_gbv = NULL;
00880     if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
00881       DCAST_INTO_V(node_gbv, node_bv);
00882     }
00883 
00884     CollisionEntry entry;
00885     entry._into_node = gnode;
00886     entry._into_node_path = level_state.get_node_path();
00887     if (_respect_prev_transform) {
00888       entry._flags |= CollisionEntry::F_respect_prev_transform;
00889     }
00890 
00891     int num_colliders = level_state.get_num_colliders();
00892     for (int c = 0; c < num_colliders; ++c) {
00893       if (level_state.has_collider(c)) {
00894         entry._from_node = level_state.get_collider_node(c);
00895 
00896         if ((entry._from_node->get_from_collide_mask() &
00897              gnode->get_into_collide_mask()) != 0) {
00898           #ifdef DO_PSTATS
00899           //PStatTimer collide_timer(_solid_collide_collectors[pass]);
00900           #endif
00901           entry._from_node_path = level_state.get_collider_node_path(c);
00902           entry._from = level_state.get_collider(c);
00903 
00904           compare_collider_to_geom_node(
00905               entry, 
00906               level_state.get_parent_bound(c),
00907               level_state.get_local_bound(c),
00908               node_gbv);
00909         }
00910       }
00911     }
00912   }
00913 
00914   if (node->has_single_child_visibility()) {
00915     // If it's a switch node or sequence node, visit just the one
00916     // visible child.
00917     int index = node->get_visible_child();
00918     if (index >= 0 && index < node->get_num_children()) {
00919       CollisionLevelStateDouble next_state(level_state, node->get_child(index));
00920       r_traverse_double(next_state, pass);
00921     }
00922 
00923   } else if (node->is_lod_node()) {
00924     // If it's an LODNode, visit the lowest level of detail with all
00925     // bits, allowing collision with geometry under the lowest level
00926     // of default; and visit all other levels without
00927     // GeomNode::get_default_collide_mask(), allowing only collision
00928     // with CollisionNodes and special geometry under higher levels of
00929     // detail.
00930     int index = DCAST(LODNode, node)->get_lowest_switch();
00931     PandaNode::Children children = node->get_children();
00932     int num_children = children.get_num_children();
00933     for (int i = 0; i < num_children; ++i) {
00934       CollisionLevelStateDouble next_state(level_state, children.get_child(i));
00935       if (i != index) {
00936         next_state.set_include_mask(next_state.get_include_mask() &
00937           ~GeomNode::get_default_collide_mask());
00938       }
00939       r_traverse_double(next_state, pass);
00940     }
00941 
00942   } else {
00943     // Otherwise, visit all the children.
00944     PandaNode::Children children = node->get_children();
00945     int num_children = children.get_num_children();
00946     for (int i = 0; i < num_children; ++i) {
00947       CollisionLevelStateDouble next_state(level_state, children.get_child(i));
00948       r_traverse_double(next_state, pass);
00949     }
00950   }
00951 }
00952 
00953 ////////////////////////////////////////////////////////////////////
00954 //     Function: CollisionTraverser::prepare_colliders_quad
00955 //       Access: Private
00956 //  Description: Fills up the set of LevelStates corresponding to the
00957 //               active colliders in use.
00958 //
00959 //               This flavor uses a CollisionLevelStateQuad, which is
00960 //               limited to a certain number of colliders per pass
00961 //               (typically 32).
00962 ////////////////////////////////////////////////////////////////////
00963 void CollisionTraverser::
00964 prepare_colliders_quad(CollisionTraverser::LevelStatesQuad &level_states, 
00965                          const NodePath &root) {
00966   int num_colliders = _colliders.size();
00967   int max_colliders = CollisionLevelStateQuad::get_max_colliders();
00968 
00969   CollisionLevelStateQuad level_state(root);
00970   // This reserve() call is only correct if there is exactly one solid
00971   // per collider added to the traverser, which is the normal case.
00972   // If there is more than one solid in any of the colliders, this
00973   // reserve() call won't reserve enough, but the code is otherwise
00974   // correct.
00975   level_state.reserve(min(num_colliders, max_colliders));
00976 
00977   // Create an indirect index array to walk through the colliders in
00978   // sorted order, without affect the actual collider order.
00979   int *indirect = (int *)alloca(sizeof(int) * num_colliders);
00980   int i;
00981   for (i = 0; i < num_colliders; ++i) {
00982     indirect[i] = i;
00983   }
00984   sort(indirect, indirect + num_colliders, SortByColliderSort(*this));
00985 
00986   int num_remaining_colliders = num_colliders;
00987   for (i = 0; i < num_colliders; ++i) {
00988     OrderedColliderDef &ocd = _ordered_colliders[indirect[i]];
00989     NodePath cnode_path = ocd._node_path;
00990 
00991     if (!cnode_path.is_same_graph(root)) {
00992       if (ocd._in_graph) {
00993         // Only report this warning once.
00994         collide_cat.info()
00995           << "Collider " << cnode_path
00996           << " is not in scene graph.  Ignoring.\n";
00997         ocd._in_graph = false;
00998       }
00999 
01000     } else {
01001       ocd._in_graph = true;
01002       CollisionNode *cnode = DCAST(CollisionNode, cnode_path.node());
01003       
01004       CollisionLevelStateQuad::ColliderDef def;
01005       def._node = cnode;
01006       def._node_path = cnode_path;
01007       
01008       int num_solids = cnode->get_num_solids();
01009       for (int s = 0; s < num_solids; ++s) {
01010         CPT(CollisionSolid) collider = cnode->get_solid(s);
01011         def._collider = collider;
01012         level_state.prepare_collider(def, root);
01013 
01014         if (level_state.get_num_colliders() == max_colliders) {
01015           // That's the limit.  Save off this level state and make a
01016           // new one.
01017           level_states.push_back(level_state);
01018           level_state.clear();
01019           level_state.reserve(min(num_remaining_colliders, max_colliders));
01020         }
01021       }
01022     }
01023 
01024     --num_remaining_colliders;
01025     nassertv(num_remaining_colliders >= 0);
01026   }
01027 
01028   if (level_state.get_num_colliders() != 0) {
01029     level_states.push_back(level_state);
01030   }
01031   nassertv(num_remaining_colliders == 0);
01032 }
01033 
01034 ////////////////////////////////////////////////////////////////////
01035 //     Function: CollisionTraverser::r_traverse_quad
01036 //       Access: Private
01037 //  Description:
01038 ////////////////////////////////////////////////////////////////////
01039 void CollisionTraverser::
01040 r_traverse_quad(CollisionLevelStateQuad &level_state, size_t pass) {
01041   if (!level_state.any_in_bounds()) {
01042     return;
01043   }
01044   if (!level_state.apply_transform()) {
01045     return;
01046   }
01047 
01048   PandaNode *node = level_state.node();
01049   if (node->is_exact_type(CollisionNode::get_class_type())) {
01050     CollisionNode *cnode;
01051     DCAST_INTO_V(cnode, node);
01052     CPT(BoundingVolume) node_bv = cnode->get_bounds();
01053     const GeometricBoundingVolume *node_gbv = NULL;
01054     if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
01055       DCAST_INTO_V(node_gbv, node_bv);
01056     }
01057 
01058     CollisionEntry entry;
01059     entry._into_node = cnode;
01060     entry._into_node_path = level_state.get_node_path();
01061     if (_respect_prev_transform) {
01062       entry._flags |= CollisionEntry::F_respect_prev_transform;
01063     }
01064 
01065     int num_colliders = level_state.get_num_colliders();
01066     for (int c = 0; c < num_colliders; ++c) {
01067       if (level_state.has_collider(c)) {
01068         entry._from_node = level_state.get_collider_node(c);
01069 
01070         if ((entry._from_node->get_from_collide_mask() &
01071              cnode->get_into_collide_mask()) != 0) {
01072           #ifdef DO_PSTATS
01073           //PStatTimer collide_timer(_solid_collide_collectors[pass]);
01074           #endif
01075           entry._from_node_path = level_state.get_collider_node_path(c);
01076           entry._from = level_state.get_collider(c);
01077 
01078           compare_collider_to_node(
01079               entry, 
01080               level_state.get_parent_bound(c),
01081               level_state.get_local_bound(c),
01082               node_gbv);
01083         }
01084       }
01085     }
01086 
01087   } else if (node->is_geom_node()) {
01088     #ifndef NDEBUG
01089     if (collide_cat.is_spam()) {
01090       collide_cat.spam()
01091         << "Reached " << *node << "\n";
01092     }
01093     #endif
01094     
01095     GeomNode *gnode;
01096     DCAST_INTO_V(gnode, node);
01097     CPT(BoundingVolume) node_bv = gnode->get_bounds();
01098     const GeometricBoundingVolume *node_gbv = NULL;
01099     if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
01100       DCAST_INTO_V(node_gbv, node_bv);
01101     }
01102 
01103     CollisionEntry entry;
01104     entry._into_node = gnode;
01105     entry._into_node_path = level_state.get_node_path();
01106     if (_respect_prev_transform) {
01107       entry._flags |= CollisionEntry::F_respect_prev_transform;
01108     }
01109 
01110     int num_colliders = level_state.get_num_colliders();
01111     for (int c = 0; c < num_colliders; ++c) {
01112       if (level_state.has_collider(c)) {
01113         entry._from_node = level_state.get_collider_node(c);
01114 
01115         if ((entry._from_node->get_from_collide_mask() &
01116              gnode->get_into_collide_mask()) != 0) {
01117           #ifdef DO_PSTATS
01118           //PStatTimer collide_timer(_solid_collide_collectors[pass]);
01119           #endif
01120           entry._from_node_path = level_state.get_collider_node_path(c);
01121           entry._from = level_state.get_collider(c);
01122 
01123           compare_collider_to_geom_node(
01124               entry, 
01125               level_state.get_parent_bound(c),
01126               level_state.get_local_bound(c),
01127               node_gbv);
01128         }
01129       }
01130     }
01131   }
01132 
01133   if (node->has_single_child_visibility()) {
01134     // If it's a switch node or sequence node, visit just the one
01135     // visible child.
01136     int index = node->get_visible_child();
01137     if (index >= 0 && index < node->get_num_children()) {
01138       CollisionLevelStateQuad next_state(level_state, node->get_child(index));
01139       r_traverse_quad(next_state, pass);
01140     }
01141 
01142   } else if (node->is_lod_node()) {
01143     // If it's an LODNode, visit the lowest level of detail with all
01144     // bits, allowing collision with geometry under the lowest level
01145     // of default; and visit all other levels without
01146     // GeomNode::get_default_collide_mask(), allowing only collision
01147     // with CollisionNodes and special geometry under higher levels of
01148     // detail.
01149     int index = DCAST(LODNode, node)->get_lowest_switch();
01150     PandaNode::Children children = node->get_children();
01151     int num_children = children.get_num_children();
01152     for (int i = 0; i < num_children; ++i) {
01153       CollisionLevelStateQuad next_state(level_state, children.get_child(i));
01154       if (i != index) {
01155         next_state.set_include_mask(next_state.get_include_mask() &
01156           ~GeomNode::get_default_collide_mask());
01157       }
01158       r_traverse_quad(next_state, pass);
01159     }
01160 
01161   } else {
01162     // Otherwise, visit all the children.
01163     PandaNode::Children children = node->get_children();
01164     int num_children = children.get_num_children();
01165     for (int i = 0; i < num_children; ++i) {
01166       CollisionLevelStateQuad next_state(level_state, children.get_child(i));
01167       r_traverse_quad(next_state, pass);
01168     }
01169   }
01170 }
01171 
01172 ////////////////////////////////////////////////////////////////////
01173 //     Function: CollisionTraverser::compare_collider_to_node
01174 //       Access: Private
01175 //  Description:
01176 ////////////////////////////////////////////////////////////////////
01177 void CollisionTraverser::
01178 compare_collider_to_node(CollisionEntry &entry,
01179                          const GeometricBoundingVolume *from_parent_gbv,
01180                          const GeometricBoundingVolume *from_node_gbv,
01181                          const GeometricBoundingVolume *into_node_gbv) {
01182   bool within_node_bounds = true;
01183   if (from_parent_gbv != (GeometricBoundingVolume *)NULL &&
01184       into_node_gbv != (GeometricBoundingVolume *)NULL) {
01185     within_node_bounds = (into_node_gbv->contains(from_parent_gbv) != 0);
01186     _cnode_volume_pcollector.add_level(1);
01187   }
01188 
01189   if (within_node_bounds) {
01190     CollisionNode *cnode;
01191     DCAST_INTO_V(cnode, entry._into_node);
01192     int num_solids = cnode->get_num_solids();
01193     collide_cat.spam()
01194       << "Colliding against CollisionNode " << entry._into_node
01195       << " which has " << num_solids << " collision solids.\n";
01196     for (int s = 0; s < num_solids; ++s) {
01197       entry._into = cnode->get_solid(s);
01198       if (entry._from != entry._into) {
01199         CPT(BoundingVolume) solid_bv = entry._into->get_bounds();
01200         const GeometricBoundingVolume *solid_gbv = NULL;
01201         if (num_solids > 1 &&
01202             solid_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
01203           // Only bother to test against each solid's bounding
01204           // volume if we have more than one solid in the node, as a
01205           // slight optimization.  (If the node contains just one
01206           // solid, then the node's bounding volume, which we just
01207           // tested, is the same as the solid's bounding volume.)
01208           DCAST_INTO_V(solid_gbv, solid_bv);
01209         }
01210 
01211         compare_collider_to_solid(entry, from_node_gbv, solid_gbv);
01212       }
01213     }
01214   }
01215 }
01216 
01217 ////////////////////////////////////////////////////////////////////
01218 //     Function: CollisionTraverser::compare_collider_to_geom_node
01219 //       Access: Private
01220 //  Description:
01221 ////////////////////////////////////////////////////////////////////
01222 void CollisionTraverser::
01223 compare_collider_to_geom_node(CollisionEntry &entry,
01224                               const GeometricBoundingVolume *from_parent_gbv,
01225                               const GeometricBoundingVolume *from_node_gbv,
01226                               const GeometricBoundingVolume *into_node_gbv) {
01227   bool within_node_bounds = true;
01228   if (from_parent_gbv != (GeometricBoundingVolume *)NULL &&
01229       into_node_gbv != (GeometricBoundingVolume *)NULL) {
01230     within_node_bounds = (into_node_gbv->contains(from_parent_gbv) != 0);
01231     _gnode_volume_pcollector.add_level(1);
01232   }
01233 
01234   if (within_node_bounds) {
01235     GeomNode *gnode;
01236     DCAST_INTO_V(gnode, entry._into_node);
01237     int num_geoms = gnode->get_num_geoms();
01238     for (int s = 0; s < num_geoms; ++s) {
01239       entry._into = (CollisionSolid *)NULL;
01240       const Geom *geom = DCAST(Geom, gnode->get_geom(s));
01241       if (geom != (Geom *)NULL) {
01242         CPT(BoundingVolume) geom_bv = geom->get_bounds();
01243         const GeometricBoundingVolume *geom_gbv = NULL;
01244         if (num_geoms > 1 &&
01245             geom_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
01246           // Only bother to test against each geom's bounding
01247           // volume if we have more than one geom in the node, as a
01248           // slight optimization.  (If the node contains just one
01249           // geom, then the node's bounding volume, which we just
01250           // tested, is the same as the geom's bounding volume.)
01251           DCAST_INTO_V(geom_gbv, geom_bv);
01252         }
01253 
01254         compare_collider_to_geom(entry, geom, from_node_gbv, geom_gbv);
01255       }
01256     }
01257   }
01258 }
01259 
01260 ////////////////////////////////////////////////////////////////////
01261 //     Function: CollisionTraverser::compare_collider_to_solid
01262 //       Access: Private
01263 //  Description:
01264 ////////////////////////////////////////////////////////////////////
01265 void CollisionTraverser::
01266 compare_collider_to_solid(CollisionEntry &entry,
01267                           const GeometricBoundingVolume *from_node_gbv,
01268                           const GeometricBoundingVolume *solid_gbv) {
01269   bool within_solid_bounds = true;
01270   if (from_node_gbv != (GeometricBoundingVolume *)NULL &&
01271       solid_gbv != (GeometricBoundingVolume *)NULL) {
01272     within_solid_bounds = (solid_gbv->contains(from_node_gbv) != 0);
01273     #ifdef DO_PSTATS
01274     ((CollisionSolid *)entry.get_into())->get_volume_pcollector().add_level(1);
01275     #endif  // DO_PSTATS
01276 #ifndef NDEBUG
01277     if (collide_cat.is_spam()) {
01278       collide_cat.spam(false)
01279         << "Comparing to solid: " << *from_node_gbv
01280         << " to " << *solid_gbv << ", within_solid_bounds = " 
01281         << within_solid_bounds << "\n";
01282     }
01283 #endif  // NDEBUG
01284   }
01285   if (within_solid_bounds) {
01286     Colliders::const_iterator ci;
01287     ci = _colliders.find(entry.get_from_node_path());
01288     nassertv(ci != _colliders.end());
01289     entry.test_intersection((*ci).second, this);
01290   }
01291 }
01292 
01293 ////////////////////////////////////////////////////////////////////
01294 //     Function: CollisionTraverser::compare_collider_to_geom
01295 //       Access: Private
01296 //  Description:
01297 ////////////////////////////////////////////////////////////////////
01298 void CollisionTraverser::
01299 compare_collider_to_geom(CollisionEntry &entry, const Geom *geom,
01300                          const GeometricBoundingVolume *from_node_gbv,
01301                          const GeometricBoundingVolume *geom_gbv) {
01302   bool within_geom_bounds = true;
01303   if (from_node_gbv != (GeometricBoundingVolume *)NULL &&
01304       geom_gbv != (GeometricBoundingVolume *)NULL) {
01305     within_geom_bounds = (geom_gbv->contains(from_node_gbv) != 0);
01306     _geom_volume_pcollector.add_level(1);
01307   }
01308   if (within_geom_bounds) {
01309     Colliders::const_iterator ci;
01310     ci = _colliders.find(entry.get_from_node_path());
01311     nassertv(ci != _colliders.end());
01312 
01313     if (geom->get_primitive_type() == Geom::PT_polygons) {
01314       Thread *current_thread = Thread::get_current_thread();
01315       CPT(GeomVertexData) data = geom->get_vertex_data()->animate_vertices(true, current_thread);
01316       GeomVertexReader vertex(data, InternalName::get_vertex());
01317       
01318       int num_primitives = geom->get_num_primitives();
01319       for (int i = 0; i < num_primitives; ++i) {
01320         const GeomPrimitive *primitive = geom->get_primitive(i);
01321         CPT(GeomPrimitive) tris = primitive->decompose();
01322         nassertv(tris->is_of_type(GeomTriangles::get_class_type()));
01323 
01324         if (tris->is_indexed()) {
01325           // Indexed case.
01326           GeomVertexReader index(tris->get_vertices(), 0);
01327           while (!index.is_at_end()) {
01328             Vertexf v[3];
01329             
01330             vertex.set_row(index.get_data1i());
01331             v[0] = vertex.get_data3f();
01332             vertex.set_row(index.get_data1i());
01333             v[1] = vertex.get_data3f();
01334             vertex.set_row(index.get_data1i());
01335             v[2] = vertex.get_data3f();
01336             
01337             // Generate a temporary CollisionGeom on the fly for each
01338             // triangle in the Geom.
01339             if (CollisionPolygon::verify_points(v[0], v[1], v[2])) {
01340               bool within_solid_bounds = true;
01341               if (from_node_gbv != (GeometricBoundingVolume *)NULL) {
01342                 PT(BoundingSphere) sphere = new BoundingSphere;
01343                 sphere->around(v, v + 3);
01344                 within_solid_bounds = (sphere->contains(from_node_gbv) != 0);
01345 #ifdef DO_PSTATS
01346                 CollisionGeom::_volume_pcollector.add_level(1);
01347 #endif  // DO_PSTATS
01348               }
01349               if (within_solid_bounds) {
01350                 PT(CollisionGeom) cgeom = new CollisionGeom(v[0], v[1], v[2]);
01351                 entry._into = cgeom;
01352                 entry.test_intersection((*ci).second, this);
01353               }
01354             }
01355           }
01356         } else {
01357           // Non-indexed case.
01358           vertex.set_row(primitive->get_first_vertex());
01359           int num_vertices = primitive->get_num_vertices();
01360           for (int i = 0; i < num_vertices; i += 3) {
01361             Vertexf v[3];
01362             
01363             v[0] = vertex.get_data3f();
01364             v[1] = vertex.get_data3f();
01365             v[2] = vertex.get_data3f();
01366             
01367             // Generate a temporary CollisionGeom on the fly for each
01368             // triangle in the Geom.
01369             if (CollisionPolygon::verify_points(v[0], v[1], v[2])) {
01370               bool within_solid_bounds = true;
01371               if (from_node_gbv != (GeometricBoundingVolume *)NULL) {
01372                 PT(BoundingSphere) sphere = new BoundingSphere;
01373                 sphere->around(v, v + 3);
01374                 within_solid_bounds = (sphere->contains(from_node_gbv) != 0);
01375 #ifdef DO_PSTATS
01376                 CollisionGeom::_volume_pcollector.add_level(1);
01377 #endif  // DO_PSTATS
01378               }
01379               if (within_solid_bounds) {
01380                 PT(CollisionGeom) cgeom = new CollisionGeom(v[0], v[1], v[2]);
01381                 entry._into = cgeom;
01382                 entry.test_intersection((*ci).second, this);
01383               }
01384             }
01385           }
01386         }
01387       }
01388     }
01389   }
01390 }
01391 
01392 ////////////////////////////////////////////////////////////////////
01393 //     Function: CollisionTraverser::remove_handler
01394 //       Access: Private
01395 //  Description: Removes the indicated CollisionHandler from the list
01396 //               of handlers to be processed, and returns the iterator
01397 //               to the next handler in the list.  This is designed to
01398 //               be called safely from within a traversal of the handler
01399 //               list.
01400 //
01401 //               This also removes any colliders that depend on this
01402 //               handler, to keep internal structures intact.
01403 ////////////////////////////////////////////////////////////////////
01404 CollisionTraverser::Handlers::iterator CollisionTraverser::
01405 remove_handler(CollisionTraverser::Handlers::iterator hi) {
01406   nassertr(hi != _handlers.end(), hi);
01407 
01408   CollisionHandler *handler = (*hi).first;
01409   Handlers::iterator hnext = hi;
01410   ++hnext;
01411   _handlers.erase(hi);
01412   hi = hnext;
01413 
01414   // Now scan for colliders that reference this handler.
01415   Colliders::iterator ci;
01416   ci = _colliders.begin();
01417   while (ci != _colliders.end()) {
01418     if ((*ci).second == handler) {
01419       // This collider references this handler; remove it.
01420       NodePath node_path = (*ci).first;
01421 
01422       Colliders::iterator cnext = ci;
01423       ++cnext;
01424       _colliders.erase(ci);
01425       ci = cnext;
01426 
01427       // Also remove it from the ordered list.
01428       OrderedColliders::iterator oci;
01429       oci = _ordered_colliders.begin();
01430       while (oci != _ordered_colliders.end() &&
01431              (*oci)._node_path != node_path) {
01432         ++oci;
01433       }
01434       nassertr(oci != _ordered_colliders.end(), hi);
01435       _ordered_colliders.erase(oci);
01436       
01437       nassertr(_ordered_colliders.size() == _colliders.size(), hi);
01438 
01439     } else {
01440       // This collider references some other handler; keep it.
01441       ++ci;
01442     }
01443   }
01444 
01445   return hi;
01446 }
01447 
01448 ////////////////////////////////////////////////////////////////////
01449 //     Function: CollisionTraverser::get_pass_collector
01450 //       Access: Private
01451 //  Description: Returns the PStatCollector suitable for timing the
01452 //               nth pass.
01453 ////////////////////////////////////////////////////////////////////
01454 PStatCollector &CollisionTraverser::
01455 get_pass_collector(int pass) {
01456   nassertr(pass >= 0, _this_pcollector);
01457   while ((int)_pass_collectors.size() <= pass) {
01458     ostringstream name;
01459     name << "pass" << (_pass_collectors.size() + 1);
01460     PStatCollector col(_this_pcollector, name.str());
01461     _pass_collectors.push_back(col);
01462     PStatCollector sc_col(col, "solid_collide");
01463     _solid_collide_collectors.push_back(sc_col);
01464   }
01465 
01466   return _pass_collectors[pass];
01467 }
 All Classes Functions Variables Enumerations