00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
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
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
00069
00070
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
00085
00086
00087
00088 CollisionTraverser::
00089 ~CollisionTraverser() {
00090 #ifdef DO_COLLISION_RECORDING
00091 clear_recorder();
00092 #endif
00093 }
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
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_collision_node());
00113 nassertv(handler != (CollisionHandler *)NULL);
00114
00115 Colliders::iterator ci = _colliders.find(collider);
00116 if (ci != _colliders.end()) {
00117
00118 if ((*ci).second != handler) {
00119
00120 PT(CollisionHandler) old_handler = (*ci).second;
00121 (*ci).second = handler;
00122
00123
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
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
00161
00162
00163
00164
00165
00166
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
00175 return false;
00176 }
00177
00178 CollisionHandler *handler = (*ci).second;
00179
00180
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
00207
00208
00209
00210
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
00220
00221
00222
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
00232
00233
00234
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
00245
00246
00247
00248
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
00261
00262
00263
00264
00265 void CollisionTraverser::
00266 clear_colliders() {
00267 _colliders.clear();
00268 _ordered_colliders.clear();
00269 _handlers.clear();
00270 }
00271
00272
00273
00274
00275
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
00299
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
00307
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
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
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
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
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401 void CollisionTraverser::
00402 set_recorder(CollisionRecorder *recorder) {
00403 if (recorder != _recorder) {
00404
00405 if (_recorder != (CollisionRecorder *)NULL) {
00406 nassertv(_recorder->_trav == this);
00407 _recorder->_trav = (CollisionTraverser *)NULL;
00408 }
00409
00410 _recorder = recorder;
00411
00412
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
00426
00427
00428
00429
00430
00431
00432
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
00445
00446
00447
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
00461
00462
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
00472
00473
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_collision_node()) {
00504 CollisionNode *cnode = DCAST(CollisionNode, cnode_path.node());
00505
00506 int num_solids = cnode->get_num_solids();
00507 for (int i = 0; i < num_solids; ++i) {
00508 cnode->get_solid(i)->write(out, indent_level + 4);
00509 }
00510 }
00511 }
00512 }
00513
00514
00515
00516
00517
00518
00519
00520
00521
00522
00523
00524 void CollisionTraverser::
00525 prepare_colliders_single(CollisionTraverser::LevelStatesSingle &level_states,
00526 const NodePath &root) {
00527 int num_colliders = _colliders.size();
00528 int max_colliders = CollisionLevelStateSingle::get_max_colliders();
00529
00530 CollisionLevelStateSingle level_state(root);
00531
00532
00533
00534
00535
00536 level_state.reserve(min(num_colliders, max_colliders));
00537
00538
00539
00540 int *indirect = (int *)alloca(sizeof(int) * num_colliders);
00541 int i;
00542 for (i = 0; i < num_colliders; ++i) {
00543 indirect[i] = i;
00544 }
00545 sort(indirect, indirect + num_colliders, SortByColliderSort(*this));
00546
00547 int num_remaining_colliders = num_colliders;
00548 for (i = 0; i < num_colliders; ++i) {
00549 OrderedColliderDef &ocd = _ordered_colliders[indirect[i]];
00550 NodePath cnode_path = ocd._node_path;
00551
00552 if (!cnode_path.is_same_graph(root)) {
00553 if (ocd._in_graph) {
00554
00555 collide_cat.info()
00556 << "Collider " << cnode_path
00557 << " is not in scene graph. Ignoring.\n";
00558 ocd._in_graph = false;
00559 }
00560
00561 } else {
00562 ocd._in_graph = true;
00563 CollisionNode *cnode = DCAST(CollisionNode, cnode_path.node());
00564
00565 CollisionLevelStateSingle::ColliderDef def;
00566 def._node = cnode;
00567 def._node_path = cnode_path;
00568
00569 int num_solids = cnode->get_num_solids();
00570 for (int s = 0; s < num_solids; ++s) {
00571 CPT(CollisionSolid) collider = cnode->get_solid(s);
00572 def._collider = collider;
00573 level_state.prepare_collider(def, root);
00574
00575 if (level_state.get_num_colliders() == max_colliders) {
00576
00577
00578 level_states.push_back(level_state);
00579 level_state.clear();
00580 level_state.reserve(min(num_remaining_colliders, max_colliders));
00581 }
00582 }
00583 }
00584
00585 --num_remaining_colliders;
00586 nassertv(num_remaining_colliders >= 0);
00587 }
00588
00589 if (level_state.get_num_colliders() != 0) {
00590 level_states.push_back(level_state);
00591 }
00592 nassertv(num_remaining_colliders == 0);
00593 }
00594
00595
00596
00597
00598
00599
00600 void CollisionTraverser::
00601 r_traverse_single(CollisionLevelStateSingle &level_state, size_t pass) {
00602 if (!level_state.any_in_bounds()) {
00603 return;
00604 }
00605 if (!level_state.apply_transform()) {
00606 return;
00607 }
00608
00609 PandaNode *node = level_state.node();
00610 if (node->is_collision_node()) {
00611 CollisionNode *cnode;
00612 DCAST_INTO_V(cnode, node);
00613 CPT(BoundingVolume) node_bv = cnode->get_bounds();
00614 const GeometricBoundingVolume *node_gbv = NULL;
00615 if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
00616 DCAST_INTO_V(node_gbv, node_bv);
00617 }
00618
00619 CollisionEntry entry;
00620 entry._into_node = cnode;
00621 entry._into_node_path = level_state.get_node_path();
00622 if (_respect_prev_transform) {
00623 entry._flags |= CollisionEntry::F_respect_prev_transform;
00624 }
00625
00626 int num_colliders = level_state.get_num_colliders();
00627 for (int c = 0; c < num_colliders; ++c) {
00628 if (level_state.has_collider(c)) {
00629 entry._from_node = level_state.get_collider_node(c);
00630
00631 if ((entry._from_node->get_from_collide_mask() &
00632 cnode->get_into_collide_mask()) != 0) {
00633 #ifdef DO_PSTATS
00634
00635 #endif
00636 entry._from_node_path = level_state.get_collider_node_path(c);
00637 entry._from = level_state.get_collider(c);
00638
00639 compare_collider_to_node(
00640 entry,
00641 level_state.get_parent_bound(c),
00642 level_state.get_local_bound(c),
00643 node_gbv);
00644 }
00645 }
00646 }
00647
00648 } else if (node->is_geom_node()) {
00649 #ifndef NDEBUG
00650 if (collide_cat.is_spam()) {
00651 collide_cat.spam()
00652 << "Reached " << *node << "\n";
00653 }
00654 #endif
00655
00656 GeomNode *gnode;
00657 DCAST_INTO_V(gnode, node);
00658 CPT(BoundingVolume) node_bv = gnode->get_bounds();
00659 const GeometricBoundingVolume *node_gbv = NULL;
00660 if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
00661 DCAST_INTO_V(node_gbv, node_bv);
00662 }
00663
00664 CollisionEntry entry;
00665 entry._into_node = gnode;
00666 entry._into_node_path = level_state.get_node_path();
00667 if (_respect_prev_transform) {
00668 entry._flags |= CollisionEntry::F_respect_prev_transform;
00669 }
00670
00671 int num_colliders = level_state.get_num_colliders();
00672 for (int c = 0; c < num_colliders; ++c) {
00673 if (level_state.has_collider(c)) {
00674 entry._from_node = level_state.get_collider_node(c);
00675
00676 if ((entry._from_node->get_from_collide_mask() &
00677 gnode->get_into_collide_mask()) != 0) {
00678 #ifdef DO_PSTATS
00679
00680 #endif
00681 entry._from_node_path = level_state.get_collider_node_path(c);
00682 entry._from = level_state.get_collider(c);
00683
00684 compare_collider_to_geom_node(
00685 entry,
00686 level_state.get_parent_bound(c),
00687 level_state.get_local_bound(c),
00688 node_gbv);
00689 }
00690 }
00691 }
00692 }
00693
00694 if (node->has_single_child_visibility()) {
00695
00696
00697 int index = node->get_visible_child();
00698 if (index >= 0 && index < node->get_num_children()) {
00699 CollisionLevelStateSingle next_state(level_state, node->get_child(index));
00700 r_traverse_single(next_state, pass);
00701 }
00702
00703 } else if (node->is_lod_node()) {
00704
00705
00706
00707
00708
00709
00710 int index = DCAST(LODNode, node)->get_lowest_switch();
00711 PandaNode::Children children = node->get_children();
00712 int num_children = children.get_num_children();
00713 for (int i = 0; i < num_children; ++i) {
00714 CollisionLevelStateSingle next_state(level_state, children.get_child(i));
00715 if (i != index) {
00716 next_state.set_include_mask(next_state.get_include_mask() &
00717 ~GeomNode::get_default_collide_mask());
00718 }
00719 r_traverse_single(next_state, pass);
00720 }
00721
00722 } else {
00723
00724 PandaNode::Children children = node->get_children();
00725 int num_children = children.get_num_children();
00726 for (int i = 0; i < num_children; ++i) {
00727 CollisionLevelStateSingle next_state(level_state, children.get_child(i));
00728 r_traverse_single(next_state, pass);
00729 }
00730 }
00731 }
00732
00733
00734
00735
00736
00737
00738
00739
00740
00741
00742
00743 void CollisionTraverser::
00744 prepare_colliders_double(CollisionTraverser::LevelStatesDouble &level_states,
00745 const NodePath &root) {
00746 int num_colliders = _colliders.size();
00747 int max_colliders = CollisionLevelStateDouble::get_max_colliders();
00748
00749 CollisionLevelStateDouble level_state(root);
00750
00751
00752
00753
00754
00755 level_state.reserve(min(num_colliders, max_colliders));
00756
00757
00758
00759 int *indirect = (int *)alloca(sizeof(int) * num_colliders);
00760 int i;
00761 for (i = 0; i < num_colliders; ++i) {
00762 indirect[i] = i;
00763 }
00764 sort(indirect, indirect + num_colliders, SortByColliderSort(*this));
00765
00766 int num_remaining_colliders = num_colliders;
00767 for (i = 0; i < num_colliders; ++i) {
00768 OrderedColliderDef &ocd = _ordered_colliders[indirect[i]];
00769 NodePath cnode_path = ocd._node_path;
00770
00771 if (!cnode_path.is_same_graph(root)) {
00772 if (ocd._in_graph) {
00773
00774 collide_cat.info()
00775 << "Collider " << cnode_path
00776 << " is not in scene graph. Ignoring.\n";
00777 ocd._in_graph = false;
00778 }
00779
00780 } else {
00781 ocd._in_graph = true;
00782 CollisionNode *cnode = DCAST(CollisionNode, cnode_path.node());
00783
00784 CollisionLevelStateDouble::ColliderDef def;
00785 def._node = cnode;
00786 def._node_path = cnode_path;
00787
00788 int num_solids = cnode->get_num_solids();
00789 for (int s = 0; s < num_solids; ++s) {
00790 CPT(CollisionSolid) collider = cnode->get_solid(s);
00791 def._collider = collider;
00792 level_state.prepare_collider(def, root);
00793
00794 if (level_state.get_num_colliders() == max_colliders) {
00795
00796
00797 level_states.push_back(level_state);
00798 level_state.clear();
00799 level_state.reserve(min(num_remaining_colliders, max_colliders));
00800 }
00801 }
00802 }
00803
00804 --num_remaining_colliders;
00805 nassertv(num_remaining_colliders >= 0);
00806 }
00807
00808 if (level_state.get_num_colliders() != 0) {
00809 level_states.push_back(level_state);
00810 }
00811 nassertv(num_remaining_colliders == 0);
00812 }
00813
00814
00815
00816
00817
00818
00819 void CollisionTraverser::
00820 r_traverse_double(CollisionLevelStateDouble &level_state, size_t pass) {
00821 if (!level_state.any_in_bounds()) {
00822 return;
00823 }
00824 if (!level_state.apply_transform()) {
00825 return;
00826 }
00827
00828 PandaNode *node = level_state.node();
00829 if (node->is_collision_node()) {
00830 CollisionNode *cnode;
00831 DCAST_INTO_V(cnode, node);
00832 CPT(BoundingVolume) node_bv = cnode->get_bounds();
00833 const GeometricBoundingVolume *node_gbv = NULL;
00834 if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
00835 DCAST_INTO_V(node_gbv, node_bv);
00836 }
00837
00838 CollisionEntry entry;
00839 entry._into_node = cnode;
00840 entry._into_node_path = level_state.get_node_path();
00841 if (_respect_prev_transform) {
00842 entry._flags |= CollisionEntry::F_respect_prev_transform;
00843 }
00844
00845 int num_colliders = level_state.get_num_colliders();
00846 for (int c = 0; c < num_colliders; ++c) {
00847 if (level_state.has_collider(c)) {
00848 entry._from_node = level_state.get_collider_node(c);
00849
00850 if ((entry._from_node->get_from_collide_mask() &
00851 cnode->get_into_collide_mask()) != 0) {
00852 #ifdef DO_PSTATS
00853
00854 #endif
00855 entry._from_node_path = level_state.get_collider_node_path(c);
00856 entry._from = level_state.get_collider(c);
00857
00858 compare_collider_to_node(
00859 entry,
00860 level_state.get_parent_bound(c),
00861 level_state.get_local_bound(c),
00862 node_gbv);
00863 }
00864 }
00865 }
00866
00867 } else if (node->is_geom_node()) {
00868 #ifndef NDEBUG
00869 if (collide_cat.is_spam()) {
00870 collide_cat.spam()
00871 << "Reached " << *node << "\n";
00872 }
00873 #endif
00874
00875 GeomNode *gnode;
00876 DCAST_INTO_V(gnode, node);
00877 CPT(BoundingVolume) node_bv = gnode->get_bounds();
00878 const GeometricBoundingVolume *node_gbv = NULL;
00879 if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
00880 DCAST_INTO_V(node_gbv, node_bv);
00881 }
00882
00883 CollisionEntry entry;
00884 entry._into_node = gnode;
00885 entry._into_node_path = level_state.get_node_path();
00886 if (_respect_prev_transform) {
00887 entry._flags |= CollisionEntry::F_respect_prev_transform;
00888 }
00889
00890 int num_colliders = level_state.get_num_colliders();
00891 for (int c = 0; c < num_colliders; ++c) {
00892 if (level_state.has_collider(c)) {
00893 entry._from_node = level_state.get_collider_node(c);
00894
00895 if ((entry._from_node->get_from_collide_mask() &
00896 gnode->get_into_collide_mask()) != 0) {
00897 #ifdef DO_PSTATS
00898
00899 #endif
00900 entry._from_node_path = level_state.get_collider_node_path(c);
00901 entry._from = level_state.get_collider(c);
00902
00903 compare_collider_to_geom_node(
00904 entry,
00905 level_state.get_parent_bound(c),
00906 level_state.get_local_bound(c),
00907 node_gbv);
00908 }
00909 }
00910 }
00911 }
00912
00913 if (node->has_single_child_visibility()) {
00914
00915
00916 int index = node->get_visible_child();
00917 if (index >= 0 && index < node->get_num_children()) {
00918 CollisionLevelStateDouble next_state(level_state, node->get_child(index));
00919 r_traverse_double(next_state, pass);
00920 }
00921
00922 } else if (node->is_lod_node()) {
00923
00924
00925
00926
00927
00928
00929 int index = DCAST(LODNode, node)->get_lowest_switch();
00930 PandaNode::Children children = node->get_children();
00931 int num_children = children.get_num_children();
00932 for (int i = 0; i < num_children; ++i) {
00933 CollisionLevelStateDouble next_state(level_state, children.get_child(i));
00934 if (i != index) {
00935 next_state.set_include_mask(next_state.get_include_mask() &
00936 ~GeomNode::get_default_collide_mask());
00937 }
00938 r_traverse_double(next_state, pass);
00939 }
00940
00941 } else {
00942
00943 PandaNode::Children children = node->get_children();
00944 int num_children = children.get_num_children();
00945 for (int i = 0; i < num_children; ++i) {
00946 CollisionLevelStateDouble next_state(level_state, children.get_child(i));
00947 r_traverse_double(next_state, pass);
00948 }
00949 }
00950 }
00951
00952
00953
00954
00955
00956
00957
00958
00959
00960
00961
00962 void CollisionTraverser::
00963 prepare_colliders_quad(CollisionTraverser::LevelStatesQuad &level_states,
00964 const NodePath &root) {
00965 int num_colliders = _colliders.size();
00966 int max_colliders = CollisionLevelStateQuad::get_max_colliders();
00967
00968 CollisionLevelStateQuad level_state(root);
00969
00970
00971
00972
00973
00974 level_state.reserve(min(num_colliders, max_colliders));
00975
00976
00977
00978 int *indirect = (int *)alloca(sizeof(int) * num_colliders);
00979 int i;
00980 for (i = 0; i < num_colliders; ++i) {
00981 indirect[i] = i;
00982 }
00983 sort(indirect, indirect + num_colliders, SortByColliderSort(*this));
00984
00985 int num_remaining_colliders = num_colliders;
00986 for (i = 0; i < num_colliders; ++i) {
00987 OrderedColliderDef &ocd = _ordered_colliders[indirect[i]];
00988 NodePath cnode_path = ocd._node_path;
00989
00990 if (!cnode_path.is_same_graph(root)) {
00991 if (ocd._in_graph) {
00992
00993 collide_cat.info()
00994 << "Collider " << cnode_path
00995 << " is not in scene graph. Ignoring.\n";
00996 ocd._in_graph = false;
00997 }
00998
00999 } else {
01000 ocd._in_graph = true;
01001 CollisionNode *cnode = DCAST(CollisionNode, cnode_path.node());
01002
01003 CollisionLevelStateQuad::ColliderDef def;
01004 def._node = cnode;
01005 def._node_path = cnode_path;
01006
01007 int num_solids = cnode->get_num_solids();
01008 for (int s = 0; s < num_solids; ++s) {
01009 CPT(CollisionSolid) collider = cnode->get_solid(s);
01010 def._collider = collider;
01011 level_state.prepare_collider(def, root);
01012
01013 if (level_state.get_num_colliders() == max_colliders) {
01014
01015
01016 level_states.push_back(level_state);
01017 level_state.clear();
01018 level_state.reserve(min(num_remaining_colliders, max_colliders));
01019 }
01020 }
01021 }
01022
01023 --num_remaining_colliders;
01024 nassertv(num_remaining_colliders >= 0);
01025 }
01026
01027 if (level_state.get_num_colliders() != 0) {
01028 level_states.push_back(level_state);
01029 }
01030 nassertv(num_remaining_colliders == 0);
01031 }
01032
01033
01034
01035
01036
01037
01038 void CollisionTraverser::
01039 r_traverse_quad(CollisionLevelStateQuad &level_state, size_t pass) {
01040 if (!level_state.any_in_bounds()) {
01041 return;
01042 }
01043 if (!level_state.apply_transform()) {
01044 return;
01045 }
01046
01047 PandaNode *node = level_state.node();
01048 if (node->is_collision_node()) {
01049 CollisionNode *cnode;
01050 DCAST_INTO_V(cnode, node);
01051 CPT(BoundingVolume) node_bv = cnode->get_bounds();
01052 const GeometricBoundingVolume *node_gbv = NULL;
01053 if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
01054 DCAST_INTO_V(node_gbv, node_bv);
01055 }
01056
01057 CollisionEntry entry;
01058 entry._into_node = cnode;
01059 entry._into_node_path = level_state.get_node_path();
01060 if (_respect_prev_transform) {
01061 entry._flags |= CollisionEntry::F_respect_prev_transform;
01062 }
01063
01064 int num_colliders = level_state.get_num_colliders();
01065 for (int c = 0; c < num_colliders; ++c) {
01066 if (level_state.has_collider(c)) {
01067 entry._from_node = level_state.get_collider_node(c);
01068
01069 if ((entry._from_node->get_from_collide_mask() &
01070 cnode->get_into_collide_mask()) != 0) {
01071 #ifdef DO_PSTATS
01072
01073 #endif
01074 entry._from_node_path = level_state.get_collider_node_path(c);
01075 entry._from = level_state.get_collider(c);
01076
01077 compare_collider_to_node(
01078 entry,
01079 level_state.get_parent_bound(c),
01080 level_state.get_local_bound(c),
01081 node_gbv);
01082 }
01083 }
01084 }
01085
01086 } else if (node->is_geom_node()) {
01087 #ifndef NDEBUG
01088 if (collide_cat.is_spam()) {
01089 collide_cat.spam()
01090 << "Reached " << *node << "\n";
01091 }
01092 #endif
01093
01094 GeomNode *gnode;
01095 DCAST_INTO_V(gnode, node);
01096 CPT(BoundingVolume) node_bv = gnode->get_bounds();
01097 const GeometricBoundingVolume *node_gbv = NULL;
01098 if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
01099 DCAST_INTO_V(node_gbv, node_bv);
01100 }
01101
01102 CollisionEntry entry;
01103 entry._into_node = gnode;
01104 entry._into_node_path = level_state.get_node_path();
01105 if (_respect_prev_transform) {
01106 entry._flags |= CollisionEntry::F_respect_prev_transform;
01107 }
01108
01109 int num_colliders = level_state.get_num_colliders();
01110 for (int c = 0; c < num_colliders; ++c) {
01111 if (level_state.has_collider(c)) {
01112 entry._from_node = level_state.get_collider_node(c);
01113
01114 if ((entry._from_node->get_from_collide_mask() &
01115 gnode->get_into_collide_mask()) != 0) {
01116 #ifdef DO_PSTATS
01117
01118 #endif
01119 entry._from_node_path = level_state.get_collider_node_path(c);
01120 entry._from = level_state.get_collider(c);
01121
01122 compare_collider_to_geom_node(
01123 entry,
01124 level_state.get_parent_bound(c),
01125 level_state.get_local_bound(c),
01126 node_gbv);
01127 }
01128 }
01129 }
01130 }
01131
01132 if (node->has_single_child_visibility()) {
01133
01134
01135 int index = node->get_visible_child();
01136 if (index >= 0 && index < node->get_num_children()) {
01137 CollisionLevelStateQuad next_state(level_state, node->get_child(index));
01138 r_traverse_quad(next_state, pass);
01139 }
01140
01141 } else if (node->is_lod_node()) {
01142
01143
01144
01145
01146
01147
01148 int index = DCAST(LODNode, node)->get_lowest_switch();
01149 PandaNode::Children children = node->get_children();
01150 int num_children = children.get_num_children();
01151 for (int i = 0; i < num_children; ++i) {
01152 CollisionLevelStateQuad next_state(level_state, children.get_child(i));
01153 if (i != index) {
01154 next_state.set_include_mask(next_state.get_include_mask() &
01155 ~GeomNode::get_default_collide_mask());
01156 }
01157 r_traverse_quad(next_state, pass);
01158 }
01159
01160 } else {
01161
01162 PandaNode::Children children = node->get_children();
01163 int num_children = children.get_num_children();
01164 for (int i = 0; i < num_children; ++i) {
01165 CollisionLevelStateQuad next_state(level_state, children.get_child(i));
01166 r_traverse_quad(next_state, pass);
01167 }
01168 }
01169 }
01170
01171
01172
01173
01174
01175
01176 void CollisionTraverser::
01177 compare_collider_to_node(CollisionEntry &entry,
01178 const GeometricBoundingVolume *from_parent_gbv,
01179 const GeometricBoundingVolume *from_node_gbv,
01180 const GeometricBoundingVolume *into_node_gbv) {
01181 bool within_node_bounds = true;
01182 if (from_parent_gbv != (GeometricBoundingVolume *)NULL &&
01183 into_node_gbv != (GeometricBoundingVolume *)NULL) {
01184 within_node_bounds = (into_node_gbv->contains(from_parent_gbv) != 0);
01185 _cnode_volume_pcollector.add_level(1);
01186 }
01187
01188 if (within_node_bounds) {
01189 CollisionNode *cnode;
01190 DCAST_INTO_V(cnode, entry._into_node);
01191 int num_solids = cnode->get_num_solids();
01192 collide_cat.spam()
01193 << "Colliding against CollisionNode " << entry._into_node
01194 << " which has " << num_solids << " collision solids.\n";
01195 for (int s = 0; s < num_solids; ++s) {
01196 entry._into = cnode->get_solid(s);
01197
01198
01199
01200
01201
01202 CPT(BoundingVolume) solid_bv = entry._into->get_bounds();
01203 const GeometricBoundingVolume *solid_gbv = NULL;
01204 if (num_solids > 1 &&
01205 solid_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
01206
01207
01208
01209
01210
01211 DCAST_INTO_V(solid_gbv, solid_bv);
01212 }
01213
01214 compare_collider_to_solid(entry, from_node_gbv, solid_gbv);
01215 }
01216 }
01217 }
01218
01219
01220
01221
01222
01223
01224 void CollisionTraverser::
01225 compare_collider_to_geom_node(CollisionEntry &entry,
01226 const GeometricBoundingVolume *from_parent_gbv,
01227 const GeometricBoundingVolume *from_node_gbv,
01228 const GeometricBoundingVolume *into_node_gbv) {
01229 bool within_node_bounds = true;
01230 if (from_parent_gbv != (GeometricBoundingVolume *)NULL &&
01231 into_node_gbv != (GeometricBoundingVolume *)NULL) {
01232 within_node_bounds = (into_node_gbv->contains(from_parent_gbv) != 0);
01233 _gnode_volume_pcollector.add_level(1);
01234 }
01235
01236 if (within_node_bounds) {
01237 GeomNode *gnode;
01238 DCAST_INTO_V(gnode, entry._into_node);
01239 int num_geoms = gnode->get_num_geoms();
01240 for (int s = 0; s < num_geoms; ++s) {
01241 entry._into = (CollisionSolid *)NULL;
01242 const Geom *geom = DCAST(Geom, gnode->get_geom(s));
01243 if (geom != (Geom *)NULL) {
01244 CPT(BoundingVolume) geom_bv = geom->get_bounds();
01245 const GeometricBoundingVolume *geom_gbv = NULL;
01246 if (num_geoms > 1 &&
01247 geom_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
01248
01249
01250
01251
01252
01253 DCAST_INTO_V(geom_gbv, geom_bv);
01254 }
01255
01256 compare_collider_to_geom(entry, geom, from_node_gbv, geom_gbv);
01257 }
01258 }
01259 }
01260 }
01261
01262
01263
01264
01265
01266
01267 void CollisionTraverser::
01268 compare_collider_to_solid(CollisionEntry &entry,
01269 const GeometricBoundingVolume *from_node_gbv,
01270 const GeometricBoundingVolume *solid_gbv) {
01271 bool within_solid_bounds = true;
01272 if (from_node_gbv != (GeometricBoundingVolume *)NULL &&
01273 solid_gbv != (GeometricBoundingVolume *)NULL) {
01274 within_solid_bounds = (solid_gbv->contains(from_node_gbv) != 0);
01275 #ifdef DO_PSTATS
01276 ((CollisionSolid *)entry.get_into())->get_volume_pcollector().add_level(1);
01277 #endif // DO_PSTATS
01278 #ifndef NDEBUG
01279 if (collide_cat.is_spam()) {
01280 collide_cat.spam(false)
01281 << "Comparing to solid: " << *from_node_gbv
01282 << " to " << *solid_gbv << ", within_solid_bounds = "
01283 << within_solid_bounds << "\n";
01284 }
01285 #endif // NDEBUG
01286 }
01287 if (within_solid_bounds) {
01288 Colliders::const_iterator ci;
01289 ci = _colliders.find(entry.get_from_node_path());
01290 nassertv(ci != _colliders.end());
01291 entry.test_intersection((*ci).second, this);
01292 }
01293 }
01294
01295
01296
01297
01298
01299
01300 void CollisionTraverser::
01301 compare_collider_to_geom(CollisionEntry &entry, const Geom *geom,
01302 const GeometricBoundingVolume *from_node_gbv,
01303 const GeometricBoundingVolume *geom_gbv) {
01304 bool within_geom_bounds = true;
01305 if (from_node_gbv != (GeometricBoundingVolume *)NULL &&
01306 geom_gbv != (GeometricBoundingVolume *)NULL) {
01307 within_geom_bounds = (geom_gbv->contains(from_node_gbv) != 0);
01308 _geom_volume_pcollector.add_level(1);
01309 }
01310 if (within_geom_bounds) {
01311 Colliders::const_iterator ci;
01312 ci = _colliders.find(entry.get_from_node_path());
01313 nassertv(ci != _colliders.end());
01314
01315 if (geom->get_primitive_type() == Geom::PT_polygons) {
01316 Thread *current_thread = Thread::get_current_thread();
01317 CPT(GeomVertexData) data = geom->get_vertex_data()->animate_vertices(true, current_thread);
01318 GeomVertexReader vertex(data, InternalName::get_vertex());
01319
01320 int num_primitives = geom->get_num_primitives();
01321 for (int i = 0; i < num_primitives; ++i) {
01322 const GeomPrimitive *primitive = geom->get_primitive(i);
01323 CPT(GeomPrimitive) tris = primitive->decompose();
01324 nassertv(tris->is_of_type(GeomTriangles::get_class_type()));
01325
01326 if (tris->is_indexed()) {
01327
01328 GeomVertexReader index(tris->get_vertices(), 0);
01329 while (!index.is_at_end()) {
01330 LPoint3 v[3];
01331
01332 vertex.set_row_unsafe(index.get_data1i());
01333 v[0] = vertex.get_data3();
01334 vertex.set_row_unsafe(index.get_data1i());
01335 v[1] = vertex.get_data3();
01336 vertex.set_row_unsafe(index.get_data1i());
01337 v[2] = vertex.get_data3();
01338
01339
01340
01341 if (CollisionPolygon::verify_points(v[0], v[1], v[2])) {
01342 bool within_solid_bounds = true;
01343 if (from_node_gbv != (GeometricBoundingVolume *)NULL) {
01344 PT(BoundingSphere) sphere = new BoundingSphere;
01345 sphere->around(v, v + 3);
01346 within_solid_bounds = (sphere->contains(from_node_gbv) != 0);
01347 #ifdef DO_PSTATS
01348 CollisionGeom::_volume_pcollector.add_level(1);
01349 #endif // DO_PSTATS
01350 }
01351 if (within_solid_bounds) {
01352 PT(CollisionGeom) cgeom = new CollisionGeom(LVecBase3(v[0]), LVecBase3(v[1]), LVecBase3(v[2]));
01353 entry._into = cgeom;
01354 entry.test_intersection((*ci).second, this);
01355 }
01356 }
01357 }
01358 } else {
01359
01360 vertex.set_row_unsafe(primitive->get_first_vertex());
01361 int num_vertices = primitive->get_num_vertices();
01362 for (int i = 0; i < num_vertices; i += 3) {
01363 LPoint3 v[3];
01364
01365 v[0] = vertex.get_data3();
01366 v[1] = vertex.get_data3();
01367 v[2] = vertex.get_data3();
01368
01369
01370
01371 if (CollisionPolygon::verify_points(v[0], v[1], v[2])) {
01372 bool within_solid_bounds = true;
01373 if (from_node_gbv != (GeometricBoundingVolume *)NULL) {
01374 PT(BoundingSphere) sphere = new BoundingSphere;
01375 sphere->around(v, v + 3);
01376 within_solid_bounds = (sphere->contains(from_node_gbv) != 0);
01377 #ifdef DO_PSTATS
01378 CollisionGeom::_volume_pcollector.add_level(1);
01379 #endif // DO_PSTATS
01380 }
01381 if (within_solid_bounds) {
01382 PT(CollisionGeom) cgeom = new CollisionGeom(LVecBase3(v[0]), LVecBase3(v[1]), LVecBase3(v[2]));
01383 entry._into = cgeom;
01384 entry.test_intersection((*ci).second, this);
01385 }
01386 }
01387 }
01388 }
01389 }
01390 }
01391 }
01392 }
01393
01394
01395
01396
01397
01398
01399
01400
01401
01402
01403
01404
01405
01406 CollisionTraverser::Handlers::iterator CollisionTraverser::
01407 remove_handler(CollisionTraverser::Handlers::iterator hi) {
01408 nassertr(hi != _handlers.end(), hi);
01409
01410 CollisionHandler *handler = (*hi).first;
01411 Handlers::iterator hnext = hi;
01412 ++hnext;
01413 _handlers.erase(hi);
01414 hi = hnext;
01415
01416
01417 Colliders::iterator ci;
01418 ci = _colliders.begin();
01419 while (ci != _colliders.end()) {
01420 if ((*ci).second == handler) {
01421
01422 NodePath node_path = (*ci).first;
01423
01424 Colliders::iterator cnext = ci;
01425 ++cnext;
01426 _colliders.erase(ci);
01427 ci = cnext;
01428
01429
01430 OrderedColliders::iterator oci;
01431 oci = _ordered_colliders.begin();
01432 while (oci != _ordered_colliders.end() &&
01433 (*oci)._node_path != node_path) {
01434 ++oci;
01435 }
01436 nassertr(oci != _ordered_colliders.end(), hi);
01437 _ordered_colliders.erase(oci);
01438
01439 nassertr(_ordered_colliders.size() == _colliders.size(), hi);
01440
01441 } else {
01442
01443 ++ci;
01444 }
01445 }
01446
01447 return hi;
01448 }
01449
01450
01451
01452
01453
01454
01455
01456 PStatCollector &CollisionTraverser::
01457 get_pass_collector(int pass) {
01458 nassertr(pass >= 0, _this_pcollector);
01459 while ((int)_pass_collectors.size() <= pass) {
01460 ostringstream name;
01461 name << "pass" << (_pass_collectors.size() + 1);
01462 PStatCollector col(_this_pcollector, name.str());
01463 _pass_collectors.push_back(col);
01464 PStatCollector sc_col(col, "solid_collide");
01465 _solid_collide_collectors.push_back(sc_col);
01466 }
01467
01468 return _pass_collectors[pass];
01469 }