dali-physics/third-party/bullet3/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp

   1 /*! \file gim_box_set.h
   2 \author Francisco Leon Najera
   3 */
   4 /*
   5 This source file is part of GIMPACT Library.
   6
   7 For the latest info, see http://gimpact.sourceforge.net/
   8
   9 Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
  10 email: projectileman@yahoo.com
  11
  12
  13 This software is provided 'as-is', without any express or implied warranty.
  14 In no event will the authors be held liable for any damages arising from the use of this software.
  15 Permission is granted to anyone to use this software for any purpose,
  16 including commercial applications, and to alter it and redistribute it freely,
  17 subject to the following restrictions:
  18
  19 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  20 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  21 3. This notice may not be removed or altered from any source distribution.
  22 */
  23
  24 #include "btGImpactQuantizedBvh.h"
  25 #include "LinearMath/btQuickprof.h"
  26
  27 #ifdef TRI_COLLISION_PROFILING
  28 btClock g_q_tree_clock;
  29
  30 float g_q_accum_tree_collision_time = 0;
  31 int g_q_count_traversing = 0;
  32
  33 void bt_begin_gim02_q_tree_time()
  34 {
  35         g_q_tree_clock.reset();
  36 }
  37
  38 void bt_end_gim02_q_tree_time()
  39 {
  40         g_q_accum_tree_collision_time += g_q_tree_clock.getTimeMicroseconds();
  41         g_q_count_traversing++;
  42 }
  43
  44 //! Gets the average time in miliseconds of tree collisions
  45 float btGImpactQuantizedBvh::getAverageTreeCollisionTime()
  46 {
  47         if (g_q_count_traversing == 0) return 0;
  48
  49         float avgtime = g_q_accum_tree_collision_time;
  50         avgtime /= (float)g_q_count_traversing;
  51
  52         g_q_accum_tree_collision_time = 0;
  53         g_q_count_traversing = 0;
  54         return avgtime;
  55
  56         //      float avgtime = g_q_count_traversing;
  57         //      g_q_count_traversing = 0;
  58         //      return avgtime;
  59 }
  60
  61 #endif  //TRI_COLLISION_PROFILING
  62
  63 /////////////////////// btQuantizedBvhTree /////////////////////////////////
  64
  65 void btQuantizedBvhTree::calc_quantization(
  66         GIM_BVH_DATA_ARRAY& primitive_boxes, btScalar boundMargin)
  67 {
  68         //calc globa box
  69         btAABB global_bound;
  70         global_bound.invalidate();
  71
  72         for (int i = 0; i < primitive_boxes.size(); i++)
  73         {
  74                 global_bound.merge(primitive_boxes[i].m_bound);
  75         }
  76
  77         bt_calc_quantization_parameters(
  78                 m_global_bound.m_min, m_global_bound.m_max, m_bvhQuantization, global_bound.m_min, global_bound.m_max, boundMargin);
  79 }
  80
  81 int btQuantizedBvhTree::_calc_splitting_axis(
  82         GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex)
  83 {
  84         int i;
  85
  86         btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.));
  87         btVector3 variance(btScalar(0.), btScalar(0.), btScalar(0.));
  88         int numIndices = endIndex - startIndex;
  89
  90         for (i = startIndex; i < endIndex; i++)
  91         {
  92                 btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
  93                                                                                         primitive_boxes[i].m_bound.m_min);
  94                 means += center;
  95         }
  96         means *= (btScalar(1.) / (btScalar)numIndices);
  97
  98         for (i = startIndex; i < endIndex; i++)
  99         {
 100                 btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
 101                                                                                         primitive_boxes[i].m_bound.m_min);
 102                 btVector3 diff2 = center - means;
 103                 diff2 = diff2 * diff2;
 104                 variance += diff2;
 105         }
 106         variance *= (btScalar(1.) / ((btScalar)numIndices - 1));
 107
 108         return variance.maxAxis();
 109 }
 110
 111 int btQuantizedBvhTree::_sort_and_calc_splitting_index(
 112         GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex,
 113         int endIndex, int splitAxis)
 114 {
 115         int i;
 116         int splitIndex = startIndex;
 117         int numIndices = endIndex - startIndex;
 118
 119         // average of centers
 120         btScalar splitValue = 0.0f;
 121
 122         btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.));
 123         for (i = startIndex; i < endIndex; i++)
 124         {
 125                 btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
 126                                                                                         primitive_boxes[i].m_bound.m_min);
 127                 means += center;
 128         }
 129         means *= (btScalar(1.) / (btScalar)numIndices);
 130
 131         splitValue = means[splitAxis];
 132
 133         //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
 134         for (i = startIndex; i < endIndex; i++)
 135         {
 136                 btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
 137                                                                                         primitive_boxes[i].m_bound.m_min);
 138                 if (center[splitAxis] > splitValue)
 139                 {
 140                         //swap
 141                         primitive_boxes.swap(i, splitIndex);
 142                         //swapLeafNodes(i,splitIndex);
 143                         splitIndex++;
 144                 }
 145         }
 146
 147         //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
 148         //otherwise the tree-building might fail due to stack-overflows in certain cases.
 149         //unbalanced1 is unsafe: it can cause stack overflows
 150         //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
 151
 152         //unbalanced2 should work too: always use center (perfect balanced trees)
 153         //bool unbalanced2 = true;
 154
 155         //this should be safe too:
 156         int rangeBalancedIndices = numIndices / 3;
 157         bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices)));
 158
 159         if (unbalanced)
 160         {
 161                 splitIndex = startIndex + (numIndices >> 1);
 162         }
 163
 164         btAssert(!((splitIndex == startIndex) || (splitIndex == (endIndex))));
 165
 166         return splitIndex;
 167 }
 168
 169 void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex)
 170 {
 171         int curIndex = m_num_nodes;
 172         m_num_nodes++;
 173
 174         btAssert((endIndex - startIndex) > 0);
 175
 176         if ((endIndex - startIndex) == 1)
 177         {
 178                 //We have a leaf node
 179                 setNodeBound(curIndex, primitive_boxes[startIndex].m_bound);
 180                 m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);
 181
 182                 return;
 183         }
 184         //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
 185
 186         //split axis
 187         int splitIndex = _calc_splitting_axis(primitive_boxes, startIndex, endIndex);
 188
 189         splitIndex = _sort_and_calc_splitting_index(
 190                 primitive_boxes, startIndex, endIndex,
 191                 splitIndex  //split axis
 192         );
 193
 194         //calc this node bounding box
 195
 196         btAABB node_bound;
 197         node_bound.invalidate();
 198
 199         for (int i = startIndex; i < endIndex; i++)
 200         {
 201                 node_bound.merge(primitive_boxes[i].m_bound);
 202         }
 203
 204         setNodeBound(curIndex, node_bound);
 205
 206         //build left branch
 207         _build_sub_tree(primitive_boxes, startIndex, splitIndex);
 208
 209         //build right branch
 210         _build_sub_tree(primitive_boxes, splitIndex, endIndex);
 211
 212         m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex);
 213 }
 214
 215 //! stackless build tree
 216 void btQuantizedBvhTree::build_tree(
 217         GIM_BVH_DATA_ARRAY& primitive_boxes)
 218 {
 219         calc_quantization(primitive_boxes);
 220         // initialize node count to 0
 221         m_num_nodes = 0;
 222         // allocate nodes
 223         m_node_array.resize(primitive_boxes.size() * 2);
 224
 225         _build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
 226 }
 227
 228 ////////////////////////////////////class btGImpactQuantizedBvh
 229
 230 void btGImpactQuantizedBvh::refit()
 231 {
 232         int nodecount = getNodeCount();
 233         while (nodecount--)
 234         {
 235                 if (isLeafNode(nodecount))
 236                 {
 237                         btAABB leafbox;
 238                         m_primitive_manager->get_primitive_box(getNodeData(nodecount), leafbox);
 239                         setNodeBound(nodecount, leafbox);
 240                 }
 241                 else
 242                 {
 243                         //const GIM_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount);
 244                         //get left bound
 245                         btAABB bound;
 246                         bound.invalidate();
 247
 248                         btAABB temp_box;
 249
 250                         int child_node = getLeftNode(nodecount);
 251                         if (child_node)
 252                         {
 253                                 getNodeBound(child_node, temp_box);
 254                                 bound.merge(temp_box);
 255                         }
 256
 257                         child_node = getRightNode(nodecount);
 258                         if (child_node)
 259                         {
 260                                 getNodeBound(child_node, temp_box);
 261                                 bound.merge(temp_box);
 262                         }
 263
 264                         setNodeBound(nodecount, bound);
 265                 }
 266         }
 267 }
 268
 269 //! this rebuild the entire set
 270 void btGImpactQuantizedBvh::buildSet()
 271 {
 272         //obtain primitive boxes
 273         GIM_BVH_DATA_ARRAY primitive_boxes;
 274         primitive_boxes.resize(m_primitive_manager->get_primitive_count());
 275
 276         for (int i = 0; i < primitive_boxes.size(); i++)
 277         {
 278                 m_primitive_manager->get_primitive_box(i, primitive_boxes[i].m_bound);
 279                 primitive_boxes[i].m_data = i;
 280         }
 281
 282         m_box_tree.build_tree(primitive_boxes);
 283 }
 284
 285 //! returns the indices of the primitives in the m_primitive_manager
 286 bool btGImpactQuantizedBvh::boxQuery(const btAABB& box, btAlignedObjectArray<int>& collided_results) const
 287 {
 288         int curIndex = 0;
 289         int numNodes = getNodeCount();
 290
 291         //quantize box
 292
 293         unsigned short quantizedMin[3];
 294         unsigned short quantizedMax[3];
 295
 296         m_box_tree.quantizePoint(quantizedMin, box.m_min);
 297         m_box_tree.quantizePoint(quantizedMax, box.m_max);
 298
 299         while (curIndex < numNodes)
 300         {
 301                 //catch bugs in tree data
 302
 303                 bool aabbOverlap = m_box_tree.testQuantizedBoxOverlapp(curIndex, quantizedMin, quantizedMax);
 304                 bool isleafnode = isLeafNode(curIndex);
 305
 306                 if (isleafnode && aabbOverlap)
 307                 {
 308                         collided_results.push_back(getNodeData(curIndex));
 309                 }
 310
 311                 if (aabbOverlap || isleafnode)
 312                 {
 313                         //next subnode
 314                         curIndex++;
 315                 }
 316                 else
 317                 {
 318                         //skip node
 319                         curIndex += getEscapeNodeIndex(curIndex);
 320                 }
 321         }
 322         if (collided_results.size() > 0) return true;
 323         return false;
 324 }
 325
 326 //! returns the indices of the primitives in the m_primitive_manager
 327 bool btGImpactQuantizedBvh::rayQuery(
 328         const btVector3& ray_dir, const btVector3& ray_origin,
 329         btAlignedObjectArray<int>& collided_results) const
 330 {
 331         int curIndex = 0;
 332         int numNodes = getNodeCount();
 333
 334         while (curIndex < numNodes)
 335         {
 336                 btAABB bound;
 337                 getNodeBound(curIndex, bound);
 338
 339                 //catch bugs in tree data
 340
 341                 bool aabbOverlap = bound.collide_ray(ray_origin, ray_dir);
 342                 bool isleafnode = isLeafNode(curIndex);
 343
 344                 if (isleafnode && aabbOverlap)
 345                 {
 346                         collided_results.push_back(getNodeData(curIndex));
 347                 }
 348
 349                 if (aabbOverlap || isleafnode)
 350                 {
 351                         //next subnode
 352                         curIndex++;
 353                 }
 354                 else
 355                 {
 356                         //skip node
 357                         curIndex += getEscapeNodeIndex(curIndex);
 358                 }
 359         }
 360         if (collided_results.size() > 0) return true;
 361         return false;
 362 }
 363
 364 SIMD_FORCE_INLINE bool _quantized_node_collision(
 365         const btGImpactQuantizedBvh* boxset0, const btGImpactQuantizedBvh* boxset1,
 366         const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0,
 367         int node0, int node1, bool complete_primitive_tests)
 368 {
 369         btAABB box0;
 370         boxset0->getNodeBound(node0, box0);
 371         btAABB box1;
 372         boxset1->getNodeBound(node1, box1);
 373
 374         return box0.overlapping_trans_cache(box1, trans_cache_1to0, complete_primitive_tests);
 375         //      box1.appy_transform_trans_cache(trans_cache_1to0);
 376         //      return box0.has_collision(box1);
 377 }
 378
 379 //stackless recursive collision routine
 380 static void _find_quantized_collision_pairs_recursive(
 381         const btGImpactQuantizedBvh* boxset0, const btGImpactQuantizedBvh* boxset1,
 382         btPairSet* collision_pairs,
 383         const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0,
 384         int node0, int node1, bool complete_primitive_tests)
 385 {
 386         if (_quantized_node_collision(
 387                         boxset0, boxset1, trans_cache_1to0,
 388                         node0, node1, complete_primitive_tests) == false) return;  //avoid colliding internal nodes
 389
 390         if (boxset0->isLeafNode(node0))
 391         {
 392                 if (boxset1->isLeafNode(node1))
 393                 {
 394                         // collision result
 395                         collision_pairs->push_pair(
 396                                 boxset0->getNodeData(node0), boxset1->getNodeData(node1));
 397                         return;
 398                 }
 399                 else
 400                 {
 401                         //collide left recursive
 402
 403                         _find_quantized_collision_pairs_recursive(
 404                                 boxset0, boxset1,
 405                                 collision_pairs, trans_cache_1to0,
 406                                 node0, boxset1->getLeftNode(node1), false);
 407
 408                         //collide right recursive
 409                         _find_quantized_collision_pairs_recursive(
 410                                 boxset0, boxset1,
 411                                 collision_pairs, trans_cache_1to0,
 412                                 node0, boxset1->getRightNode(node1), false);
 413                 }
 414         }
 415         else
 416         {
 417                 if (boxset1->isLeafNode(node1))
 418                 {
 419                         //collide left recursive
 420                         _find_quantized_collision_pairs_recursive(
 421                                 boxset0, boxset1,
 422                                 collision_pairs, trans_cache_1to0,
 423                                 boxset0->getLeftNode(node0), node1, false);
 424
 425                         //collide right recursive
 426
 427                         _find_quantized_collision_pairs_recursive(
 428                                 boxset0, boxset1,
 429                                 collision_pairs, trans_cache_1to0,
 430                                 boxset0->getRightNode(node0), node1, false);
 431                 }
 432                 else
 433                 {
 434                         //collide left0 left1
 435
 436                         _find_quantized_collision_pairs_recursive(
 437                                 boxset0, boxset1,
 438                                 collision_pairs, trans_cache_1to0,
 439                                 boxset0->getLeftNode(node0), boxset1->getLeftNode(node1), false);
 440
 441                         //collide left0 right1
 442
 443                         _find_quantized_collision_pairs_recursive(
 444                                 boxset0, boxset1,
 445                                 collision_pairs, trans_cache_1to0,
 446                                 boxset0->getLeftNode(node0), boxset1->getRightNode(node1), false);
 447
 448                         //collide right0 left1
 449
 450                         _find_quantized_collision_pairs_recursive(
 451                                 boxset0, boxset1,
 452                                 collision_pairs, trans_cache_1to0,
 453                                 boxset0->getRightNode(node0), boxset1->getLeftNode(node1), false);
 454
 455                         //collide right0 right1
 456
 457                         _find_quantized_collision_pairs_recursive(
 458                                 boxset0, boxset1,
 459                                 collision_pairs, trans_cache_1to0,
 460                                 boxset0->getRightNode(node0), boxset1->getRightNode(node1), false);
 461
 462                 }  // else if node1 is not a leaf
 463         }      // else if node0 is not a leaf
 464 }
 465
 466 void btGImpactQuantizedBvh::find_collision(const btGImpactQuantizedBvh* boxset0, const btTransform& trans0,
 467                                                                                    const btGImpactQuantizedBvh* boxset1, const btTransform& trans1,
 468                                                                                    btPairSet& collision_pairs)
 469 {
 470         if (boxset0->getNodeCount() == 0 || boxset1->getNodeCount() == 0) return;
 471
 472         BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
 473
 474         trans_cache_1to0.calc_from_homogenic(trans0, trans1);
 475
 476 #ifdef TRI_COLLISION_PROFILING
 477         bt_begin_gim02_q_tree_time();
 478 #endif  //TRI_COLLISION_PROFILING
 479
 480         _find_quantized_collision_pairs_recursive(
 481                 boxset0, boxset1,
 482                 &collision_pairs, trans_cache_1to0, 0, 0, true);
 483 #ifdef TRI_COLLISION_PROFILING
 484         bt_end_gim02_q_tree_time();
 485 #endif  //TRI_COLLISION_PROFILING
 486 }