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