dali-physics/third-party/bullet3/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 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.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15 ///b3DynamicBvh implementation by Nathanael Presson
  16
  17 #include "b3DynamicBvh.h"
  18
  19 //
  20 typedef b3AlignedObjectArray<b3DbvtNode*> b3NodeArray;
  21 typedef b3AlignedObjectArray<const b3DbvtNode*> b3ConstNodeArray;
  22
  23 //
  24 struct b3DbvtNodeEnumerator : b3DynamicBvh::ICollide
  25 {
  26         b3ConstNodeArray nodes;
  27         void Process(const b3DbvtNode* n) { nodes.push_back(n); }
  28 };
  29
  30 //
  31 static B3_DBVT_INLINE int b3IndexOf(const b3DbvtNode* node)
  32 {
  33         return (node->parent->childs[1] == node);
  34 }
  35
  36 //
  37 static B3_DBVT_INLINE b3DbvtVolume b3Merge(const b3DbvtVolume& a,
  38                                                                                    const b3DbvtVolume& b)
  39 {
  40 #if (B3_DBVT_MERGE_IMPL == B3_DBVT_IMPL_SSE)
  41         B3_ATTRIBUTE_ALIGNED16(char locals[sizeof(b3DbvtAabbMm)]);
  42         b3DbvtVolume& res = *(b3DbvtVolume*)locals;
  43 #else
  44         b3DbvtVolume res;
  45 #endif
  46         b3Merge(a, b, res);
  47         return (res);
  48 }
  49
  50 // volume+edge lengths
  51 static B3_DBVT_INLINE b3Scalar b3Size(const b3DbvtVolume& a)
  52 {
  53         const b3Vector3 edges = a.Lengths();
  54         return (edges.x * edges.y * edges.z +
  55                         edges.x + edges.y + edges.z);
  56 }
  57
  58 //
  59 static void b3GetMaxDepth(const b3DbvtNode* node, int depth, int& maxdepth)
  60 {
  61         if (node->isinternal())
  62         {
  63                 b3GetMaxDepth(node->childs[0], depth + 1, maxdepth);
  64                 b3GetMaxDepth(node->childs[1], depth + 1, maxdepth);
  65         }
  66         else
  67                 maxdepth = b3Max(maxdepth, depth);
  68 }
  69
  70 //
  71 static B3_DBVT_INLINE void b3DeleteNode(b3DynamicBvh* pdbvt,
  72                                                                                 b3DbvtNode* node)
  73 {
  74         b3AlignedFree(pdbvt->m_free);
  75         pdbvt->m_free = node;
  76 }
  77
  78 //
  79 static void b3RecurseDeleteNode(b3DynamicBvh* pdbvt,
  80                                                                 b3DbvtNode* node)
  81 {
  82         if (!node->isleaf())
  83         {
  84                 b3RecurseDeleteNode(pdbvt, node->childs[0]);
  85                 b3RecurseDeleteNode(pdbvt, node->childs[1]);
  86         }
  87         if (node == pdbvt->m_root) pdbvt->m_root = 0;
  88         b3DeleteNode(pdbvt, node);
  89 }
  90
  91 //
  92 static B3_DBVT_INLINE b3DbvtNode* b3CreateNode(b3DynamicBvh* pdbvt,
  93                                                                                            b3DbvtNode* parent,
  94                                                                                            void* data)
  95 {
  96         b3DbvtNode* node;
  97         if (pdbvt->m_free)
  98         {
  99                 node = pdbvt->m_free;
 100                 pdbvt->m_free = 0;
 101         }
 102         else
 103         {
 104                 node = new (b3AlignedAlloc(sizeof(b3DbvtNode), 16)) b3DbvtNode();
 105         }
 106         node->parent = parent;
 107         node->data = data;
 108         node->childs[1] = 0;
 109         return (node);
 110 }
 111
 112 //
 113 static B3_DBVT_INLINE b3DbvtNode* b3CreateNode(b3DynamicBvh* pdbvt,
 114                                                                                            b3DbvtNode* parent,
 115                                                                                            const b3DbvtVolume& volume,
 116                                                                                            void* data)
 117 {
 118         b3DbvtNode* node = b3CreateNode(pdbvt, parent, data);
 119         node->volume = volume;
 120         return (node);
 121 }
 122
 123 //
 124 static B3_DBVT_INLINE b3DbvtNode* b3CreateNode(b3DynamicBvh* pdbvt,
 125                                                                                            b3DbvtNode* parent,
 126                                                                                            const b3DbvtVolume& volume0,
 127                                                                                            const b3DbvtVolume& volume1,
 128                                                                                            void* data)
 129 {
 130         b3DbvtNode* node = b3CreateNode(pdbvt, parent, data);
 131         b3Merge(volume0, volume1, node->volume);
 132         return (node);
 133 }
 134
 135 //
 136 static void b3InsertLeaf(b3DynamicBvh* pdbvt,
 137                                                  b3DbvtNode* root,
 138                                                  b3DbvtNode* leaf)
 139 {
 140         if (!pdbvt->m_root)
 141         {
 142                 pdbvt->m_root = leaf;
 143                 leaf->parent = 0;
 144         }
 145         else
 146         {
 147                 if (!root->isleaf())
 148                 {
 149                         do
 150                         {
 151                                 root = root->childs[b3Select(leaf->volume,
 152                                                                                          root->childs[0]->volume,
 153                                                                                          root->childs[1]->volume)];
 154                         } while (!root->isleaf());
 155                 }
 156                 b3DbvtNode* prev = root->parent;
 157                 b3DbvtNode* node = b3CreateNode(pdbvt, prev, leaf->volume, root->volume, 0);
 158                 if (prev)
 159                 {
 160                         prev->childs[b3IndexOf(root)] = node;
 161                         node->childs[0] = root;
 162                         root->parent = node;
 163                         node->childs[1] = leaf;
 164                         leaf->parent = node;
 165                         do
 166                         {
 167                                 if (!prev->volume.Contain(node->volume))
 168                                         b3Merge(prev->childs[0]->volume, prev->childs[1]->volume, prev->volume);
 169                                 else
 170                                         break;
 171                                 node = prev;
 172                         } while (0 != (prev = node->parent));
 173                 }
 174                 else
 175                 {
 176                         node->childs[0] = root;
 177                         root->parent = node;
 178                         node->childs[1] = leaf;
 179                         leaf->parent = node;
 180                         pdbvt->m_root = node;
 181                 }
 182         }
 183 }
 184
 185 //
 186 static b3DbvtNode* b3RemoveLeaf(b3DynamicBvh* pdbvt,
 187                                                                 b3DbvtNode* leaf)
 188 {
 189         if (leaf == pdbvt->m_root)
 190         {
 191                 pdbvt->m_root = 0;
 192                 return (0);
 193         }
 194         else
 195         {
 196                 b3DbvtNode* parent = leaf->parent;
 197                 b3DbvtNode* prev = parent->parent;
 198                 b3DbvtNode* sibling = parent->childs[1 - b3IndexOf(leaf)];
 199                 if (prev)
 200                 {
 201                         prev->childs[b3IndexOf(parent)] = sibling;
 202                         sibling->parent = prev;
 203                         b3DeleteNode(pdbvt, parent);
 204                         while (prev)
 205                         {
 206                                 const b3DbvtVolume pb = prev->volume;
 207                                 b3Merge(prev->childs[0]->volume, prev->childs[1]->volume, prev->volume);
 208                                 if (b3NotEqual(pb, prev->volume))
 209                                 {
 210                                         prev = prev->parent;
 211                                 }
 212                                 else
 213                                         break;
 214                         }
 215                         return (prev ? prev : pdbvt->m_root);
 216                 }
 217                 else
 218                 {
 219                         pdbvt->m_root = sibling;
 220                         sibling->parent = 0;
 221                         b3DeleteNode(pdbvt, parent);
 222                         return (pdbvt->m_root);
 223                 }
 224         }
 225 }
 226
 227 //
 228 static void b3FetchLeaves(b3DynamicBvh* pdbvt,
 229                                                   b3DbvtNode* root,
 230                                                   b3NodeArray& leaves,
 231                                                   int depth = -1)
 232 {
 233         if (root->isinternal() && depth)
 234         {
 235                 b3FetchLeaves(pdbvt, root->childs[0], leaves, depth - 1);
 236                 b3FetchLeaves(pdbvt, root->childs[1], leaves, depth - 1);
 237                 b3DeleteNode(pdbvt, root);
 238         }
 239         else
 240         {
 241                 leaves.push_back(root);
 242         }
 243 }
 244
 245 static bool b3LeftOfAxis(const b3DbvtNode* node,
 246                                                  const b3Vector3& org,
 247                                                  const b3Vector3& axis)
 248 {
 249         return b3Dot(axis, node->volume.Center() - org) <= 0;
 250 }
 251
 252 // Partitions leaves such that leaves[0, n) are on the
 253 // left of axis, and leaves[n, count) are on the right
 254 // of axis. returns N.
 255 static int b3Split(b3DbvtNode** leaves,
 256                                    int count,
 257                                    const b3Vector3& org,
 258                                    const b3Vector3& axis)
 259 {
 260         int begin = 0;
 261         int end = count;
 262         for (;;)
 263         {
 264                 while (begin != end && b3LeftOfAxis(leaves[begin], org, axis))
 265                 {
 266                         ++begin;
 267                 }
 268
 269                 if (begin == end)
 270                 {
 271                         break;
 272                 }
 273
 274                 while (begin != end && !b3LeftOfAxis(leaves[end - 1], org, axis))
 275                 {
 276                         --end;
 277                 }
 278
 279                 if (begin == end)
 280                 {
 281                         break;
 282                 }
 283
 284                 // swap out of place nodes
 285                 --end;
 286                 b3DbvtNode* temp = leaves[begin];
 287                 leaves[begin] = leaves[end];
 288                 leaves[end] = temp;
 289                 ++begin;
 290         }
 291
 292         return begin;
 293 }
 294
 295 //
 296 static b3DbvtVolume b3Bounds(b3DbvtNode** leaves,
 297                                                          int count)
 298 {
 299 #if B3_DBVT_MERGE_IMPL == B3_DBVT_IMPL_SSE
 300         B3_ATTRIBUTE_ALIGNED16(char locals[sizeof(b3DbvtVolume)]);
 301         b3DbvtVolume& volume = *(b3DbvtVolume*)locals;
 302         volume = leaves[0]->volume;
 303 #else
 304         b3DbvtVolume volume = leaves[0]->volume;
 305 #endif
 306         for (int i = 1, ni = count; i < ni; ++i)
 307         {
 308                 b3Merge(volume, leaves[i]->volume, volume);
 309         }
 310         return (volume);
 311 }
 312
 313 //
 314 static void b3BottomUp(b3DynamicBvh* pdbvt,
 315                                            b3DbvtNode** leaves,
 316                                            int count)
 317 {
 318         while (count > 1)
 319         {
 320                 b3Scalar minsize = B3_INFINITY;
 321                 int minidx[2] = {-1, -1};
 322                 for (int i = 0; i < count; ++i)
 323                 {
 324                         for (int j = i + 1; j < count; ++j)
 325                         {
 326                                 const b3Scalar sz = b3Size(b3Merge(leaves[i]->volume, leaves[j]->volume));
 327                                 if (sz < minsize)
 328                                 {
 329                                         minsize = sz;
 330                                         minidx[0] = i;
 331                                         minidx[1] = j;
 332                                 }
 333                         }
 334                 }
 335                 b3DbvtNode* n[] = {leaves[minidx[0]], leaves[minidx[1]]};
 336                 b3DbvtNode* p = b3CreateNode(pdbvt, 0, n[0]->volume, n[1]->volume, 0);
 337                 p->childs[0] = n[0];
 338                 p->childs[1] = n[1];
 339                 n[0]->parent = p;
 340                 n[1]->parent = p;
 341                 leaves[minidx[0]] = p;
 342                 leaves[minidx[1]] = leaves[count - 1];
 343                 --count;
 344         }
 345 }
 346
 347 //
 348 static b3DbvtNode* b3TopDown(b3DynamicBvh* pdbvt,
 349                                                          b3DbvtNode** leaves,
 350                                                          int count,
 351                                                          int bu_treshold)
 352 {
 353         static const b3Vector3 axis[] = {b3MakeVector3(1, 0, 0),
 354                                                                          b3MakeVector3(0, 1, 0),
 355                                                                          b3MakeVector3(0, 0, 1)};
 356         b3Assert(bu_treshold > 1);
 357         if (count > 1)
 358         {
 359                 if (count > bu_treshold)
 360                 {
 361                         const b3DbvtVolume vol = b3Bounds(leaves, count);
 362                         const b3Vector3 org = vol.Center();
 363                         int partition;
 364                         int bestaxis = -1;
 365                         int bestmidp = count;
 366                         int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}};
 367                         int i;
 368                         for (i = 0; i < count; ++i)
 369                         {
 370                                 const b3Vector3 x = leaves[i]->volume.Center() - org;
 371                                 for (int j = 0; j < 3; ++j)
 372                                 {
 373                                         ++splitcount[j][b3Dot(x, axis[j]) > 0 ? 1 : 0];
 374                                 }
 375                         }
 376                         for (i = 0; i < 3; ++i)
 377                         {
 378                                 if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0))
 379                                 {
 380                                         const int midp = (int)b3Fabs(b3Scalar(splitcount[i][0] - splitcount[i][1]));
 381                                         if (midp < bestmidp)
 382                                         {
 383                                                 bestaxis = i;
 384                                                 bestmidp = midp;
 385                                         }
 386                                 }
 387                         }
 388                         if (bestaxis >= 0)
 389                         {
 390                                 partition = b3Split(leaves, count, org, axis[bestaxis]);
 391                                 b3Assert(partition != 0 && partition != count);
 392                         }
 393                         else
 394                         {
 395                                 partition = count / 2 + 1;
 396                         }
 397                         b3DbvtNode* node = b3CreateNode(pdbvt, 0, vol, 0);
 398                         node->childs[0] = b3TopDown(pdbvt, &leaves[0], partition, bu_treshold);
 399                         node->childs[1] = b3TopDown(pdbvt, &leaves[partition], count - partition, bu_treshold);
 400                         node->childs[0]->parent = node;
 401                         node->childs[1]->parent = node;
 402                         return (node);
 403                 }
 404                 else
 405                 {
 406                         b3BottomUp(pdbvt, leaves, count);
 407                         return (leaves[0]);
 408                 }
 409         }
 410         return (leaves[0]);
 411 }
 412
 413 //
 414 static B3_DBVT_INLINE b3DbvtNode* b3Sort(b3DbvtNode* n, b3DbvtNode*& r)
 415 {
 416         b3DbvtNode* p = n->parent;
 417         b3Assert(n->isinternal());
 418         if (p > n)
 419         {
 420                 const int i = b3IndexOf(n);
 421                 const int j = 1 - i;
 422                 b3DbvtNode* s = p->childs[j];
 423                 b3DbvtNode* q = p->parent;
 424                 b3Assert(n == p->childs[i]);
 425                 if (q)
 426                         q->childs[b3IndexOf(p)] = n;
 427                 else
 428                         r = n;
 429                 s->parent = n;
 430                 p->parent = n;
 431                 n->parent = q;
 432                 p->childs[0] = n->childs[0];
 433                 p->childs[1] = n->childs[1];
 434                 n->childs[0]->parent = p;
 435                 n->childs[1]->parent = p;
 436                 n->childs[i] = p;
 437                 n->childs[j] = s;
 438                 b3Swap(p->volume, n->volume);
 439                 return (p);
 440         }
 441         return (n);
 442 }
 443
 444 #if 0
 445 static B3_DBVT_INLINE b3DbvtNode*       walkup(b3DbvtNode* n,int count)
 446 {
 447         while(n&&(count--)) n=n->parent;
 448         return(n);
 449 }
 450 #endif
 451
 452 //
 453 // Api
 454 //
 455
 456 //
 457 b3DynamicBvh::b3DynamicBvh()
 458 {
 459         m_root = 0;
 460         m_free = 0;
 461         m_lkhd = -1;
 462         m_leaves = 0;
 463         m_opath = 0;
 464 }
 465
 466 //
 467 b3DynamicBvh::~b3DynamicBvh()
 468 {
 469         clear();
 470 }
 471
 472 //
 473 void b3DynamicBvh::clear()
 474 {
 475         if (m_root)
 476                 b3RecurseDeleteNode(this, m_root);
 477         b3AlignedFree(m_free);
 478         m_free = 0;
 479         m_lkhd = -1;
 480         m_stkStack.clear();
 481         m_opath = 0;
 482 }
 483
 484 //
 485 void b3DynamicBvh::optimizeBottomUp()
 486 {
 487         if (m_root)
 488         {
 489                 b3NodeArray leaves;
 490                 leaves.reserve(m_leaves);
 491                 b3FetchLeaves(this, m_root, leaves);
 492                 b3BottomUp(this, &leaves[0], leaves.size());
 493                 m_root = leaves[0];
 494         }
 495 }
 496
 497 //
 498 void b3DynamicBvh::optimizeTopDown(int bu_treshold)
 499 {
 500         if (m_root)
 501         {
 502                 b3NodeArray leaves;
 503                 leaves.reserve(m_leaves);
 504                 b3FetchLeaves(this, m_root, leaves);
 505                 m_root = b3TopDown(this, &leaves[0], leaves.size(), bu_treshold);
 506         }
 507 }
 508
 509 //
 510 void b3DynamicBvh::optimizeIncremental(int passes)
 511 {
 512         if (passes < 0) passes = m_leaves;
 513         if (m_root && (passes > 0))
 514         {
 515                 do
 516                 {
 517                         b3DbvtNode* node = m_root;
 518                         unsigned bit = 0;
 519                         while (node->isinternal())
 520                         {
 521                                 node = b3Sort(node, m_root)->childs[(m_opath >> bit) & 1];
 522                                 bit = (bit + 1) & (sizeof(unsigned) * 8 - 1);
 523                         }
 524                         update(node);
 525                         ++m_opath;
 526                 } while (--passes);
 527         }
 528 }
 529
 530 //
 531 b3DbvtNode* b3DynamicBvh::insert(const b3DbvtVolume& volume, void* data)
 532 {
 533         b3DbvtNode* leaf = b3CreateNode(this, 0, volume, data);
 534         b3InsertLeaf(this, m_root, leaf);
 535         ++m_leaves;
 536         return (leaf);
 537 }
 538
 539 //
 540 void b3DynamicBvh::update(b3DbvtNode* leaf, int lookahead)
 541 {
 542         b3DbvtNode* root = b3RemoveLeaf(this, leaf);
 543         if (root)
 544         {
 545                 if (lookahead >= 0)
 546                 {
 547                         for (int i = 0; (i < lookahead) && root->parent; ++i)
 548                         {
 549                                 root = root->parent;
 550                         }
 551                 }
 552                 else
 553                         root = m_root;
 554         }
 555         b3InsertLeaf(this, root, leaf);
 556 }
 557
 558 //
 559 void b3DynamicBvh::update(b3DbvtNode* leaf, b3DbvtVolume& volume)
 560 {
 561         b3DbvtNode* root = b3RemoveLeaf(this, leaf);
 562         if (root)
 563         {
 564                 if (m_lkhd >= 0)
 565                 {
 566                         for (int i = 0; (i < m_lkhd) && root->parent; ++i)
 567                         {
 568                                 root = root->parent;
 569                         }
 570                 }
 571                 else
 572                         root = m_root;
 573         }
 574         leaf->volume = volume;
 575         b3InsertLeaf(this, root, leaf);
 576 }
 577
 578 //
 579 bool b3DynamicBvh::update(b3DbvtNode* leaf, b3DbvtVolume& volume, const b3Vector3& velocity, b3Scalar margin)
 580 {
 581         if (leaf->volume.Contain(volume)) return (false);
 582         volume.Expand(b3MakeVector3(margin, margin, margin));
 583         volume.SignedExpand(velocity);
 584         update(leaf, volume);
 585         return (true);
 586 }
 587
 588 //
 589 bool b3DynamicBvh::update(b3DbvtNode* leaf, b3DbvtVolume& volume, const b3Vector3& velocity)
 590 {
 591         if (leaf->volume.Contain(volume)) return (false);
 592         volume.SignedExpand(velocity);
 593         update(leaf, volume);
 594         return (true);
 595 }
 596
 597 //
 598 bool b3DynamicBvh::update(b3DbvtNode* leaf, b3DbvtVolume& volume, b3Scalar margin)
 599 {
 600         if (leaf->volume.Contain(volume)) return (false);
 601         volume.Expand(b3MakeVector3(margin, margin, margin));
 602         update(leaf, volume);
 603         return (true);
 604 }
 605
 606 //
 607 void b3DynamicBvh::remove(b3DbvtNode* leaf)
 608 {
 609         b3RemoveLeaf(this, leaf);
 610         b3DeleteNode(this, leaf);
 611         --m_leaves;
 612 }
 613
 614 //
 615 void b3DynamicBvh::write(IWriter* iwriter) const
 616 {
 617         b3DbvtNodeEnumerator nodes;
 618         nodes.nodes.reserve(m_leaves * 2);
 619         enumNodes(m_root, nodes);
 620         iwriter->Prepare(m_root, nodes.nodes.size());
 621         for (int i = 0; i < nodes.nodes.size(); ++i)
 622         {
 623                 const b3DbvtNode* n = nodes.nodes[i];
 624                 int p = -1;
 625                 if (n->parent) p = nodes.nodes.findLinearSearch(n->parent);
 626                 if (n->isinternal())
 627                 {
 628                         const int c0 = nodes.nodes.findLinearSearch(n->childs[0]);
 629                         const int c1 = nodes.nodes.findLinearSearch(n->childs[1]);
 630                         iwriter->WriteNode(n, i, p, c0, c1);
 631                 }
 632                 else
 633                 {
 634                         iwriter->WriteLeaf(n, i, p);
 635                 }
 636         }
 637 }
 638
 639 //
 640 void b3DynamicBvh::clone(b3DynamicBvh& dest, IClone* iclone) const
 641 {
 642         dest.clear();
 643         if (m_root != 0)
 644         {
 645                 b3AlignedObjectArray<sStkCLN> stack;
 646                 stack.reserve(m_leaves);
 647                 stack.push_back(sStkCLN(m_root, 0));
 648                 do
 649                 {
 650                         const int i = stack.size() - 1;
 651                         const sStkCLN e = stack[i];
 652                         b3DbvtNode* n = b3CreateNode(&dest, e.parent, e.node->volume, e.node->data);
 653                         stack.pop_back();
 654                         if (e.parent != 0)
 655                                 e.parent->childs[i & 1] = n;
 656                         else
 657                                 dest.m_root = n;
 658                         if (e.node->isinternal())
 659                         {
 660                                 stack.push_back(sStkCLN(e.node->childs[0], n));
 661                                 stack.push_back(sStkCLN(e.node->childs[1], n));
 662                         }
 663                         else
 664                         {
 665                                 iclone->CloneLeaf(n);
 666                         }
 667                 } while (stack.size() > 0);
 668         }
 669 }
 670
 671 //
 672 int b3DynamicBvh::maxdepth(const b3DbvtNode* node)
 673 {
 674         int depth = 0;
 675         if (node) b3GetMaxDepth(node, 1, depth);
 676         return (depth);
 677 }
 678
 679 //
 680 int b3DynamicBvh::countLeaves(const b3DbvtNode* node)
 681 {
 682         if (node->isinternal())
 683                 return (countLeaves(node->childs[0]) + countLeaves(node->childs[1]));
 684         else
 685                 return (1);
 686 }
 687
 688 //
 689 void b3DynamicBvh::extractLeaves(const b3DbvtNode* node, b3AlignedObjectArray<const b3DbvtNode*>& leaves)
 690 {
 691         if (node->isinternal())
 692         {
 693                 extractLeaves(node->childs[0], leaves);
 694                 extractLeaves(node->childs[1], leaves);
 695         }
 696         else
 697         {
 698                 leaves.push_back(node);
 699         }
 700 }
 701
 702 //
 703 #if B3_DBVT_ENABLE_BENCHMARK
 704
 705 #include <stdio.h>
 706 #include <stdlib.h>
 707
 708 /*
 709 q6600,2.4ghz
 710
 711 /Ox /Ob2 /Oi /Ot /I "." /I "..\.." /I "..\..\src" /D "NDEBUG" /D "_LIB" /D "_WINDOWS" /D "_CRT_SECURE_NO_DEPRECATE" /D "_CRT_NONSTDC_NO_DEPRECATE" /D "WIN32"
 712 /GF /FD /MT /GS- /Gy /arch:SSE2 /Zc:wchar_t- /Fp"..\..\out\release8\build\libbulletcollision\libbulletcollision.pch"
 713 /Fo"..\..\out\release8\build\libbulletcollision\\"
 714 /Fd"..\..\out\release8\build\libbulletcollision\bulletcollision.pdb"
 715 /W3 /nologo /c /Wp64 /Zi /errorReport:prompt
 716
 717 Benchmarking dbvt...
 718 World scale: 100.000000
 719 Extents base: 1.000000
 720 Extents range: 4.000000
 721 Leaves: 8192
 722 sizeof(b3DbvtVolume): 32 bytes
 723 sizeof(b3DbvtNode):   44 bytes
 724 [1] b3DbvtVolume intersections: 3499 ms (-1%)
 725 [2] b3DbvtVolume merges: 1934 ms (0%)
 726 [3] b3DynamicBvh::collideTT: 5485 ms (-21%)
 727 [4] b3DynamicBvh::collideTT self: 2814 ms (-20%)
 728 [5] b3DynamicBvh::collideTT xform: 7379 ms (-1%)
 729 [6] b3DynamicBvh::collideTT xform,self: 7270 ms (-2%)
 730 [7] b3DynamicBvh::rayTest: 6314 ms (0%),(332143 r/s)
 731 [8] insert/remove: 2093 ms (0%),(1001983 ir/s)
 732 [9] updates (teleport): 1879 ms (-3%),(1116100 u/s)
 733 [10] updates (jitter): 1244 ms (-4%),(1685813 u/s)
 734 [11] optimize (incremental): 2514 ms (0%),(1668000 o/s)
 735 [12] b3DbvtVolume notequal: 3659 ms (0%)
 736 [13] culling(OCL+fullsort): 2218 ms (0%),(461 t/s)
 737 [14] culling(OCL+qsort): 3688 ms (5%),(2221 t/s)
 738 [15] culling(KDOP+qsort): 1139 ms (-1%),(7192 t/s)
 739 [16] insert/remove batch(256): 5092 ms (0%),(823704 bir/s)
 740 [17] b3DbvtVolume select: 3419 ms (0%)
 741 */
 742
 743 struct b3DbvtBenchmark
 744 {
 745         struct NilPolicy : b3DynamicBvh::ICollide
 746         {
 747                 NilPolicy() : m_pcount(0), m_depth(-B3_INFINITY), m_checksort(true) {}
 748                 void Process(const b3DbvtNode*, const b3DbvtNode*) { ++m_pcount; }
 749                 void Process(const b3DbvtNode*) { ++m_pcount; }
 750                 void Process(const b3DbvtNode*, b3Scalar depth)
 751                 {
 752                         ++m_pcount;
 753                         if (m_checksort)
 754                         {
 755                                 if (depth >= m_depth)
 756                                         m_depth = depth;
 757                                 else
 758                                         printf("wrong depth: %f (should be >= %f)\r\n", depth, m_depth);
 759                         }
 760                 }
 761                 int m_pcount;
 762                 b3Scalar m_depth;
 763                 bool m_checksort;
 764         };
 765         struct P14 : b3DynamicBvh::ICollide
 766         {
 767                 struct Node
 768                 {
 769                         const b3DbvtNode* leaf;
 770                         b3Scalar depth;
 771                 };
 772                 void Process(const b3DbvtNode* leaf, b3Scalar depth)
 773                 {
 774                         Node n;
 775                         n.leaf = leaf;
 776                         n.depth = depth;
 777                 }
 778                 static int sortfnc(const Node& a, const Node& b)
 779                 {
 780                         if (a.depth < b.depth) return (+1);
 781                         if (a.depth > b.depth) return (-1);
 782                         return (0);
 783                 }
 784                 b3AlignedObjectArray<Node> m_nodes;
 785         };
 786         struct P15 : b3DynamicBvh::ICollide
 787         {
 788                 struct Node
 789                 {
 790                         const b3DbvtNode* leaf;
 791                         b3Scalar depth;
 792                 };
 793                 void Process(const b3DbvtNode* leaf)
 794                 {
 795                         Node n;
 796                         n.leaf = leaf;
 797                         n.depth = dot(leaf->volume.Center(), m_axis);
 798                 }
 799                 static int sortfnc(const Node& a, const Node& b)
 800                 {
 801                         if (a.depth < b.depth) return (+1);
 802                         if (a.depth > b.depth) return (-1);
 803                         return (0);
 804                 }
 805                 b3AlignedObjectArray<Node> m_nodes;
 806                 b3Vector3 m_axis;
 807         };
 808         static b3Scalar RandUnit()
 809         {
 810                 return (rand() / (b3Scalar)RAND_MAX);
 811         }
 812         static b3Vector3 RandVector3()
 813         {
 814                 return (b3Vector3(RandUnit(), RandUnit(), RandUnit()));
 815         }
 816         static b3Vector3 RandVector3(b3Scalar cs)
 817         {
 818                 return (RandVector3() * cs - b3Vector3(cs, cs, cs) / 2);
 819         }
 820         static b3DbvtVolume RandVolume(b3Scalar cs, b3Scalar eb, b3Scalar es)
 821         {
 822                 return (b3DbvtVolume::FromCE(RandVector3(cs), b3Vector3(eb, eb, eb) + RandVector3() * es));
 823         }
 824         static b3Transform RandTransform(b3Scalar cs)
 825         {
 826                 b3Transform t;
 827                 t.setOrigin(RandVector3(cs));
 828                 t.setRotation(b3Quaternion(RandUnit() * B3_PI * 2, RandUnit() * B3_PI * 2, RandUnit() * B3_PI * 2).normalized());
 829                 return (t);
 830         }
 831         static void RandTree(b3Scalar cs, b3Scalar eb, b3Scalar es, int leaves, b3DynamicBvh& dbvt)
 832         {
 833                 dbvt.clear();
 834                 for (int i = 0; i < leaves; ++i)
 835                 {
 836                         dbvt.insert(RandVolume(cs, eb, es), 0);
 837                 }
 838         }
 839 };
 840
 841 void b3DynamicBvh::benchmark()
 842 {
 843         static const b3Scalar cfgVolumeCenterScale = 100;
 844         static const b3Scalar cfgVolumeExentsBase = 1;
 845         static const b3Scalar cfgVolumeExentsScale = 4;
 846         static const int cfgLeaves = 8192;
 847         static const bool cfgEnable = true;
 848
 849         //[1] b3DbvtVolume intersections
 850         bool cfgBenchmark1_Enable = cfgEnable;
 851         static const int cfgBenchmark1_Iterations = 8;
 852         static const int cfgBenchmark1_Reference = 3499;
 853         //[2] b3DbvtVolume merges
 854         bool cfgBenchmark2_Enable = cfgEnable;
 855         static const int cfgBenchmark2_Iterations = 4;
 856         static const int cfgBenchmark2_Reference = 1945;
 857         //[3] b3DynamicBvh::collideTT
 858         bool cfgBenchmark3_Enable = cfgEnable;
 859         static const int cfgBenchmark3_Iterations = 512;
 860         static const int cfgBenchmark3_Reference = 5485;
 861         //[4] b3DynamicBvh::collideTT self
 862         bool cfgBenchmark4_Enable = cfgEnable;
 863         static const int cfgBenchmark4_Iterations = 512;
 864         static const int cfgBenchmark4_Reference = 2814;
 865         //[5] b3DynamicBvh::collideTT xform
 866         bool cfgBenchmark5_Enable = cfgEnable;
 867         static const int cfgBenchmark5_Iterations = 512;
 868         static const b3Scalar cfgBenchmark5_OffsetScale = 2;
 869         static const int cfgBenchmark5_Reference = 7379;
 870         //[6] b3DynamicBvh::collideTT xform,self
 871         bool cfgBenchmark6_Enable = cfgEnable;
 872         static const int cfgBenchmark6_Iterations = 512;
 873         static const b3Scalar cfgBenchmark6_OffsetScale = 2;
 874         static const int cfgBenchmark6_Reference = 7270;
 875         //[7] b3DynamicBvh::rayTest
 876         bool cfgBenchmark7_Enable = cfgEnable;
 877         static const int cfgBenchmark7_Passes = 32;
 878         static const int cfgBenchmark7_Iterations = 65536;
 879         static const int cfgBenchmark7_Reference = 6307;
 880         //[8] insert/remove
 881         bool cfgBenchmark8_Enable = cfgEnable;
 882         static const int cfgBenchmark8_Passes = 32;
 883         static const int cfgBenchmark8_Iterations = 65536;
 884         static const int cfgBenchmark8_Reference = 2105;
 885         //[9] updates (teleport)
 886         bool cfgBenchmark9_Enable = cfgEnable;
 887         static const int cfgBenchmark9_Passes = 32;
 888         static const int cfgBenchmark9_Iterations = 65536;
 889         static const int cfgBenchmark9_Reference = 1879;
 890         //[10] updates (jitter)
 891         bool cfgBenchmark10_Enable = cfgEnable;
 892         static const b3Scalar cfgBenchmark10_Scale = cfgVolumeCenterScale / 10000;
 893         static const int cfgBenchmark10_Passes = 32;
 894         static const int cfgBenchmark10_Iterations = 65536;
 895         static const int cfgBenchmark10_Reference = 1244;
 896         //[11] optimize (incremental)
 897         bool cfgBenchmark11_Enable = cfgEnable;
 898         static const int cfgBenchmark11_Passes = 64;
 899         static const int cfgBenchmark11_Iterations = 65536;
 900         static const int cfgBenchmark11_Reference = 2510;
 901         //[12] b3DbvtVolume notequal
 902         bool cfgBenchmark12_Enable = cfgEnable;
 903         static const int cfgBenchmark12_Iterations = 32;
 904         static const int cfgBenchmark12_Reference = 3677;
 905         //[13] culling(OCL+fullsort)
 906         bool cfgBenchmark13_Enable = cfgEnable;
 907         static const int cfgBenchmark13_Iterations = 1024;
 908         static const int cfgBenchmark13_Reference = 2231;
 909         //[14] culling(OCL+qsort)
 910         bool cfgBenchmark14_Enable = cfgEnable;
 911         static const int cfgBenchmark14_Iterations = 8192;
 912         static const int cfgBenchmark14_Reference = 3500;
 913         //[15] culling(KDOP+qsort)
 914         bool cfgBenchmark15_Enable = cfgEnable;
 915         static const int cfgBenchmark15_Iterations = 8192;
 916         static const int cfgBenchmark15_Reference = 1151;
 917         //[16] insert/remove batch
 918         bool cfgBenchmark16_Enable = cfgEnable;
 919         static const int cfgBenchmark16_BatchCount = 256;
 920         static const int cfgBenchmark16_Passes = 16384;
 921         static const int cfgBenchmark16_Reference = 5138;
 922         //[17] select
 923         bool cfgBenchmark17_Enable = cfgEnable;
 924         static const int cfgBenchmark17_Iterations = 4;
 925         static const int cfgBenchmark17_Reference = 3390;
 926
 927         b3Clock wallclock;
 928         printf("Benchmarking dbvt...\r\n");
 929         printf("\tWorld scale: %f\r\n", cfgVolumeCenterScale);
 930         printf("\tExtents base: %f\r\n", cfgVolumeExentsBase);
 931         printf("\tExtents range: %f\r\n", cfgVolumeExentsScale);
 932         printf("\tLeaves: %u\r\n", cfgLeaves);
 933         printf("\tsizeof(b3DbvtVolume): %u bytes\r\n", sizeof(b3DbvtVolume));
 934         printf("\tsizeof(b3DbvtNode):   %u bytes\r\n", sizeof(b3DbvtNode));
 935         if (cfgBenchmark1_Enable)
 936         {  // Benchmark 1
 937                 srand(380843);
 938                 b3AlignedObjectArray<b3DbvtVolume> volumes;
 939                 b3AlignedObjectArray<bool> results;
 940                 volumes.resize(cfgLeaves);
 941                 results.resize(cfgLeaves);
 942                 for (int i = 0; i < cfgLeaves; ++i)
 943                 {
 944                         volumes[i] = b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale);
 945                 }
 946                 printf("[1] b3DbvtVolume intersections: ");
 947                 wallclock.reset();
 948                 for (int i = 0; i < cfgBenchmark1_Iterations; ++i)
 949                 {
 950                         for (int j = 0; j < cfgLeaves; ++j)
 951                         {
 952                                 for (int k = 0; k < cfgLeaves; ++k)
 953                                 {
 954                                         results[k] = Intersect(volumes[j], volumes[k]);
 955                                 }
 956                         }
 957                 }
 958                 const int time = (int)wallclock.getTimeMilliseconds();
 959                 printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark1_Reference) * 100 / time);
 960         }
 961         if (cfgBenchmark2_Enable)
 962         {  // Benchmark 2
 963                 srand(380843);
 964                 b3AlignedObjectArray<b3DbvtVolume> volumes;
 965                 b3AlignedObjectArray<b3DbvtVolume> results;
 966                 volumes.resize(cfgLeaves);
 967                 results.resize(cfgLeaves);
 968                 for (int i = 0; i < cfgLeaves; ++i)
 969                 {
 970                         volumes[i] = b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale);
 971                 }
 972                 printf("[2] b3DbvtVolume merges: ");
 973                 wallclock.reset();
 974                 for (int i = 0; i < cfgBenchmark2_Iterations; ++i)
 975                 {
 976                         for (int j = 0; j < cfgLeaves; ++j)
 977                         {
 978                                 for (int k = 0; k < cfgLeaves; ++k)
 979                                 {
 980                                         Merge(volumes[j], volumes[k], results[k]);
 981                                 }
 982                         }
 983                 }
 984                 const int time = (int)wallclock.getTimeMilliseconds();
 985                 printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark2_Reference) * 100 / time);
 986         }
 987         if (cfgBenchmark3_Enable)
 988         {  // Benchmark 3
 989                 srand(380843);
 990                 b3DynamicBvh dbvt[2];
 991                 b3DbvtBenchmark::NilPolicy policy;
 992                 b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt[0]);
 993                 b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt[1]);
 994                 dbvt[0].optimizeTopDown();
 995                 dbvt[1].optimizeTopDown();
 996                 printf("[3] b3DynamicBvh::collideTT: ");
 997                 wallclock.reset();
 998                 for (int i = 0; i < cfgBenchmark3_Iterations; ++i)
 999                 {
1000                         b3DynamicBvh::collideTT(dbvt[0].m_root, dbvt[1].m_root, policy);
1001                 }
1002                 const int time = (int)wallclock.getTimeMilliseconds();
1003                 printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark3_Reference) * 100 / time);
1004         }
1005         if (cfgBenchmark4_Enable)
1006         {  // Benchmark 4
1007                 srand(380843);
1008                 b3DynamicBvh dbvt;
1009                 b3DbvtBenchmark::NilPolicy policy;
1010                 b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
1011                 dbvt.optimizeTopDown();
1012                 printf("[4] b3DynamicBvh::collideTT self: ");
1013                 wallclock.reset();
1014                 for (int i = 0; i < cfgBenchmark4_Iterations; ++i)
1015                 {
1016                         b3DynamicBvh::collideTT(dbvt.m_root, dbvt.m_root, policy);
1017                 }
1018                 const int time = (int)wallclock.getTimeMilliseconds();
1019                 printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark4_Reference) * 100 / time);
1020         }
1021         if (cfgBenchmark5_Enable)
1022         {  // Benchmark 5
1023                 srand(380843);
1024                 b3DynamicBvh dbvt[2];
1025                 b3AlignedObjectArray<b3Transform> transforms;
1026                 b3DbvtBenchmark::NilPolicy policy;
1027                 transforms.resize(cfgBenchmark5_Iterations);
1028                 for (int i = 0; i < transforms.size(); ++i)
1029                 {
1030                         transforms[i] = b3DbvtBenchmark::RandTransform(cfgVolumeCenterScale * cfgBenchmark5_OffsetScale);
1031                 }
1032                 b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt[0]);
1033                 b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt[1]);
1034                 dbvt[0].optimizeTopDown();
1035                 dbvt[1].optimizeTopDown();
1036                 printf("[5] b3DynamicBvh::collideTT xform: ");
1037                 wallclock.reset();
1038                 for (int i = 0; i < cfgBenchmark5_Iterations; ++i)
1039                 {
1040                         b3DynamicBvh::collideTT(dbvt[0].m_root, dbvt[1].m_root, transforms[i], policy);
1041                 }
1042                 const int time = (int)wallclock.getTimeMilliseconds();
1043                 printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark5_Reference) * 100 / time);
1044         }
1045         if (cfgBenchmark6_Enable)
1046         {  // Benchmark 6
1047                 srand(380843);
1048                 b3DynamicBvh dbvt;
1049                 b3AlignedObjectArray<b3Transform> transforms;
1050                 b3DbvtBenchmark::NilPolicy policy;
1051                 transforms.resize(cfgBenchmark6_Iterations);
1052                 for (int i = 0; i < transforms.size(); ++i)
1053                 {
1054                         transforms[i] = b3DbvtBenchmark::RandTransform(cfgVolumeCenterScale * cfgBenchmark6_OffsetScale);
1055                 }
1056                 b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
1057                 dbvt.optimizeTopDown();
1058                 printf("[6] b3DynamicBvh::collideTT xform,self: ");
1059                 wallclock.reset();
1060                 for (int i = 0; i < cfgBenchmark6_Iterations; ++i)
1061                 {
1062                         b3DynamicBvh::collideTT(dbvt.m_root, dbvt.m_root, transforms[i], policy);
1063                 }
1064                 const int time = (int)wallclock.getTimeMilliseconds();
1065                 printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark6_Reference) * 100 / time);
1066         }
1067         if (cfgBenchmark7_Enable)
1068         {  // Benchmark 7
1069                 srand(380843);
1070                 b3DynamicBvh dbvt;
1071                 b3AlignedObjectArray<b3Vector3> rayorg;
1072                 b3AlignedObjectArray<b3Vector3> raydir;
1073                 b3DbvtBenchmark::NilPolicy policy;
1074                 rayorg.resize(cfgBenchmark7_Iterations);
1075                 raydir.resize(cfgBenchmark7_Iterations);
1076                 for (int i = 0; i < rayorg.size(); ++i)
1077                 {
1078                         rayorg[i] = b3DbvtBenchmark::RandVector3(cfgVolumeCenterScale * 2);
1079                         raydir[i] = b3DbvtBenchmark::RandVector3(cfgVolumeCenterScale * 2);
1080                 }
1081                 b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
1082                 dbvt.optimizeTopDown();
1083                 printf("[7] b3DynamicBvh::rayTest: ");
1084                 wallclock.reset();
1085                 for (int i = 0; i < cfgBenchmark7_Passes; ++i)
1086                 {
1087                         for (int j = 0; j < cfgBenchmark7_Iterations; ++j)
1088                         {
1089                                 b3DynamicBvh::rayTest(dbvt.m_root, rayorg[j], rayorg[j] + raydir[j], policy);
1090                         }
1091                 }
1092                 const int time = (int)wallclock.getTimeMilliseconds();
1093                 unsigned rays = cfgBenchmark7_Passes * cfgBenchmark7_Iterations;
1094                 printf("%u ms (%i%%),(%u r/s)\r\n", time, (time - cfgBenchmark7_Reference) * 100 / time, (rays * 1000) / time);
1095         }
1096         if (cfgBenchmark8_Enable)
1097         {  // Benchmark 8
1098                 srand(380843);
1099                 b3DynamicBvh dbvt;
1100                 b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
1101                 dbvt.optimizeTopDown();
1102                 printf("[8] insert/remove: ");
1103                 wallclock.reset();
1104                 for (int i = 0; i < cfgBenchmark8_Passes; ++i)
1105                 {
1106                         for (int j = 0; j < cfgBenchmark8_Iterations; ++j)
1107                         {
1108                                 dbvt.remove(dbvt.insert(b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale), 0));
1109                         }
1110                 }
1111                 const int time = (int)wallclock.getTimeMilliseconds();
1112                 const int ir = cfgBenchmark8_Passes * cfgBenchmark8_Iterations;
1113                 printf("%u ms (%i%%),(%u ir/s)\r\n", time, (time - cfgBenchmark8_Reference) * 100 / time, ir * 1000 / time);
1114         }
1115         if (cfgBenchmark9_Enable)
1116         {  // Benchmark 9
1117                 srand(380843);
1118                 b3DynamicBvh dbvt;
1119                 b3AlignedObjectArray<const b3DbvtNode*> leaves;
1120                 b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
1121                 dbvt.optimizeTopDown();
1122                 dbvt.extractLeaves(dbvt.m_root, leaves);
1123                 printf("[9] updates (teleport): ");
1124                 wallclock.reset();
1125                 for (int i = 0; i < cfgBenchmark9_Passes; ++i)
1126                 {
1127                         for (int j = 0; j < cfgBenchmark9_Iterations; ++j)
1128                         {
1129                                 dbvt.update(const_cast<b3DbvtNode*>(leaves[rand() % cfgLeaves]),
1130                                                         b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale));
1131                         }
1132                 }
1133                 const int time = (int)wallclock.getTimeMilliseconds();
1134                 const int up = cfgBenchmark9_Passes * cfgBenchmark9_Iterations;
1135                 printf("%u ms (%i%%),(%u u/s)\r\n", time, (time - cfgBenchmark9_Reference) * 100 / time, up * 1000 / time);
1136         }
1137         if (cfgBenchmark10_Enable)
1138         {  // Benchmark 10
1139                 srand(380843);
1140                 b3DynamicBvh dbvt;
1141                 b3AlignedObjectArray<const b3DbvtNode*> leaves;
1142                 b3AlignedObjectArray<b3Vector3> vectors;
1143                 vectors.resize(cfgBenchmark10_Iterations);
1144                 for (int i = 0; i < vectors.size(); ++i)
1145                 {
1146                         vectors[i] = (b3DbvtBenchmark::RandVector3() * 2 - b3Vector3(1, 1, 1)) * cfgBenchmark10_Scale;
1147                 }
1148                 b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
1149                 dbvt.optimizeTopDown();
1150                 dbvt.extractLeaves(dbvt.m_root, leaves);
1151                 printf("[10] updates (jitter): ");
1152                 wallclock.reset();
1153
1154                 for (int i = 0; i < cfgBenchmark10_Passes; ++i)
1155                 {
1156                         for (int j = 0; j < cfgBenchmark10_Iterations; ++j)
1157                         {
1158                                 const b3Vector3& d = vectors[j];
1159                                 b3DbvtNode* l = const_cast<b3DbvtNode*>(leaves[rand() % cfgLeaves]);
1160                                 b3DbvtVolume v = b3DbvtVolume::FromMM(l->volume.Mins() + d, l->volume.Maxs() + d);
1161                                 dbvt.update(l, v);
1162                         }
1163                 }
1164                 const int time = (int)wallclock.getTimeMilliseconds();
1165                 const int up = cfgBenchmark10_Passes * cfgBenchmark10_Iterations;
1166                 printf("%u ms (%i%%),(%u u/s)\r\n", time, (time - cfgBenchmark10_Reference) * 100 / time, up * 1000 / time);
1167         }
1168         if (cfgBenchmark11_Enable)
1169         {  // Benchmark 11
1170                 srand(380843);
1171                 b3DynamicBvh dbvt;
1172                 b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
1173                 dbvt.optimizeTopDown();
1174                 printf("[11] optimize (incremental): ");
1175                 wallclock.reset();
1176                 for (int i = 0; i < cfgBenchmark11_Passes; ++i)
1177                 {
1178                         dbvt.optimizeIncremental(cfgBenchmark11_Iterations);
1179                 }
1180                 const int time = (int)wallclock.getTimeMilliseconds();
1181                 const int op = cfgBenchmark11_Passes * cfgBenchmark11_Iterations;
1182                 printf("%u ms (%i%%),(%u o/s)\r\n", time, (time - cfgBenchmark11_Reference) * 100 / time, op / time * 1000);
1183         }
1184         if (cfgBenchmark12_Enable)
1185         {  // Benchmark 12
1186                 srand(380843);
1187                 b3AlignedObjectArray<b3DbvtVolume> volumes;
1188                 b3AlignedObjectArray<bool> results;
1189                 volumes.resize(cfgLeaves);
1190                 results.resize(cfgLeaves);
1191                 for (int i = 0; i < cfgLeaves; ++i)
1192                 {
1193                         volumes[i] = b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale);
1194                 }
1195                 printf("[12] b3DbvtVolume notequal: ");
1196                 wallclock.reset();
1197                 for (int i = 0; i < cfgBenchmark12_Iterations; ++i)
1198                 {
1199                         for (int j = 0; j < cfgLeaves; ++j)
1200                         {
1201                                 for (int k = 0; k < cfgLeaves; ++k)
1202                                 {
1203                                         results[k] = NotEqual(volumes[j], volumes[k]);
1204                                 }
1205                         }
1206                 }
1207                 const int time = (int)wallclock.getTimeMilliseconds();
1208                 printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark12_Reference) * 100 / time);
1209         }
1210         if (cfgBenchmark13_Enable)
1211         {  // Benchmark 13
1212                 srand(380843);
1213                 b3DynamicBvh dbvt;
1214                 b3AlignedObjectArray<b3Vector3> vectors;
1215                 b3DbvtBenchmark::NilPolicy policy;
1216                 vectors.resize(cfgBenchmark13_Iterations);
1217                 for (int i = 0; i < vectors.size(); ++i)
1218                 {
1219                         vectors[i] = (b3DbvtBenchmark::RandVector3() * 2 - b3Vector3(1, 1, 1)).normalized();
1220                 }
1221                 b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
1222                 dbvt.optimizeTopDown();
1223                 printf("[13] culling(OCL+fullsort): ");
1224                 wallclock.reset();
1225                 for (int i = 0; i < cfgBenchmark13_Iterations; ++i)
1226                 {
1227                         static const b3Scalar offset = 0;
1228                         policy.m_depth = -B3_INFINITY;
1229                         dbvt.collideOCL(dbvt.m_root, &vectors[i], &offset, vectors[i], 1, policy);
1230                 }
1231                 const int time = (int)wallclock.getTimeMilliseconds();
1232                 const int t = cfgBenchmark13_Iterations;
1233                 printf("%u ms (%i%%),(%u t/s)\r\n", time, (time - cfgBenchmark13_Reference) * 100 / time, (t * 1000) / time);
1234         }
1235         if (cfgBenchmark14_Enable)
1236         {  // Benchmark 14
1237                 srand(380843);
1238                 b3DynamicBvh dbvt;
1239                 b3AlignedObjectArray<b3Vector3> vectors;
1240                 b3DbvtBenchmark::P14 policy;
1241                 vectors.resize(cfgBenchmark14_Iterations);
1242                 for (int i = 0; i < vectors.size(); ++i)
1243                 {
1244                         vectors[i] = (b3DbvtBenchmark::RandVector3() * 2 - b3Vector3(1, 1, 1)).normalized();
1245                 }
1246                 b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
1247                 dbvt.optimizeTopDown();
1248                 policy.m_nodes.reserve(cfgLeaves);
1249                 printf("[14] culling(OCL+qsort): ");
1250                 wallclock.reset();
1251                 for (int i = 0; i < cfgBenchmark14_Iterations; ++i)
1252                 {
1253                         static const b3Scalar offset = 0;
1254                         policy.m_nodes.resize(0);
1255                         dbvt.collideOCL(dbvt.m_root, &vectors[i], &offset, vectors[i], 1, policy, false);
1256                         policy.m_nodes.quickSort(b3DbvtBenchmark::P14::sortfnc);
1257                 }
1258                 const int time = (int)wallclock.getTimeMilliseconds();
1259                 const int t = cfgBenchmark14_Iterations;
1260                 printf("%u ms (%i%%),(%u t/s)\r\n", time, (time - cfgBenchmark14_Reference) * 100 / time, (t * 1000) / time);
1261         }
1262         if (cfgBenchmark15_Enable)
1263         {  // Benchmark 15
1264                 srand(380843);
1265                 b3DynamicBvh dbvt;
1266                 b3AlignedObjectArray<b3Vector3> vectors;
1267                 b3DbvtBenchmark::P15 policy;
1268                 vectors.resize(cfgBenchmark15_Iterations);
1269                 for (int i = 0; i < vectors.size(); ++i)
1270                 {
1271                         vectors[i] = (b3DbvtBenchmark::RandVector3() * 2 - b3Vector3(1, 1, 1)).normalized();
1272                 }
1273                 b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
1274                 dbvt.optimizeTopDown();
1275                 policy.m_nodes.reserve(cfgLeaves);
1276                 printf("[15] culling(KDOP+qsort): ");
1277                 wallclock.reset();
1278                 for (int i = 0; i < cfgBenchmark15_Iterations; ++i)
1279                 {
1280                         static const b3Scalar offset = 0;
1281                         policy.m_nodes.resize(0);
1282                         policy.m_axis = vectors[i];
1283                         dbvt.collideKDOP(dbvt.m_root, &vectors[i], &offset, 1, policy);
1284                         policy.m_nodes.quickSort(b3DbvtBenchmark::P15::sortfnc);
1285                 }
1286                 const int time = (int)wallclock.getTimeMilliseconds();
1287                 const int t = cfgBenchmark15_Iterations;
1288                 printf("%u ms (%i%%),(%u t/s)\r\n", time, (time - cfgBenchmark15_Reference) * 100 / time, (t * 1000) / time);
1289         }
1290         if (cfgBenchmark16_Enable)
1291         {  // Benchmark 16
1292                 srand(380843);
1293                 b3DynamicBvh dbvt;
1294                 b3AlignedObjectArray<b3DbvtNode*> batch;
1295                 b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
1296                 dbvt.optimizeTopDown();
1297                 batch.reserve(cfgBenchmark16_BatchCount);
1298                 printf("[16] insert/remove batch(%u): ", cfgBenchmark16_BatchCount);
1299                 wallclock.reset();
1300                 for (int i = 0; i < cfgBenchmark16_Passes; ++i)
1301                 {
1302                         for (int j = 0; j < cfgBenchmark16_BatchCount; ++j)
1303                         {
1304                                 batch.push_back(dbvt.insert(b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale), 0));
1305                         }
1306                         for (int j = 0; j < cfgBenchmark16_BatchCount; ++j)
1307                         {
1308                                 dbvt.remove(batch[j]);
1309                         }
1310                         batch.resize(0);
1311                 }
1312                 const int time = (int)wallclock.getTimeMilliseconds();
1313                 const int ir = cfgBenchmark16_Passes * cfgBenchmark16_BatchCount;
1314                 printf("%u ms (%i%%),(%u bir/s)\r\n", time, (time - cfgBenchmark16_Reference) * 100 / time, int(ir * 1000.0 / time));
1315         }
1316         if (cfgBenchmark17_Enable)
1317         {  // Benchmark 17
1318                 srand(380843);
1319                 b3AlignedObjectArray<b3DbvtVolume> volumes;
1320                 b3AlignedObjectArray<int> results;
1321                 b3AlignedObjectArray<int> indices;
1322                 volumes.resize(cfgLeaves);
1323                 results.resize(cfgLeaves);
1324                 indices.resize(cfgLeaves);
1325                 for (int i = 0; i < cfgLeaves; ++i)
1326                 {
1327                         indices[i] = i;
1328                         volumes[i] = b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale);
1329                 }
1330                 for (int i = 0; i < cfgLeaves; ++i)
1331                 {
1332                         b3Swap(indices[i], indices[rand() % cfgLeaves]);
1333                 }
1334                 printf("[17] b3DbvtVolume select: ");
1335                 wallclock.reset();
1336                 for (int i = 0; i < cfgBenchmark17_Iterations; ++i)
1337                 {
1338                         for (int j = 0; j < cfgLeaves; ++j)
1339                         {
1340                                 for (int k = 0; k < cfgLeaves; ++k)
1341                                 {
1342                                         const int idx = indices[k];
1343                                         results[idx] = Select(volumes[idx], volumes[j], volumes[k]);
1344                                 }
1345                         }
1346                 }
1347                 const int time = (int)wallclock.getTimeMilliseconds();
1348                 printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark17_Reference) * 100 / time);
1349         }
1350         printf("\r\n\r\n");
1351 }
1352 #endif