1 #ifndef GIM_BOX_SET_H_INCLUDED 2 #define GIM_BOX_SET_H_INCLUDED 3 4 /*! \file gim_box_set.h 5 \author Francisco Leon Najera 6 */ 7 /* 8 ----------------------------------------------------------------------------- 9 This source file is part of GIMPACT Library. 10 11 For the latest info, see http://gimpact.sourceforge.net/ 12 13 Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. 14 email: projectileman@yahoo.com 15 16 This library is free software; you can redistribute it and/or 17 modify it under the terms of EITHER: 18 (1) The GNU Lesser General Public License as published by the Free 19 Software Foundation; either version 2.1 of the License, or (at 20 your option) any later version. The text of the GNU Lesser 21 General Public License is included with this library in the 22 file GIMPACT-LICENSE-LGPL.TXT. 23 (2) The BSD-style license that is included with this library in 24 the file GIMPACT-LICENSE-BSD.TXT. 25 (3) The zlib/libpng license that is included with this library in 26 the file GIMPACT-LICENSE-ZLIB.TXT. 27 28 This library is distributed in the hope that it will be useful, 29 but WITHOUT ANY WARRANTY; without even the implied warranty of 30 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files 31 GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. 32 33 ----------------------------------------------------------------------------- 34 */ 35 36 #include "gim_array.h" 37 #include "gim_radixsort.h" 38 #include "gim_box_collision.h" 39 #include "gim_tri_collision.h" 40 #include "gim_pair.h" 41 42 //! A pairset array 43 class gim_pair_set : public gim_array<GIM_PAIR> 44 { 45 public: gim_pair_set()46 gim_pair_set() : gim_array<GIM_PAIR>(32) 47 { 48 } push_pair(GUINT index1,GUINT index2)49 inline void push_pair(GUINT index1, GUINT index2) 50 { 51 push_back(GIM_PAIR(index1, index2)); 52 } 53 push_pair_inv(GUINT index1,GUINT index2)54 inline void push_pair_inv(GUINT index1, GUINT index2) 55 { 56 push_back(GIM_PAIR(index2, index1)); 57 } 58 }; 59 60 //! Prototype Base class for primitive classification 61 /*! 62 This class is a wrapper for primitive collections. 63 This tells relevant info for the Bounding Box set classes, which take care of space classification. 64 This class can manage Compound shapes and trimeshes, and if it is managing trimesh then the Hierarchy Bounding Box classes will take advantage of primitive Vs Box overlapping tests for getting optimal results and less Per Box compairisons. 65 */ 66 class GIM_PRIMITIVE_MANAGER_PROTOTYPE 67 { 68 public: ~GIM_PRIMITIVE_MANAGER_PROTOTYPE()69 virtual ~GIM_PRIMITIVE_MANAGER_PROTOTYPE() {} 70 //! determines if this manager consist on only triangles, which special case will be optimized 71 virtual bool is_trimesh() = 0; 72 virtual GUINT get_primitive_count() = 0; 73 virtual void get_primitive_box(GUINT prim_index, GIM_AABB& primbox) = 0; 74 virtual void get_primitive_triangle(GUINT prim_index, GIM_TRIANGLE& triangle) = 0; 75 }; 76 77 struct GIM_AABB_DATA 78 { 79 GIM_AABB m_bound; 80 GUINT m_data; 81 }; 82 83 //! Node Structure for trees 84 struct GIM_BOX_TREE_NODE 85 { 86 GIM_AABB m_bound; 87 GUINT m_left; //!< Left subtree 88 GUINT m_right; //!< Right subtree 89 GUINT m_escapeIndex; //!< Scape index for traversing 90 GUINT m_data; //!< primitive index if apply 91 GIM_BOX_TREE_NODEGIM_BOX_TREE_NODE92 GIM_BOX_TREE_NODE() 93 { 94 m_left = 0; 95 m_right = 0; 96 m_escapeIndex = 0; 97 m_data = 0; 98 } 99 is_leaf_nodeGIM_BOX_TREE_NODE100 SIMD_FORCE_INLINE bool is_leaf_node() const 101 { 102 return (!m_left && !m_right); 103 } 104 }; 105 106 //! Basic Box tree structure 107 class GIM_BOX_TREE 108 { 109 protected: 110 GUINT m_num_nodes; 111 gim_array<GIM_BOX_TREE_NODE> m_node_array; 112 113 protected: 114 GUINT _sort_and_calc_splitting_index( 115 gim_array<GIM_AABB_DATA>& primitive_boxes, 116 GUINT startIndex, GUINT endIndex, GUINT splitAxis); 117 118 GUINT _calc_splitting_axis(gim_array<GIM_AABB_DATA>& primitive_boxes, GUINT startIndex, GUINT endIndex); 119 120 void _build_sub_tree(gim_array<GIM_AABB_DATA>& primitive_boxes, GUINT startIndex, GUINT endIndex); 121 122 public: GIM_BOX_TREE()123 GIM_BOX_TREE() 124 { 125 m_num_nodes = 0; 126 } 127 128 //! prototype functions for box tree management 129 //!@{ 130 void build_tree(gim_array<GIM_AABB_DATA>& primitive_boxes); 131 clearNodes()132 SIMD_FORCE_INLINE void clearNodes() 133 { 134 m_node_array.clear(); 135 m_num_nodes = 0; 136 } 137 138 //! node count getNodeCount()139 SIMD_FORCE_INLINE GUINT getNodeCount() const 140 { 141 return m_num_nodes; 142 } 143 144 //! tells if the node is a leaf isLeafNode(GUINT nodeindex)145 SIMD_FORCE_INLINE bool isLeafNode(GUINT nodeindex) const 146 { 147 return m_node_array[nodeindex].is_leaf_node(); 148 } 149 getNodeData(GUINT nodeindex)150 SIMD_FORCE_INLINE GUINT getNodeData(GUINT nodeindex) const 151 { 152 return m_node_array[nodeindex].m_data; 153 } 154 getNodeBound(GUINT nodeindex,GIM_AABB & bound)155 SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB& bound) const 156 { 157 bound = m_node_array[nodeindex].m_bound; 158 } 159 setNodeBound(GUINT nodeindex,const GIM_AABB & bound)160 SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB& bound) 161 { 162 m_node_array[nodeindex].m_bound = bound; 163 } 164 getLeftNodeIndex(GUINT nodeindex)165 SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex) const 166 { 167 return m_node_array[nodeindex].m_left; 168 } 169 getRightNodeIndex(GUINT nodeindex)170 SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex) const 171 { 172 return m_node_array[nodeindex].m_right; 173 } 174 getScapeNodeIndex(GUINT nodeindex)175 SIMD_FORCE_INLINE GUINT getScapeNodeIndex(GUINT nodeindex) const 176 { 177 return m_node_array[nodeindex].m_escapeIndex; 178 } 179 180 //!@} 181 }; 182 183 //! Generic Box Tree Template 184 /*! 185 This class offers an structure for managing a box tree of primitives. 186 Requires a Primitive prototype (like GIM_PRIMITIVE_MANAGER_PROTOTYPE ) and 187 a Box tree structure ( like GIM_BOX_TREE). 188 */ 189 template <typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE, typename _GIM_BOX_TREE_PROTOTYPE> 190 class GIM_BOX_TREE_TEMPLATE_SET 191 { 192 protected: 193 _GIM_PRIMITIVE_MANAGER_PROTOTYPE m_primitive_manager; 194 _GIM_BOX_TREE_PROTOTYPE m_box_tree; 195 196 protected: 197 //stackless refit refit()198 SIMD_FORCE_INLINE void refit() 199 { 200 GUINT nodecount = getNodeCount(); 201 while (nodecount--) 202 { 203 if (isLeafNode(nodecount)) 204 { 205 GIM_AABB leafbox; 206 m_primitive_manager.get_primitive_box(getNodeData(nodecount), leafbox); 207 setNodeBound(nodecount, leafbox); 208 } 209 else 210 { 211 //get left bound 212 GUINT childindex = getLeftNodeIndex(nodecount); 213 GIM_AABB bound; 214 getNodeBound(childindex, bound); 215 //get right bound 216 childindex = getRightNodeIndex(nodecount); 217 GIM_AABB bound2; 218 getNodeBound(childindex, bound2); 219 bound.merge(bound2); 220 221 setNodeBound(nodecount, bound); 222 } 223 } 224 } 225 226 public: GIM_BOX_TREE_TEMPLATE_SET()227 GIM_BOX_TREE_TEMPLATE_SET() 228 { 229 } 230 getGlobalBox()231 SIMD_FORCE_INLINE GIM_AABB getGlobalBox() const 232 { 233 GIM_AABB totalbox; 234 getNodeBound(0, totalbox); 235 return totalbox; 236 } 237 setPrimitiveManager(const _GIM_PRIMITIVE_MANAGER_PROTOTYPE & primitive_manager)238 SIMD_FORCE_INLINE void setPrimitiveManager(const _GIM_PRIMITIVE_MANAGER_PROTOTYPE& primitive_manager) 239 { 240 m_primitive_manager = primitive_manager; 241 } 242 getPrimitiveManager()243 const _GIM_PRIMITIVE_MANAGER_PROTOTYPE& getPrimitiveManager() const 244 { 245 return m_primitive_manager; 246 } 247 getPrimitiveManager()248 _GIM_PRIMITIVE_MANAGER_PROTOTYPE& getPrimitiveManager() 249 { 250 return m_primitive_manager; 251 } 252 253 //! node manager prototype functions 254 ///@{ 255 256 //! this attemps to refit the box set. update()257 SIMD_FORCE_INLINE void update() 258 { 259 refit(); 260 } 261 262 //! this rebuild the entire set buildSet()263 SIMD_FORCE_INLINE void buildSet() 264 { 265 //obtain primitive boxes 266 gim_array<GIM_AABB_DATA> primitive_boxes; 267 primitive_boxes.resize(m_primitive_manager.get_primitive_count(), false); 268 269 for (GUINT i = 0; i < primitive_boxes.size(); i++) 270 { 271 m_primitive_manager.get_primitive_box(i, primitive_boxes[i].m_bound); 272 primitive_boxes[i].m_data = i; 273 } 274 275 m_box_tree.build_tree(primitive_boxes); 276 } 277 278 //! returns the indices of the primitives in the m_primitive_manager boxQuery(const GIM_AABB & box,gim_array<GUINT> & collided_results)279 SIMD_FORCE_INLINE bool boxQuery(const GIM_AABB& box, gim_array<GUINT>& collided_results) const 280 { 281 GUINT curIndex = 0; 282 GUINT numNodes = getNodeCount(); 283 284 while (curIndex < numNodes) 285 { 286 GIM_AABB bound; 287 getNodeBound(curIndex, bound); 288 289 //catch bugs in tree data 290 291 bool aabbOverlap = bound.has_collision(box); 292 bool isleafnode = isLeafNode(curIndex); 293 294 if (isleafnode && aabbOverlap) 295 { 296 collided_results.push_back(getNodeData(curIndex)); 297 } 298 299 if (aabbOverlap || isleafnode) 300 { 301 //next subnode 302 curIndex++; 303 } 304 else 305 { 306 //skip node 307 curIndex += getScapeNodeIndex(curIndex); 308 } 309 } 310 if (collided_results.size() > 0) return true; 311 return false; 312 } 313 314 //! returns the indices of the primitives in the m_primitive_manager boxQueryTrans(const GIM_AABB & box,const btTransform & transform,gim_array<GUINT> & collided_results)315 SIMD_FORCE_INLINE bool boxQueryTrans(const GIM_AABB& box, 316 const btTransform& transform, gim_array<GUINT>& collided_results) const 317 { 318 GIM_AABB transbox = box; 319 transbox.appy_transform(transform); 320 return boxQuery(transbox, collided_results); 321 } 322 323 //! returns the indices of the primitives in the m_primitive_manager rayQuery(const btVector3 & ray_dir,const btVector3 & ray_origin,gim_array<GUINT> & collided_results)324 SIMD_FORCE_INLINE bool rayQuery( 325 const btVector3& ray_dir, const btVector3& ray_origin, 326 gim_array<GUINT>& collided_results) const 327 { 328 GUINT curIndex = 0; 329 GUINT numNodes = getNodeCount(); 330 331 while (curIndex < numNodes) 332 { 333 GIM_AABB bound; 334 getNodeBound(curIndex, bound); 335 336 //catch bugs in tree data 337 338 bool aabbOverlap = bound.collide_ray(ray_origin, ray_dir); 339 bool isleafnode = isLeafNode(curIndex); 340 341 if (isleafnode && aabbOverlap) 342 { 343 collided_results.push_back(getNodeData(curIndex)); 344 } 345 346 if (aabbOverlap || isleafnode) 347 { 348 //next subnode 349 curIndex++; 350 } 351 else 352 { 353 //skip node 354 curIndex += getScapeNodeIndex(curIndex); 355 } 356 } 357 if (collided_results.size() > 0) return true; 358 return false; 359 } 360 361 //! tells if this set has hierarcht hasHierarchy()362 SIMD_FORCE_INLINE bool hasHierarchy() const 363 { 364 return true; 365 } 366 367 //! tells if this set is a trimesh isTrimesh()368 SIMD_FORCE_INLINE bool isTrimesh() const 369 { 370 return m_primitive_manager.is_trimesh(); 371 } 372 373 //! node count getNodeCount()374 SIMD_FORCE_INLINE GUINT getNodeCount() const 375 { 376 return m_box_tree.getNodeCount(); 377 } 378 379 //! tells if the node is a leaf isLeafNode(GUINT nodeindex)380 SIMD_FORCE_INLINE bool isLeafNode(GUINT nodeindex) const 381 { 382 return m_box_tree.isLeafNode(nodeindex); 383 } 384 getNodeData(GUINT nodeindex)385 SIMD_FORCE_INLINE GUINT getNodeData(GUINT nodeindex) const 386 { 387 return m_box_tree.getNodeData(nodeindex); 388 } 389 getNodeBound(GUINT nodeindex,GIM_AABB & bound)390 SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB& bound) const 391 { 392 m_box_tree.getNodeBound(nodeindex, bound); 393 } 394 setNodeBound(GUINT nodeindex,const GIM_AABB & bound)395 SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB& bound) 396 { 397 m_box_tree.setNodeBound(nodeindex, bound); 398 } 399 getLeftNodeIndex(GUINT nodeindex)400 SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex) const 401 { 402 return m_box_tree.getLeftNodeIndex(nodeindex); 403 } 404 getRightNodeIndex(GUINT nodeindex)405 SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex) const 406 { 407 return m_box_tree.getRightNodeIndex(nodeindex); 408 } 409 getScapeNodeIndex(GUINT nodeindex)410 SIMD_FORCE_INLINE GUINT getScapeNodeIndex(GUINT nodeindex) const 411 { 412 return m_box_tree.getScapeNodeIndex(nodeindex); 413 } 414 getNodeTriangle(GUINT nodeindex,GIM_TRIANGLE & triangle)415 SIMD_FORCE_INLINE void getNodeTriangle(GUINT nodeindex, GIM_TRIANGLE& triangle) const 416 { 417 m_primitive_manager.get_primitive_triangle(getNodeData(nodeindex), triangle); 418 } 419 }; 420 421 //! Class for Box Tree Sets 422 /*! 423 this has the GIM_BOX_TREE implementation for bounding boxes. 424 */ 425 template <typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE> 426 class GIM_BOX_TREE_SET : public GIM_BOX_TREE_TEMPLATE_SET<_GIM_PRIMITIVE_MANAGER_PROTOTYPE, GIM_BOX_TREE> 427 { 428 public: 429 }; 430 431 /// GIM_BOX_SET collision methods 432 template <typename BOX_SET_CLASS0, typename BOX_SET_CLASS1> 433 class GIM_TREE_TREE_COLLIDER 434 { 435 public: 436 gim_pair_set* m_collision_pairs; 437 BOX_SET_CLASS0* m_boxset0; 438 BOX_SET_CLASS1* m_boxset1; 439 GUINT current_node0; 440 GUINT current_node1; 441 bool node0_is_leaf; 442 bool node1_is_leaf; 443 bool t0_is_trimesh; 444 bool t1_is_trimesh; 445 bool node0_has_triangle; 446 bool node1_has_triangle; 447 GIM_AABB m_box0; 448 GIM_AABB m_box1; 449 GIM_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0; 450 btTransform trans_cache_0to1; 451 GIM_TRIANGLE m_tri0; 452 btVector4 m_tri0_plane; 453 GIM_TRIANGLE m_tri1; 454 btVector4 m_tri1_plane; 455 456 public: GIM_TREE_TREE_COLLIDER()457 GIM_TREE_TREE_COLLIDER() 458 { 459 current_node0 = G_UINT_INFINITY; 460 current_node1 = G_UINT_INFINITY; 461 } 462 463 protected: retrieve_node0_triangle(GUINT node0)464 SIMD_FORCE_INLINE void retrieve_node0_triangle(GUINT node0) 465 { 466 if (node0_has_triangle) return; 467 m_boxset0->getNodeTriangle(node0, m_tri0); 468 //transform triangle 469 m_tri0.m_vertices[0] = trans_cache_0to1(m_tri0.m_vertices[0]); 470 m_tri0.m_vertices[1] = trans_cache_0to1(m_tri0.m_vertices[1]); 471 m_tri0.m_vertices[2] = trans_cache_0to1(m_tri0.m_vertices[2]); 472 m_tri0.get_plane(m_tri0_plane); 473 474 node0_has_triangle = true; 475 } 476 retrieve_node1_triangle(GUINT node1)477 SIMD_FORCE_INLINE void retrieve_node1_triangle(GUINT node1) 478 { 479 if (node1_has_triangle) return; 480 m_boxset1->getNodeTriangle(node1, m_tri1); 481 //transform triangle 482 m_tri1.m_vertices[0] = trans_cache_1to0.transform(m_tri1.m_vertices[0]); 483 m_tri1.m_vertices[1] = trans_cache_1to0.transform(m_tri1.m_vertices[1]); 484 m_tri1.m_vertices[2] = trans_cache_1to0.transform(m_tri1.m_vertices[2]); 485 m_tri1.get_plane(m_tri1_plane); 486 487 node1_has_triangle = true; 488 } 489 retrieve_node0_info(GUINT node0)490 SIMD_FORCE_INLINE void retrieve_node0_info(GUINT node0) 491 { 492 if (node0 == current_node0) return; 493 m_boxset0->getNodeBound(node0, m_box0); 494 node0_is_leaf = m_boxset0->isLeafNode(node0); 495 node0_has_triangle = false; 496 current_node0 = node0; 497 } 498 retrieve_node1_info(GUINT node1)499 SIMD_FORCE_INLINE void retrieve_node1_info(GUINT node1) 500 { 501 if (node1 == current_node1) return; 502 m_boxset1->getNodeBound(node1, m_box1); 503 node1_is_leaf = m_boxset1->isLeafNode(node1); 504 node1_has_triangle = false; 505 current_node1 = node1; 506 } 507 node_collision(GUINT node0,GUINT node1)508 SIMD_FORCE_INLINE bool node_collision(GUINT node0, GUINT node1) 509 { 510 retrieve_node0_info(node0); 511 retrieve_node1_info(node1); 512 bool result = m_box0.overlapping_trans_cache(m_box1, trans_cache_1to0, true); 513 if (!result) return false; 514 515 if (t0_is_trimesh && node0_is_leaf) 516 { 517 //perform primitive vs box collision 518 retrieve_node0_triangle(node0); 519 //do triangle vs box collision 520 m_box1.increment_margin(m_tri0.m_margin); 521 522 result = m_box1.collide_triangle_exact( 523 m_tri0.m_vertices[0], m_tri0.m_vertices[1], m_tri0.m_vertices[2], m_tri0_plane); 524 525 m_box1.increment_margin(-m_tri0.m_margin); 526 527 if (!result) return false; 528 return true; 529 } 530 else if (t1_is_trimesh && node1_is_leaf) 531 { 532 //perform primitive vs box collision 533 retrieve_node1_triangle(node1); 534 //do triangle vs box collision 535 m_box0.increment_margin(m_tri1.m_margin); 536 537 result = m_box0.collide_triangle_exact( 538 m_tri1.m_vertices[0], m_tri1.m_vertices[1], m_tri1.m_vertices[2], m_tri1_plane); 539 540 m_box0.increment_margin(-m_tri1.m_margin); 541 542 if (!result) return false; 543 return true; 544 } 545 return true; 546 } 547 548 //stackless collision routine find_collision_pairs()549 void find_collision_pairs() 550 { 551 gim_pair_set stack_collisions; 552 stack_collisions.reserve(32); 553 554 //add the first pair 555 stack_collisions.push_pair(0, 0); 556 557 while (stack_collisions.size()) 558 { 559 //retrieve the last pair and pop 560 GUINT node0 = stack_collisions.back().m_index1; 561 GUINT node1 = stack_collisions.back().m_index2; 562 stack_collisions.pop_back(); 563 if (node_collision(node0, node1)) // a collision is found 564 { 565 if (node0_is_leaf) 566 { 567 if (node1_is_leaf) 568 { 569 m_collision_pairs->push_pair(m_boxset0->getNodeData(node0), m_boxset1->getNodeData(node1)); 570 } 571 else 572 { 573 //collide left 574 stack_collisions.push_pair(node0, m_boxset1->getLeftNodeIndex(node1)); 575 576 //collide right 577 stack_collisions.push_pair(node0, m_boxset1->getRightNodeIndex(node1)); 578 } 579 } 580 else 581 { 582 if (node1_is_leaf) 583 { 584 //collide left 585 stack_collisions.push_pair(m_boxset0->getLeftNodeIndex(node0), node1); 586 //collide right 587 stack_collisions.push_pair(m_boxset0->getRightNodeIndex(node0), node1); 588 } 589 else 590 { 591 GUINT left0 = m_boxset0->getLeftNodeIndex(node0); 592 GUINT right0 = m_boxset0->getRightNodeIndex(node0); 593 GUINT left1 = m_boxset1->getLeftNodeIndex(node1); 594 GUINT right1 = m_boxset1->getRightNodeIndex(node1); 595 //collide left 596 stack_collisions.push_pair(left0, left1); 597 //collide right 598 stack_collisions.push_pair(left0, right1); 599 //collide left 600 stack_collisions.push_pair(right0, left1); 601 //collide right 602 stack_collisions.push_pair(right0, right1); 603 604 } // else if node1 is not a leaf 605 } // else if node0 is not a leaf 606 607 } // if(node_collision(node0,node1)) 608 } //while(stack_collisions.size()) 609 } 610 611 public: 612 void find_collision(BOX_SET_CLASS0* boxset1, const btTransform& trans1, 613 BOX_SET_CLASS1* boxset2, const btTransform& trans2, 614 gim_pair_set& collision_pairs, bool complete_primitive_tests = true) 615 { 616 m_collision_pairs = &collision_pairs; 617 m_boxset0 = boxset1; 618 m_boxset1 = boxset2; 619 620 trans_cache_1to0.calc_from_homogenic(trans1, trans2); 621 622 trans_cache_0to1 = trans2.inverse(); 623 trans_cache_0to1 *= trans1; 624 625 if (complete_primitive_tests) 626 { 627 t0_is_trimesh = boxset1->getPrimitiveManager().is_trimesh(); 628 t1_is_trimesh = boxset2->getPrimitiveManager().is_trimesh(); 629 } 630 else 631 { 632 t0_is_trimesh = false; 633 t1_is_trimesh = false; 634 } 635 636 find_collision_pairs(); 637 } 638 }; 639 640 #endif // GIM_BOXPRUNING_H_INCLUDED 641