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();
260 #ifdef DO_COLLISION_RECORDING
261 if (has_recorder()) {
262 get_recorder()->begin_traversal();
264 #endif // DO_COLLISION_RECORDING
266 Handlers::iterator hi;
267 for (hi = _handlers.begin(); hi != _handlers.end(); ++hi) {
268 if ((*hi).first->wants_all_potential_collidees()) {
269 (*hi).first->set_root(root);
271 (*hi).first->begin_group();
274 bool traversal_done =
false;
276 !allow_collider_multiple) {
280 prepare_colliders_single(level_states, root);
282 if (level_states.size() == 1 || !allow_collider_multiple) {
283 traversal_done =
true;
287 for (
size_t pass = 0; pass < level_states.size(); ++pass) {
289 PStatTimer pass_timer(get_pass_collector(pass));
291 r_traverse_single(level_states[pass], pass);
296 if (!traversal_done &&
300 prepare_colliders_double(level_states, root);
302 if (level_states.size() == 1) {
303 traversal_done =
true;
305 for (
size_t pass = 0; pass < level_states.size(); ++pass) {
307 PStatTimer pass_timer(get_pass_collector(pass));
309 r_traverse_double(level_states[pass], pass);
314 if (!traversal_done) {
317 prepare_colliders_quad(level_states, root);
319 traversal_done =
true;
321 for (
size_t pass = 0; pass < level_states.size(); ++pass) {
323 PStatTimer pass_timer(get_pass_collector(pass));
325 r_traverse_quad(level_states[pass], pass);
329 hi = _handlers.begin();
330 while (hi != _handlers.end()) {
331 if (!(*hi).first->end_group()) {
333 hi = remove_handler(hi);
339 #ifdef DO_COLLISION_RECORDING
340 if (has_recorder()) {
341 get_recorder()->end_traversal();
343 #endif // DO_COLLISION_RECORDING
345 CollisionLevelStateBase::_node_volume_pcollector.flush_level();
346 _cnode_volume_pcollector.flush_level();
347 _gnode_volume_pcollector.flush_level();
348 _geom_volume_pcollector.flush_level();
357 #if defined(DO_COLLISION_RECORDING) || !defined(CPPPARSER)
376 #ifdef DO_COLLISION_RECORDING
377 if (recorder != _recorder) {
379 if (_recorder !=
nullptr) {
380 nassertv(_recorder->_trav ==
this);
381 _recorder->_trav =
nullptr;
384 _recorder = recorder;
387 if (_recorder !=
nullptr) {
388 nassertv(_recorder->_trav !=
this);
389 if (_recorder->_trav !=
nullptr) {
390 _recorder->_trav->clear_recorder();
392 nassertv(_recorder->_trav ==
nullptr);
393 _recorder->_trav =
this;
407 #ifdef DO_COLLISION_RECORDING
409 CollisionVisualizer *viz =
new CollisionVisualizer(
"show_collisions");
423 #ifdef DO_COLLISION_RECORDING
424 if (!_collision_visualizer_np.is_empty()) {
425 _collision_visualizer_np.remove_node();
431 #endif // DO_COLLISION_RECORDING
436 void CollisionTraverser::
437 output(std::ostream &out)
const {
438 out <<
"CollisionTraverser, " << _colliders.size()
439 <<
" colliders and " << _handlers.size() <<
" handlers.\n";
445 void CollisionTraverser::
446 write(std::ostream &out,
int indent_level)
const {
448 <<
"CollisionTraverser, " << _colliders.size()
449 <<
" colliders and " << _handlers.size() <<
" handlers:\n";
451 OrderedColliders::const_iterator oci;
452 for (oci = _ordered_colliders.begin();
453 oci != _ordered_colliders.end();
455 NodePath cnode_path = (*oci)._node_path;
456 bool in_graph = (*oci)._in_graph;
458 Colliders::const_iterator ci;
459 ci = _colliders.find(cnode_path);
460 nassertv(ci != _colliders.end());
463 nassertv(handler !=
nullptr);
465 indent(out, indent_level + 2)
468 out <<
" handled by " << handler->get_type() <<
"\n";
476 int num_solids = cnode->get_num_solids();
477 for (
int i = 0; i < num_solids; ++i) {
478 cnode->get_solid(i)->write(out, indent_level + 4);
491 void CollisionTraverser::
494 int num_colliders = _colliders.size();
502 level_state.reserve(min(num_colliders, max_colliders));
506 int *indirect = (
int *)alloca(
sizeof(
int) * num_colliders);
508 for (i = 0; i < num_colliders; ++i) {
511 std::sort(indirect, indirect + num_colliders, SortByColliderSort(*
this));
513 int num_remaining_colliders = num_colliders;
514 for (i = 0; i < num_colliders; ++i) {
515 OrderedColliderDef &ocd = _ordered_colliders[indirect[i]];
516 NodePath cnode_path = ocd._node_path;
522 <<
"Collider " << cnode_path
523 <<
" is not in scene graph. Ignoring.\n";
524 ocd._in_graph =
false;
528 ocd._in_graph =
true;
533 def._node_path = cnode_path;
535 int num_solids = cnode->get_num_solids();
536 for (
int s = 0; s < num_solids; ++s) {
538 def._collider = collider;
539 level_state.prepare_collider(def, root);
541 if (level_state.get_num_colliders() == max_colliders) {
543 level_states.push_back(level_state);
545 level_state.reserve(min(num_remaining_colliders, max_colliders));
550 --num_remaining_colliders;
551 nassertv(num_remaining_colliders >= 0);
554 if (level_state.get_num_colliders() != 0) {
555 level_states.push_back(level_state);
557 nassertv(num_remaining_colliders == 0);
563 void CollisionTraverser::
575 DCAST_INTO_V(cnode, node);
578 if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
579 DCAST_INTO_V(node_gbv, node_bv);
583 entry._into_node = cnode;
585 if (_respect_prev_transform) {
586 entry._flags |= CollisionEntry::F_respect_prev_transform;
589 int num_colliders = level_state.get_num_colliders();
590 for (
int c = 0; c < num_colliders; ++c) {
592 entry._from_node = level_state.get_collider_node(c);
594 if ((entry._from_node->get_from_collide_mask() &
599 entry._from_node_path = level_state.get_collider_node_path(c);
600 entry._from = level_state.get_collider(c);
602 compare_collider_to_node(
613 if (collide_cat.is_spam()) {
615 <<
"Reached " << *node <<
"\n";
620 DCAST_INTO_V(gnode, node);
623 if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
624 DCAST_INTO_V(node_gbv, node_bv);
628 entry._into_node = gnode;
630 if (_respect_prev_transform) {
631 entry._flags |= CollisionEntry::F_respect_prev_transform;
634 int num_colliders = level_state.get_num_colliders();
635 for (
int c = 0; c < num_colliders; ++c) {
637 entry._from_node = level_state.get_collider_node(c);
639 if ((entry._from_node->get_from_collide_mask() &
644 entry._from_node_path = level_state.get_collider_node_path(c);
645 entry._from = level_state.get_collider(c);
647 compare_collider_to_geom_node(
661 if (index >= 0 && index < node->get_num_children()) {
663 r_traverse_single(next_state, pass);
672 int index = DCAST(
LODNode, node)->get_lowest_switch();
675 for (
int i = 0; i < num_children; ++i) {
678 next_state.set_include_mask(next_state.get_include_mask() &
681 r_traverse_single(next_state, pass);
688 for (
int i = 0; i < num_children; ++i) {
690 r_traverse_single(next_state, pass);
702 void CollisionTraverser::
705 int num_colliders = _colliders.size();
713 level_state.
reserve(min(num_colliders, max_colliders));
717 int *indirect = (
int *)alloca(
sizeof(
int) * num_colliders);
719 for (i = 0; i < num_colliders; ++i) {
722 std::sort(indirect, indirect + num_colliders, SortByColliderSort(*
this));
724 int num_remaining_colliders = num_colliders;
725 for (i = 0; i < num_colliders; ++i) {
726 OrderedColliderDef &ocd = _ordered_colliders[indirect[i]];
727 NodePath cnode_path = ocd._node_path;
733 <<
"Collider " << cnode_path
734 <<
" is not in scene graph. Ignoring.\n";
735 ocd._in_graph =
false;
739 ocd._in_graph =
true;
744 def._node_path = cnode_path;
746 int num_solids = cnode->get_num_solids();
747 for (
int s = 0; s < num_solids; ++s) {
749 def._collider = collider;
752 if (level_state.get_num_colliders() == max_colliders) {
754 level_states.push_back(level_state);
756 level_state.
reserve(min(num_remaining_colliders, max_colliders));
761 --num_remaining_colliders;
762 nassertv(num_remaining_colliders >= 0);
765 if (level_state.get_num_colliders() != 0) {
766 level_states.push_back(level_state);
768 nassertv(num_remaining_colliders == 0);
774 void CollisionTraverser::
786 DCAST_INTO_V(cnode, node);
789 if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
790 DCAST_INTO_V(node_gbv, node_bv);
794 entry._into_node = cnode;
796 if (_respect_prev_transform) {
797 entry._flags |= CollisionEntry::F_respect_prev_transform;
800 int num_colliders = level_state.get_num_colliders();
801 for (
int c = 0; c < num_colliders; ++c) {
803 entry._from_node = level_state.get_collider_node(c);
805 if ((entry._from_node->get_from_collide_mask() &
810 entry._from_node_path = level_state.get_collider_node_path(c);
811 entry._from = level_state.get_collider(c);
813 compare_collider_to_node(
824 if (collide_cat.is_spam()) {
826 <<
"Reached " << *node <<
"\n";
831 DCAST_INTO_V(gnode, node);
834 if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
835 DCAST_INTO_V(node_gbv, node_bv);
839 entry._into_node = gnode;
841 if (_respect_prev_transform) {
842 entry._flags |= CollisionEntry::F_respect_prev_transform;
845 int num_colliders = level_state.get_num_colliders();
846 for (
int c = 0; c < num_colliders; ++c) {
848 entry._from_node = level_state.get_collider_node(c);
850 if ((entry._from_node->get_from_collide_mask() &
855 entry._from_node_path = level_state.get_collider_node_path(c);
856 entry._from = level_state.get_collider(c);
858 compare_collider_to_geom_node(
872 if (index >= 0 && index < node->get_num_children()) {
874 r_traverse_double(next_state, pass);
883 int index = DCAST(
LODNode, node)->get_lowest_switch();
886 for (
int i = 0; i < num_children; ++i) {
889 next_state.set_include_mask(next_state.get_include_mask() &
892 r_traverse_double(next_state, pass);
899 for (
int i = 0; i < num_children; ++i) {
901 r_traverse_double(next_state, pass);
913 void CollisionTraverser::
916 int num_colliders = _colliders.size();
924 level_state.
reserve(min(num_colliders, max_colliders));
928 int *indirect = (
int *)alloca(
sizeof(
int) * num_colliders);
930 for (i = 0; i < num_colliders; ++i) {
933 std::sort(indirect, indirect + num_colliders, SortByColliderSort(*
this));
935 int num_remaining_colliders = num_colliders;
936 for (i = 0; i < num_colliders; ++i) {
937 OrderedColliderDef &ocd = _ordered_colliders[indirect[i]];
938 NodePath cnode_path = ocd._node_path;
944 <<
"Collider " << cnode_path
945 <<
" is not in scene graph. Ignoring.\n";
946 ocd._in_graph =
false;
950 ocd._in_graph =
true;
955 def._node_path = cnode_path;
957 int num_solids = cnode->get_num_solids();
958 for (
int s = 0; s < num_solids; ++s) {
960 def._collider = collider;
963 if (level_state.get_num_colliders() == max_colliders) {
965 level_states.push_back(level_state);
967 level_state.
reserve(min(num_remaining_colliders, max_colliders));
972 --num_remaining_colliders;
973 nassertv(num_remaining_colliders >= 0);
976 if (level_state.get_num_colliders() != 0) {
977 level_states.push_back(level_state);
979 nassertv(num_remaining_colliders == 0);
985 void CollisionTraverser::
997 DCAST_INTO_V(cnode, node);
1000 if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
1001 DCAST_INTO_V(node_gbv, node_bv);
1005 entry._into_node = cnode;
1007 if (_respect_prev_transform) {
1008 entry._flags |= CollisionEntry::F_respect_prev_transform;
1011 int num_colliders = level_state.get_num_colliders();
1012 for (
int c = 0; c < num_colliders; ++c) {
1014 entry._from_node = level_state.get_collider_node(c);
1016 if ((entry._from_node->get_from_collide_mask() &
1021 entry._from_node_path = level_state.get_collider_node_path(c);
1022 entry._from = level_state.get_collider(c);
1024 compare_collider_to_node(
1035 if (collide_cat.is_spam()) {
1037 <<
"Reached " << *node <<
"\n";
1042 DCAST_INTO_V(gnode, node);
1045 if (node_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
1046 DCAST_INTO_V(node_gbv, node_bv);
1050 entry._into_node = gnode;
1052 if (_respect_prev_transform) {
1053 entry._flags |= CollisionEntry::F_respect_prev_transform;
1056 int num_colliders = level_state.get_num_colliders();
1057 for (
int c = 0; c < num_colliders; ++c) {
1059 entry._from_node = level_state.get_collider_node(c);
1061 if ((entry._from_node->get_from_collide_mask() &
1066 entry._from_node_path = level_state.get_collider_node_path(c);
1067 entry._from = level_state.get_collider(c);
1069 compare_collider_to_geom_node(
1083 if (index >= 0 && index < node->get_num_children()) {
1085 r_traverse_quad(next_state, pass);
1094 int index = DCAST(
LODNode, node)->get_lowest_switch();
1097 for (
int i = 0; i < num_children; ++i) {
1100 next_state.set_include_mask(next_state.get_include_mask() &
1103 r_traverse_quad(next_state, pass);
1110 for (
int i = 0; i < num_children; ++i) {
1112 r_traverse_quad(next_state, pass);
1120 void CollisionTraverser::
1125 bool within_node_bounds =
true;
1126 if (from_parent_gbv !=
nullptr &&
1127 into_node_gbv !=
nullptr) {
1128 within_node_bounds = (into_node_gbv->
contains(from_parent_gbv) != 0);
1129 _cnode_volume_pcollector.add_level(1);
1132 if (within_node_bounds) {
1136 DCAST_INTO_V(cnode, entry._into_node);
1138 int num_solids = cnode->get_num_solids();
1139 if (collide_cat.is_spam()) {
1141 <<
"Colliding against CollisionNode " << entry._into_node
1142 <<
" which has " << num_solids <<
" collision solids.\n";
1149 if (num_solids == 1) {
1150 entry._into = cnode->_solids[0].get_read_pointer(current_thread);
1151 Colliders::const_iterator ci;
1153 nassertv(ci != _colliders.end());
1154 entry.test_intersection((*ci).second,
this);
1156 CollisionNode::Solids::const_iterator si;
1157 for (si = cnode->_solids.begin(); si != cnode->_solids.end(); ++si) {
1158 entry._into = (*si).get_read_pointer(current_thread);
1166 if (solid_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
1170 compare_collider_to_solid(entry, from_node_gbv, solid_gbv);
1179 void CollisionTraverser::
1184 bool within_node_bounds =
true;
1185 if (from_parent_gbv !=
nullptr &&
1186 into_node_gbv !=
nullptr) {
1187 within_node_bounds = (into_node_gbv->
contains(from_parent_gbv) != 0);
1188 _gnode_volume_pcollector.add_level(1);
1191 if (within_node_bounds) {
1193 DCAST_INTO_V(gnode, entry._into_node);
1195 for (
int s = 0; s < num_geoms; ++s) {
1196 entry._into =
nullptr;
1197 const Geom *geom = DCAST(
Geom, gnode->get_geom(s));
1198 if (geom !=
nullptr) {
1201 if (num_geoms > 1 &&
1202 geom_bv->is_of_type(GeometricBoundingVolume::get_class_type())) {
1208 DCAST_INTO_V(geom_gbv, geom_bv);
1211 compare_collider_to_geom(entry, geom, from_node_gbv, geom_gbv);
1220 void CollisionTraverser::
1224 bool within_solid_bounds =
true;
1225 if (from_node_gbv !=
nullptr &&
1226 solid_gbv !=
nullptr) {
1227 within_solid_bounds = (solid_gbv->
contains(from_node_gbv) != 0);
1232 if (collide_cat.is_spam()) {
1233 collide_cat.spam(
false)
1234 <<
"Comparing to solid: " << *from_node_gbv
1235 <<
" to " << *solid_gbv <<
", within_solid_bounds = "
1236 << within_solid_bounds <<
"\n";
1240 if (within_solid_bounds) {
1241 Colliders::const_iterator ci;
1243 nassertv(ci != _colliders.end());
1244 entry.test_intersection((*ci).second,
this);
1251 void CollisionTraverser::
1255 bool within_geom_bounds =
true;
1256 if (from_node_gbv !=
nullptr &&
1257 geom_gbv !=
nullptr) {
1258 within_geom_bounds = (geom_gbv->
contains(from_node_gbv) != 0);
1259 _geom_volume_pcollector.add_level(1);
1261 if (within_geom_bounds) {
1262 Colliders::const_iterator ci;
1264 nassertv(ci != _colliders.end());
1268 CPT(
GeomVertexData) data = geom->get_animated_vertex_data(
true, current_thread);
1271 int num_primitives = geom->get_num_primitives();
1272 for (
int i = 0; i < num_primitives; ++i) {
1275 nassertv(tris->is_of_type(GeomTriangles::get_class_type()));
1277 if (tris->is_indexed()) {
1280 while (!index.is_at_end()) {
1283 vertex.set_row_unsafe(index.get_data1i());
1284 v[0] = vertex.get_data3();
1285 vertex.set_row_unsafe(index.get_data1i());
1286 v[1] = vertex.get_data3();
1287 vertex.set_row_unsafe(index.get_data1i());
1288 v[2] = vertex.get_data3();
1293 bool within_solid_bounds =
true;
1294 if (from_node_gbv !=
nullptr) {
1296 sphere->
around(v, v + 3);
1297 within_solid_bounds = (sphere->contains(from_node_gbv) != 0);
1299 CollisionGeom::_volume_pcollector.add_level(1);
1302 if (within_solid_bounds) {
1304 entry._into = cgeom;
1305 entry.test_intersection((*ci).second,
this);
1313 for (
int i = 0; i < num_vertices; i += 3) {
1316 v[0] = vertex.get_data3();
1317 v[1] = vertex.get_data3();
1318 v[2] = vertex.get_data3();
1323 bool within_solid_bounds =
true;
1324 if (from_node_gbv !=
nullptr) {
1326 sphere->
around(v, v + 3);
1327 within_solid_bounds = (sphere->contains(from_node_gbv) != 0);
1329 CollisionGeom::_volume_pcollector.add_level(1);
1332 if (within_solid_bounds) {
1334 entry._into = cgeom;
1335 entry.test_intersection((*ci).second,
this);
1354 CollisionTraverser::Handlers::iterator CollisionTraverser::
1355 remove_handler(CollisionTraverser::Handlers::iterator hi) {
1356 nassertr(hi != _handlers.end(), hi);
1359 Handlers::iterator hnext = hi;
1361 _handlers.erase(hi);
1365 Colliders::iterator ci;
1366 ci = _colliders.begin();
1367 while (ci != _colliders.end()) {
1368 if ((*ci).second == handler) {
1372 Colliders::iterator cnext = ci;
1374 _colliders.erase(ci);
1378 OrderedColliders::iterator oci;
1379 oci = _ordered_colliders.begin();
1380 while (oci != _ordered_colliders.end() &&
1381 (*oci)._node_path != node_path) {
1384 nassertr(oci != _ordered_colliders.end(), hi);
1385 _ordered_colliders.erase(oci);
1387 nassertr(_ordered_colliders.size() == _colliders.size(), hi);
1402 get_pass_collector(
int pass) {
1403 nassertr(pass >= 0, _this_pcollector);
1404 while ((
int)_pass_collectors.size() <= pass) {
1405 std::ostringstream name;
1406 name <<
"pass" << (_pass_collectors.size() + 1);
1408 _pass_collectors.push_back(col);
1410 _solid_collide_collectors.push_back(sc_col);
1413 return _pass_collectors[pass];