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