43 PStatCollector CollisionTraverser::_collisions_pcollector(
"App:Collisions");
45 PStatCollector CollisionTraverser::_cnode_volume_pcollector(
"Collision Volumes:CollisionNode");
46 PStatCollector CollisionTraverser::_gnode_volume_pcollector(
"Collision Volumes:GeomNode");
47 PStatCollector CollisionTraverser::_geom_volume_pcollector(
"Collision Volumes:Geom");
52 class SortByColliderSort {
59 inline bool operator () (
int a,
int b)
const {
60 const CollisionTraverser::OrderedColliderDef &ocd_a = _trav._ordered_colliders[a];
61 const CollisionTraverser::OrderedColliderDef &ocd_b = _trav._ordered_colliders[b];
62 return ((
const CollisionNode *)ocd_a._node_path.node())->get_collider_sort() < ((
const CollisionNode *)ocd_b._node_path.node())->get_collider_sort();
72 CollisionTraverser(
const std::string &name) :
74 _this_pcollector(_collisions_pcollector, name)
76 _respect_prev_transform = respect_prev_transform;
77 #ifdef DO_COLLISION_RECORDING
86 ~CollisionTraverser() {
87 #ifdef DO_COLLISION_RECORDING
103 nassertv(_ordered_colliders.size() == _colliders.size());
105 nassertv(handler !=
nullptr);
107 Colliders::iterator ci = _colliders.find(collider);
108 if (ci != _colliders.end()) {
110 if ((*ci).second != handler) {
113 (*ci).second = handler;
116 Handlers::iterator hi = _handlers.find(old_handler);
117 nassertv(hi != _handlers.end());
119 nassertv((*hi).second >= 0);
120 if ((*hi).second == 0) {
124 hi = _handlers.find(handler);
125 if (hi == _handlers.end()) {
126 _handlers.insert(Handlers::value_type(handler, 1));
134 _colliders.insert(Colliders::value_type(collider, handler));
135 OrderedColliderDef ocdef;
136 ocdef._node_path = collider;
137 ocdef._in_graph =
true;
138 _ordered_colliders.push_back(ocdef);
140 Handlers::iterator hi = _handlers.find(handler);
141 if (hi == _handlers.end()) {
142 _handlers.insert(Handlers::value_type(handler, 1));
148 nassertv(_ordered_colliders.size() == _colliders.size());
159 nassertr(_ordered_colliders.size() == _colliders.size(),
false);
161 Colliders::iterator ci = _colliders.find(collider);
162 if (ci == _colliders.end()) {
170 Handlers::iterator hi = _handlers.find(handler);
171 nassertr(hi != _handlers.end(),
false);
173 nassertr((*hi).second >= 0,
false);
174 if ((*hi).second == 0) {
178 _colliders.erase(ci);
180 OrderedColliders::iterator oci;
181 oci = _ordered_colliders.begin();
182 while (oci != _ordered_colliders.end() &&
183 (*oci)._node_path != collider) {
187 nassertr(oci != _ordered_colliders.end(),
false);
188 _ordered_colliders.erase(oci);
190 nassertr(_ordered_colliders.size() == _colliders.size(),
false);
200 Colliders::const_iterator ci = _colliders.find(collider);
201 return (ci != _colliders.end());
210 nassertr(_ordered_colliders.size() == _colliders.size(), 0);
211 return _ordered_colliders.size();
220 nassertr(_ordered_colliders.size() == _colliders.size(),
NodePath());
221 nassertr(n >= 0 && n < (
int)_ordered_colliders.size(),
NodePath());
222 return _ordered_colliders[n]._node_path;
232 Colliders::const_iterator ci = _colliders.find(collider);
233 if (ci != _colliders.end()) {
246 _ordered_colliders.clear();
253 void CollisionTraverser::
257 #ifdef DO_COLLISION_RECORDING
258 if (has_recorder()) {
259 get_recorder()->begin_traversal();
261 #endif // DO_COLLISION_RECORDING
263 Handlers::iterator hi;
264 for (hi = _handlers.begin(); hi != _handlers.end(); ++hi) {
265 if ((*hi).first->wants_all_potential_collidees()) {
266 (*hi).first->set_root(root);
268 (*hi).first->begin_group();
271 bool traversal_done =
false;
273 !allow_collider_multiple) {
276 LevelStatesSingle level_states;
277 prepare_colliders_single(level_states, root);
279 if (level_states.size() == 1 || !allow_collider_multiple) {
280 traversal_done =
true;
284 for (
size_t pass = 0; pass < level_states.size(); ++pass) {
286 PStatTimer pass_timer(get_pass_collector(pass));
288 r_traverse_single(level_states[pass], pass);
293 if (!traversal_done &&
296 LevelStatesDouble level_states;
297 prepare_colliders_double(level_states, root);
299 if (level_states.size() == 1) {
300 traversal_done =
true;
302 for (
size_t pass = 0; pass < level_states.size(); ++pass) {
304 PStatTimer pass_timer(get_pass_collector(pass));
306 r_traverse_double(level_states[pass], pass);
311 if (!traversal_done) {
313 LevelStatesQuad level_states;
314 prepare_colliders_quad(level_states, root);
316 traversal_done =
true;
318 for (
size_t pass = 0; pass < level_states.size(); ++pass) {
320 PStatTimer pass_timer(get_pass_collector(pass));
322 r_traverse_quad(level_states[pass], pass);
326 hi = _handlers.begin();
327 while (hi != _handlers.end()) {
328 if (!(*hi).first->end_group()) {
330 hi = remove_handler(hi);
336 #ifdef DO_COLLISION_RECORDING
337 if (has_recorder()) {
338 get_recorder()->end_traversal();
340 #endif // DO_COLLISION_RECORDING
342 CollisionLevelStateBase::_node_volume_pcollector.flush_level();
343 _cnode_volume_pcollector.flush_level();
344 _gnode_volume_pcollector.flush_level();
345 _geom_volume_pcollector.flush_level();
354 #if defined(DO_COLLISION_RECORDING) || !defined(CPPPARSER)
373 #ifdef DO_COLLISION_RECORDING
374 if (recorder != _recorder) {
376 if (_recorder !=
nullptr) {
377 nassertv(_recorder->_trav ==
this);
378 _recorder->_trav =
nullptr;
381 _recorder = recorder;
384 if (_recorder !=
nullptr) {
385 nassertv(_recorder->_trav !=
this);
386 if (_recorder->_trav !=
nullptr) {
387 _recorder->_trav->clear_recorder();
389 nassertv(_recorder->_trav ==
nullptr);
390 _recorder->_trav =
this;
404 #ifdef DO_COLLISION_RECORDING
406 CollisionVisualizer *viz =
new CollisionVisualizer(
"show_collisions");
420 #ifdef DO_COLLISION_RECORDING
421 if (!_collision_visualizer_np.is_empty()) {
422 _collision_visualizer_np.remove_node();
428 #endif // DO_COLLISION_RECORDING
433 void CollisionTraverser::
434 output(std::ostream &out)
const {
435 out <<
"CollisionTraverser, " << _colliders.size()
436 <<
" colliders and " << _handlers.size() <<
" handlers.\n";
442 void CollisionTraverser::
443 write(std::ostream &out,
int indent_level)
const {
445 <<
"CollisionTraverser, " << _colliders.size()
446 <<
" colliders and " << _handlers.size() <<
" handlers:\n";
448 OrderedColliders::const_iterator oci;
449 for (oci = _ordered_colliders.begin();
450 oci != _ordered_colliders.end();
452 NodePath cnode_path = (*oci)._node_path;
453 bool in_graph = (*oci)._in_graph;
455 Colliders::const_iterator ci;
456 ci = _colliders.find(cnode_path);
457 nassertv(ci != _colliders.end());
460 nassertv(handler !=
nullptr);
462 indent(out, indent_level + 2)
465 out <<
" handled by " << handler->get_type() <<
"\n";
473 int num_solids = cnode->get_num_solids();
474 for (
int i = 0; i < num_solids; ++i) {
475 cnode->get_solid(i)->write(out, indent_level + 4);
488 void CollisionTraverser::
491 int num_colliders = _colliders.size();
499 level_state.reserve(min(num_colliders, max_colliders));
503 int *indirect = (
int *)alloca(
sizeof(
int) * num_colliders);
505 for (i = 0; i < num_colliders; ++i) {
508 std::sort(indirect, indirect + num_colliders, SortByColliderSort(*
this));
510 int num_remaining_colliders = num_colliders;
511 for (i = 0; i < num_colliders; ++i) {
512 OrderedColliderDef &ocd = _ordered_colliders[indirect[i]];
513 NodePath cnode_path = ocd._node_path;
519 <<
"Collider " << cnode_path
520 <<
" is not in scene graph. Ignoring.\n";
521 ocd._in_graph =
false;
525 ocd._in_graph =
true;
530 def._node_path = cnode_path;
532 int num_solids = cnode->get_num_solids();
533 for (
int s = 0; s < num_solids; ++s) {
535 def._collider = collider;
536 level_state.prepare_collider(def, root);
538 if (level_state.get_num_colliders() == max_colliders) {
540 level_states.push_back(level_state);
542 level_state.reserve(min(num_remaining_colliders, max_colliders));
547 --num_remaining_colliders;
548 nassertv(num_remaining_colliders >= 0);
551 if (level_state.get_num_colliders() != 0) {
552 level_states.push_back(level_state);
554 nassertv(num_remaining_colliders == 0);
560 void CollisionTraverser::
572 DCAST_INTO_V(cnode, node);
575 if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
576 DCAST_INTO_V(node_gbv, node_bv);
580 entry._into_node = cnode;
582 if (_respect_prev_transform) {
583 entry._flags |= CollisionEntry::F_respect_prev_transform;
586 int num_colliders = level_state.get_num_colliders();
587 for (
int c = 0; c < num_colliders; ++c) {
589 entry._from_node = level_state.get_collider_node(c);
591 if ((entry._from_node->get_from_collide_mask() &
596 entry._from_node_path = level_state.get_collider_node_path(c);
597 entry._from = level_state.get_collider(c);
599 compare_collider_to_node(
610 if (collide_cat.is_spam()) {
612 <<
"Reached " << *node <<
"\n";
617 DCAST_INTO_V(gnode, node);
620 if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
621 DCAST_INTO_V(node_gbv, node_bv);
625 entry._into_node = gnode;
627 if (_respect_prev_transform) {
628 entry._flags |= CollisionEntry::F_respect_prev_transform;
631 int num_colliders = level_state.get_num_colliders();
632 for (
int c = 0; c < num_colliders; ++c) {
634 entry._from_node = level_state.get_collider_node(c);
636 if ((entry._from_node->get_from_collide_mask() &
641 entry._from_node_path = level_state.get_collider_node_path(c);
642 entry._from = level_state.get_collider(c);
644 compare_collider_to_geom_node(
658 if (index >= 0 && index < node->get_num_children()) {
660 r_traverse_single(next_state, pass);
669 int index = DCAST(
LODNode, node)->get_lowest_switch();
672 for (
int i = 0; i < num_children; ++i) {
675 next_state.set_include_mask(next_state.get_include_mask() &
678 r_traverse_single(next_state, pass);
685 for (
int i = 0; i < num_children; ++i) {
687 r_traverse_single(next_state, pass);
699 void CollisionTraverser::
702 int num_colliders = _colliders.size();
710 level_state.
reserve(min(num_colliders, max_colliders));
714 int *indirect = (
int *)alloca(
sizeof(
int) * num_colliders);
716 for (i = 0; i < num_colliders; ++i) {
719 std::sort(indirect, indirect + num_colliders, SortByColliderSort(*
this));
721 int num_remaining_colliders = num_colliders;
722 for (i = 0; i < num_colliders; ++i) {
723 OrderedColliderDef &ocd = _ordered_colliders[indirect[i]];
724 NodePath cnode_path = ocd._node_path;
730 <<
"Collider " << cnode_path
731 <<
" is not in scene graph. Ignoring.\n";
732 ocd._in_graph =
false;
736 ocd._in_graph =
true;
741 def._node_path = cnode_path;
743 int num_solids = cnode->get_num_solids();
744 for (
int s = 0; s < num_solids; ++s) {
746 def._collider = collider;
749 if (level_state.get_num_colliders() == max_colliders) {
751 level_states.push_back(level_state);
753 level_state.
reserve(min(num_remaining_colliders, max_colliders));
758 --num_remaining_colliders;
759 nassertv(num_remaining_colliders >= 0);
762 if (level_state.get_num_colliders() != 0) {
763 level_states.push_back(level_state);
765 nassertv(num_remaining_colliders == 0);
771 void CollisionTraverser::
783 DCAST_INTO_V(cnode, node);
786 if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
787 DCAST_INTO_V(node_gbv, node_bv);
791 entry._into_node = cnode;
793 if (_respect_prev_transform) {
794 entry._flags |= CollisionEntry::F_respect_prev_transform;
797 int num_colliders = level_state.get_num_colliders();
798 for (
int c = 0; c < num_colliders; ++c) {
800 entry._from_node = level_state.get_collider_node(c);
802 if ((entry._from_node->get_from_collide_mask() &
807 entry._from_node_path = level_state.get_collider_node_path(c);
808 entry._from = level_state.get_collider(c);
810 compare_collider_to_node(
821 if (collide_cat.is_spam()) {
823 <<
"Reached " << *node <<
"\n";
828 DCAST_INTO_V(gnode, node);
831 if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
832 DCAST_INTO_V(node_gbv, node_bv);
836 entry._into_node = gnode;
838 if (_respect_prev_transform) {
839 entry._flags |= CollisionEntry::F_respect_prev_transform;
842 int num_colliders = level_state.get_num_colliders();
843 for (
int c = 0; c < num_colliders; ++c) {
845 entry._from_node = level_state.get_collider_node(c);
847 if ((entry._from_node->get_from_collide_mask() &
852 entry._from_node_path = level_state.get_collider_node_path(c);
853 entry._from = level_state.get_collider(c);
855 compare_collider_to_geom_node(
869 if (index >= 0 && index < node->get_num_children()) {
871 r_traverse_double(next_state, pass);
880 int index = DCAST(
LODNode, node)->get_lowest_switch();
883 for (
int i = 0; i < num_children; ++i) {
886 next_state.set_include_mask(next_state.get_include_mask() &
889 r_traverse_double(next_state, pass);
896 for (
int i = 0; i < num_children; ++i) {
898 r_traverse_double(next_state, pass);
910 void CollisionTraverser::
913 int num_colliders = _colliders.size();
921 level_state.
reserve(min(num_colliders, max_colliders));
925 int *indirect = (
int *)alloca(
sizeof(
int) * num_colliders);
927 for (i = 0; i < num_colliders; ++i) {
930 std::sort(indirect, indirect + num_colliders, SortByColliderSort(*
this));
932 int num_remaining_colliders = num_colliders;
933 for (i = 0; i < num_colliders; ++i) {
934 OrderedColliderDef &ocd = _ordered_colliders[indirect[i]];
935 NodePath cnode_path = ocd._node_path;
941 <<
"Collider " << cnode_path
942 <<
" is not in scene graph. Ignoring.\n";
943 ocd._in_graph =
false;
947 ocd._in_graph =
true;
952 def._node_path = cnode_path;
954 int num_solids = cnode->get_num_solids();
955 for (
int s = 0; s < num_solids; ++s) {
957 def._collider = collider;
960 if (level_state.get_num_colliders() == max_colliders) {
962 level_states.push_back(level_state);
964 level_state.
reserve(min(num_remaining_colliders, max_colliders));
969 --num_remaining_colliders;
970 nassertv(num_remaining_colliders >= 0);
973 if (level_state.get_num_colliders() != 0) {
974 level_states.push_back(level_state);
976 nassertv(num_remaining_colliders == 0);
982 void CollisionTraverser::
994 DCAST_INTO_V(cnode, node);
997 if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
998 DCAST_INTO_V(node_gbv, node_bv);
1002 entry._into_node = cnode;
1004 if (_respect_prev_transform) {
1005 entry._flags |= CollisionEntry::F_respect_prev_transform;
1008 int num_colliders = level_state.get_num_colliders();
1009 for (
int c = 0; c < num_colliders; ++c) {
1011 entry._from_node = level_state.get_collider_node(c);
1013 if ((entry._from_node->get_from_collide_mask() &
1018 entry._from_node_path = level_state.get_collider_node_path(c);
1019 entry._from = level_state.get_collider(c);
1021 compare_collider_to_node(
1032 if (collide_cat.is_spam()) {
1034 <<
"Reached " << *node <<
"\n";
1039 DCAST_INTO_V(gnode, node);
1042 if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
1043 DCAST_INTO_V(node_gbv, node_bv);
1047 entry._into_node = gnode;
1049 if (_respect_prev_transform) {
1050 entry._flags |= CollisionEntry::F_respect_prev_transform;
1053 int num_colliders = level_state.get_num_colliders();
1054 for (
int c = 0; c < num_colliders; ++c) {
1056 entry._from_node = level_state.get_collider_node(c);
1058 if ((entry._from_node->get_from_collide_mask() &
1063 entry._from_node_path = level_state.get_collider_node_path(c);
1064 entry._from = level_state.get_collider(c);
1066 compare_collider_to_geom_node(
1080 if (index >= 0 && index < node->get_num_children()) {
1082 r_traverse_quad(next_state, pass);
1091 int index = DCAST(
LODNode, node)->get_lowest_switch();
1094 for (
int i = 0; i < num_children; ++i) {
1097 next_state.set_include_mask(next_state.get_include_mask() &
1100 r_traverse_quad(next_state, pass);
1107 for (
int i = 0; i < num_children; ++i) {
1109 r_traverse_quad(next_state, pass);
1117 void CollisionTraverser::
1122 bool within_node_bounds =
true;
1123 if (from_parent_gbv !=
nullptr &&
1124 into_node_gbv !=
nullptr) {
1125 within_node_bounds = (into_node_gbv->
contains(from_parent_gbv) != 0);
1126 _cnode_volume_pcollector.add_level(1);
1129 if (within_node_bounds) {
1133 DCAST_INTO_V(cnode, entry._into_node);
1135 int num_solids = cnode->get_num_solids();
1136 if (collide_cat.is_spam()) {
1138 <<
"Colliding against CollisionNode " << entry._into_node
1139 <<
" which has " << num_solids <<
" collision solids.\n";
1146 if (num_solids == 1) {
1147 entry._into = cnode->_solids[0].get_read_pointer(current_thread);
1148 Colliders::const_iterator ci;
1150 nassertv(ci != _colliders.end());
1151 entry.test_intersection((*ci).second,
this);
1153 CollisionNode::Solids::const_iterator si;
1154 for (si = cnode->_solids.begin(); si != cnode->_solids.end(); ++si) {
1155 entry._into = (*si).get_read_pointer(current_thread);
1163 if (solid_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
1167 compare_collider_to_solid(entry, from_node_gbv, solid_gbv);
1176 void CollisionTraverser::
1181 bool within_node_bounds =
true;
1182 if (from_parent_gbv !=
nullptr &&
1183 into_node_gbv !=
nullptr) {
1184 within_node_bounds = (into_node_gbv->
contains(from_parent_gbv) != 0);
1185 _gnode_volume_pcollector.add_level(1);
1188 if (within_node_bounds) {
1190 DCAST_INTO_V(gnode, entry._into_node);
1192 for (
int s = 0; s < num_geoms; ++s) {
1193 entry._into =
nullptr;
1194 const Geom *geom = DCAST(
Geom, gnode->get_geom(s));
1195 if (geom !=
nullptr) {
1198 if (num_geoms > 1 &&
1199 geom_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
1205 DCAST_INTO_V(geom_gbv, geom_bv);
1208 compare_collider_to_geom(entry, geom, from_node_gbv, geom_gbv);
1217 void CollisionTraverser::
1221 bool within_solid_bounds =
true;
1222 if (from_node_gbv !=
nullptr &&
1223 solid_gbv !=
nullptr) {
1224 within_solid_bounds = (solid_gbv->
contains(from_node_gbv) != 0);
1229 if (collide_cat.is_spam()) {
1230 collide_cat.spam(
false)
1231 <<
"Comparing to solid: " << *from_node_gbv
1232 <<
" to " << *solid_gbv <<
", within_solid_bounds = "
1233 << within_solid_bounds <<
"\n";
1237 if (within_solid_bounds) {
1238 Colliders::const_iterator ci;
1240 nassertv(ci != _colliders.end());
1241 entry.test_intersection((*ci).second,
this);
1248 void CollisionTraverser::
1252 bool within_geom_bounds =
true;
1253 if (from_node_gbv !=
nullptr &&
1254 geom_gbv !=
nullptr) {
1255 within_geom_bounds = (geom_gbv->
contains(from_node_gbv) != 0);
1256 _geom_volume_pcollector.add_level(1);
1258 if (within_geom_bounds) {
1259 Colliders::const_iterator ci;
1261 nassertv(ci != _colliders.end());
1265 CPT(
GeomVertexData) data = geom->get_animated_vertex_data(
true, current_thread);
1268 int num_primitives = geom->get_num_primitives();
1269 for (
int i = 0; i < num_primitives; ++i) {
1272 nassertv(tris->is_of_type(GeomTriangles::get_class_type()));
1274 if (tris->is_indexed()) {
1277 while (!index.is_at_end()) {
1280 vertex.set_row_unsafe(index.get_data1i());
1281 v[0] = vertex.get_data3();
1282 vertex.set_row_unsafe(index.get_data1i());
1283 v[1] = vertex.get_data3();
1284 vertex.set_row_unsafe(index.get_data1i());
1285 v[2] = vertex.get_data3();
1290 bool within_solid_bounds =
true;
1291 if (from_node_gbv !=
nullptr) {
1293 sphere->
around(v, v + 3);
1294 within_solid_bounds = (sphere->contains(from_node_gbv) != 0);
1296 CollisionGeom::_volume_pcollector.add_level(1);
1299 if (within_solid_bounds) {
1301 entry._into = cgeom;
1302 entry.test_intersection((*ci).second,
this);
1310 for (
int i = 0; i < num_vertices; i += 3) {
1313 v[0] = vertex.get_data3();
1314 v[1] = vertex.get_data3();
1315 v[2] = vertex.get_data3();
1320 bool within_solid_bounds =
true;
1321 if (from_node_gbv !=
nullptr) {
1323 sphere->
around(v, v + 3);
1324 within_solid_bounds = (sphere->contains(from_node_gbv) != 0);
1326 CollisionGeom::_volume_pcollector.add_level(1);
1329 if (within_solid_bounds) {
1331 entry._into = cgeom;
1332 entry.test_intersection((*ci).second,
this);
1351 CollisionTraverser::Handlers::iterator CollisionTraverser::
1352 remove_handler(CollisionTraverser::Handlers::iterator hi) {
1353 nassertr(hi != _handlers.end(), hi);
1356 Handlers::iterator hnext = hi;
1358 _handlers.erase(hi);
1362 Colliders::iterator ci;
1363 ci = _colliders.begin();
1364 while (ci != _colliders.end()) {
1365 if ((*ci).second == handler) {
1369 Colliders::iterator cnext = ci;
1371 _colliders.erase(ci);
1375 OrderedColliders::iterator oci;
1376 oci = _ordered_colliders.begin();
1377 while (oci != _ordered_colliders.end() &&
1378 (*oci)._node_path != node_path) {
1381 nassertr(oci != _ordered_colliders.end(), hi);
1382 _ordered_colliders.erase(oci);
1384 nassertr(_ordered_colliders.size() == _colliders.size(), hi);
1399 get_pass_collector(
int pass) {
1400 nassertr(pass >= 0, _this_pcollector);
1401 while ((
int)_pass_collectors.size() <= pass) {
1402 std::ostringstream name;
1403 name <<
"pass" << (_pass_collectors.size() + 1);
1405 _pass_collectors.push_back(col);
1407 _solid_collide_collectors.push_back(sc_col);
1410 return _pass_collectors[pass];