source/blender/blenlib/intern/BLI_kdopbvh.c

   1 /*
   2  * ***** BEGIN GPL LICENSE BLOCK *****
   3  *
   4  * This program is free software; you can redistribute it and/or
   5  * modify it under the terms of the GNU General Public License
   6  * as published by the Free Software Foundation; either version 2
   7  * of the License, or (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program; if not, write to the Free Software Foundation,
  16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
  17  *
  18  * The Original Code is Copyright (C) 2006 by NaN Holding BV.
  19  * All rights reserved.
  20  *
  21  * The Original Code is: all of this file.
  22  *
  23  * Contributor(s): Daniel Genrich, Andre Pinto
  24  *
  25  * ***** END GPL LICENSE BLOCK *****
  26  */
  27
  28 /** \file blender/blenlib/intern/BLI_kdopbvh.c
  29  *  \ingroup bli
  30  */
  31
  32
  33 #include <assert.h>
  34
  35 #include "MEM_guardedalloc.h"
  36
  37 #include "BLI_utildefines.h"
  38
  39
  40
  41 #include "BLI_kdopbvh.h"
  42 #include "BLI_math.h"
  43
  44 #ifdef _OPENMP
  45 #include <omp.h>
  46 #endif
  47
  48
  49
  50 #define MAX_TREETYPE 32
  51 #define DEFAULT_FIND_NEAREST_HEAP_SIZE 1024
  52
  53 typedef struct BVHNode
  54 {
  55         struct BVHNode **children;
  56         struct BVHNode *parent; // some user defined traversed need that
  57         struct BVHNode *skip[2];
  58         float *bv;              // Bounding volume of all nodes, max 13 axis
  59         int index;              // face, edge, vertex index
  60         char totnode;   // how many nodes are used, used for speedup
  61         char main_axis; // Axis used to split this node
  62 } BVHNode;
  63
  64 struct BVHTree
  65 {
  66         BVHNode **nodes;
  67         BVHNode *nodearray; /* pre-alloc branch nodes */
  68         BVHNode **nodechild;    // pre-alloc childs for nodes
  69         float   *nodebv;                // pre-alloc bounding-volumes for nodes
  70         float   epsilon; /* epslion is used for inflation of the k-dop     */
  71         int     totleaf; // leafs
  72         int     totbranch;
  73         char    tree_type; // type of tree (4 => quadtree)
  74         char    axis; // kdop type (6 => OBB, 7 => AABB, ...)
  75         char    start_axis, stop_axis; // KDOP_AXES array indices according to axis
  76 };
  77
  78 typedef struct BVHOverlapData
  79 {
  80         BVHTree *tree1, *tree2;
  81         BVHTreeOverlap *overlap;
  82         int i, max_overlap; /* i is number of overlaps */
  83         int start_axis, stop_axis;
  84 } BVHOverlapData;
  85
  86 typedef struct BVHNearestData
  87 {
  88         BVHTree *tree;
  89         const float     *co;
  90         BVHTree_NearestPointCallback callback;
  91         void    *userdata;
  92         float proj[13];                 //coordinates projection over axis
  93         BVHTreeNearest nearest;
  94
  95 } BVHNearestData;
  96
  97 typedef struct BVHRayCastData
  98 {
  99         BVHTree *tree;
 100
 101         BVHTree_RayCastCallback callback;
 102         void    *userdata;
 103
 104
 105         BVHTreeRay    ray;
 106         float ray_dot_axis[13];
 107         float idot_axis[13];
 108         int index[6];
 109
 110         BVHTreeRayHit hit;
 111 } BVHRayCastData;
 112 ////////////////////////////////////////m
 113
 114
 115 ////////////////////////////////////////////////////////////////////////
 116 // Bounding Volume Hierarchy Definition
 117 //
 118 // Notes: From OBB until 26-DOP --> all bounding volumes possible, just choose type below
 119 // Notes: You have to choose the type at compile time ITM
 120 // Notes: You can choose the tree type --> binary, quad, octree, choose below
 121 ////////////////////////////////////////////////////////////////////////
 122
 123 static float KDOP_AXES[13][3] =
 124 { {1.0, 0, 0}, {0, 1.0, 0}, {0, 0, 1.0}, {1.0, 1.0, 1.0}, {1.0, -1.0, 1.0}, {1.0, 1.0, -1.0},
 125 {1.0, -1.0, -1.0}, {1.0, 1.0, 0}, {1.0, 0, 1.0}, {0, 1.0, 1.0}, {1.0, -1.0, 0}, {1.0, 0, -1.0},
 126 {0, 1.0, -1.0}
 127 };
 128
 129 /*
 130  * Generic push and pop heap
 131  */
 132 #define PUSH_HEAP_BODY(HEAP_TYPE, PRIORITY, heap, heap_size)    \
 133 {                                                                                                       \
 134         HEAP_TYPE element = heap[heap_size-1];                  \
 135         int child = heap_size-1;                                                \
 136         while (child != 0)                                                              \
 137         {                                                                                               \
 138                 int parent = (child-1) / 2;                                     \
 139                 if (PRIORITY(element, heap[parent]))                    \
 140                 {                                                                                       \
 141                         heap[child] = heap[parent];                             \
 142                         child = parent;                                                 \
 143                 }                                                                                       \
 144                 else break;                                                                     \
 145         }                                                                                               \
 146         heap[child] = element;                                                  \
 147 }
 148
 149 #define POP_HEAP_BODY(HEAP_TYPE, PRIORITY, heap, heap_size)     \
 150 {                                                                                                       \
 151         HEAP_TYPE element = heap[heap_size-1];                  \
 152         int parent = 0;                                                                 \
 153         while (parent < (heap_size-1)/2 )                               \
 154         {                                                                                               \
 155                 int child2 = (parent+1)*2;                                      \
 156                 if (PRIORITY(heap[child2-1], heap[child2]))     \
 157                         --child2;                                                               \
 158                                                                                                         \
 159                 if (PRIORITY(element, heap[child2]))                    \
 160                         break;                                                                  \
 161                                                                                                         \
 162                 heap[parent] = heap[child2];                            \
 163                 parent = child2;                                                        \
 164         }                                                                                               \
 165         heap[parent] = element;                                                 \
 166 }
 167
 168 #if 0
 169 static int ADJUST_MEMORY(void *local_memblock, void **memblock, int new_size, int *max_size, int size_per_item)
 170 {
 171         int   new_max_size = *max_size * 2;
 172         void *new_memblock = NULL;
 173
 174         if (new_size <= *max_size)
 175                 return TRUE;
 176
 177         if (*memblock == local_memblock)
 178         {
 179                 new_memblock = malloc( size_per_item * new_max_size );
 180                 memcpy(new_memblock, *memblock, size_per_item * *max_size);
 181         }
 182         else
 183                 new_memblock = realloc(*memblock, size_per_item * new_max_size );
 184
 185         if (new_memblock)
 186         {
 187                 *memblock = new_memblock;
 188                 *max_size = new_max_size;
 189                 return TRUE;
 190         }
 191         else
 192                 return FALSE;
 193 }
 194 #endif
 195
 196 //////////////////////////////////////////////////////////////////////////////////////////////////////
 197 // Introsort
 198 // with permission deriven from the following Java code:
 199 // http://ralphunden.net/content/tutorials/a-guide-to-introsort/
 200 // and he derived it from the SUN STL
 201 //////////////////////////////////////////////////////////////////////////////////////////////////////
 202 //static int size_threshold = 16;
 203 /*
 204  * Common methods for all algorithms
 205  */
 206 #if 0
 207 static int floor_lg(int a)
 208 {
 209         return (int)(floor(log(a)/log(2)));
 210 }
 211 #endif
 212
 213 /*
 214  * Insertion sort algorithm
 215  */
 216 static void bvh_insertionsort(BVHNode **a, int lo, int hi, int axis)
 217 {
 218         int i, j;
 219         BVHNode *t;
 220         for (i=lo; i < hi; i++) {
 221                 j=i;
 222                 t = a[i];
 223                 while ((j!=lo) && (t->bv[axis] < (a[j-1])->bv[axis])) {
 224                         a[j] = a[j-1];
 225                         j--;
 226                 }
 227                 a[j] = t;
 228         }
 229 }
 230
 231 static int bvh_partition(BVHNode **a, int lo, int hi, BVHNode * x, int axis)
 232 {
 233         int i=lo, j=hi;
 234         while (1) {
 235                 while ((a[i])->bv[axis] < x->bv[axis]) i++;
 236                 j--;
 237                 while (x->bv[axis] < (a[j])->bv[axis]) j--;
 238                 if (!(i < j))
 239                         return i;
 240                 SWAP(BVHNode *, a[i], a[j]);
 241                 i++;
 242         }
 243 }
 244
 245 /*
 246  * Heapsort algorithm
 247  */
 248 #if 0
 249 static void bvh_downheap(BVHNode **a, int i, int n, int lo, int axis)
 250 {
 251         BVHNode * d = a[lo+i-1];
 252         int child;
 253         while (i<=n/2)
 254         {
 255                 child = 2*i;
 256                 if ((child < n) && ((a[lo+child-1])->bv[axis] < (a[lo+child])->bv[axis]))
 257                 {
 258                         child++;
 259                 }
 260                 if (!(d->bv[axis] < (a[lo+child-1])->bv[axis])) break;
 261                 a[lo+i-1] = a[lo+child-1];
 262                 i = child;
 263         }
 264         a[lo+i-1] = d;
 265 }
 266
 267 static void bvh_heapsort(BVHNode **a, int lo, int hi, int axis)
 268 {
 269         int n = hi-lo, i;
 270         for (i=n/2; i>=1; i=i-1)
 271         {
 272                 bvh_downheap(a, i, n, lo, axis);
 273         }
 274         for (i=n; i>1; i=i-1)
 275         {
 276                 SWAP(BVHNode*, a[lo], a[lo+i-1]);
 277                 bvh_downheap(a, 1, i-1, lo, axis);
 278         }
 279 }
 280 #endif
 281
 282 static BVHNode *bvh_medianof3(BVHNode **a, int lo, int mid, int hi, int axis) // returns Sortable
 283 {
 284         if ((a[mid])->bv[axis] < (a[lo])->bv[axis]) {
 285                 if ((a[hi])->bv[axis] < (a[mid])->bv[axis])
 286                         return a[mid];
 287                 else {
 288                         if ((a[hi])->bv[axis] < (a[lo])->bv[axis])
 289                                 return a[hi];
 290                         else
 291                                 return a[lo];
 292                 }
 293         }
 294         else {
 295                 if ((a[hi])->bv[axis] < (a[mid])->bv[axis]) {
 296                         if ((a[hi])->bv[axis] < (a[lo])->bv[axis])
 297                                 return a[lo];
 298                         else
 299                                 return a[hi];
 300                 }
 301                 else
 302                         return a[mid];
 303         }
 304 }
 305
 306 #if 0
 307 /*
 308  * Quicksort algorithm modified for Introsort
 309  */
 310 static void bvh_introsort_loop (BVHNode **a, int lo, int hi, int depth_limit, int axis)
 311 {
 312         int p;
 313
 314         while (hi-lo > size_threshold)
 315         {
 316                 if (depth_limit == 0)
 317                 {
 318                         bvh_heapsort(a, lo, hi, axis);
 319                         return;
 320                 }
 321                 depth_limit=depth_limit-1;
 322                 p=bvh_partition(a, lo, hi, bvh_medianof3(a, lo, lo+((hi-lo)/2)+1, hi-1, axis), axis);
 323                 bvh_introsort_loop(a, p, hi, depth_limit, axis);
 324                 hi=p;
 325         }
 326 }
 327
 328 static void sort(BVHNode **a0, int begin, int end, int axis)
 329 {
 330         if (begin < end)
 331         {
 332                 BVHNode **a=a0;
 333                 bvh_introsort_loop(a, begin, end, 2*floor_lg(end-begin), axis);
 334                 bvh_insertionsort(a, begin, end, axis);
 335         }
 336 }
 337
 338 static void sort_along_axis(BVHTree *tree, int start, int end, int axis)
 339 {
 340         sort(tree->nodes, start, end, axis);
 341 }
 342 #endif
 343
 344 //after a call to this function you can expect one of:
 345 //      every node to left of a[n] are smaller or equal to it
 346 //      every node to the right of a[n] are greater or equal to it
 347 static int partition_nth_element(BVHNode **a, int _begin, int _end, int n, int axis)
 348 {
 349         int begin = _begin, end = _end, cut;
 350         while (end-begin > 3) {
 351                 cut = bvh_partition(a, begin, end, bvh_medianof3(a, begin, (begin+end)/2, end-1, axis), axis );
 352                 if (cut <= n)
 353                         begin = cut;
 354                 else
 355                         end = cut;
 356         }
 357         bvh_insertionsort(a, begin, end, axis);
 358
 359         return n;
 360 }
 361
 362 //////////////////////////////////////////////////////////////////////////////////////////////////////
 363 static void build_skip_links(BVHTree *tree, BVHNode *node, BVHNode *left, BVHNode *right)
 364 {
 365         int i;
 366
 367         node->skip[0] = left;
 368         node->skip[1] = right;
 369
 370         for (i = 0; i < node->totnode; i++) {
 371                 if (i+1 < node->totnode)
 372                         build_skip_links(tree, node->children[i], left, node->children[i+1] );
 373                 else
 374                         build_skip_links(tree, node->children[i], left, right );
 375
 376                 left = node->children[i];
 377         }
 378 }
 379
 380 /*
 381  * BVHTree bounding volumes functions
 382  */
 383 static void create_kdop_hull(BVHTree *tree, BVHNode *node, const float *co, int numpoints, int moving)
 384 {
 385         float newminmax;
 386         float *bv = node->bv;
 387         int i, k;
 388
 389         // don't init boudings for the moving case
 390         if (!moving) {
 391                 for (i = tree->start_axis; i < tree->stop_axis; i++) {
 392                         bv[2 * i] = FLT_MAX;
 393                         bv[2 * i + 1] = -FLT_MAX;
 394                 }
 395         }
 396
 397         for (k = 0; k < numpoints; k++) {
 398                 /* for all Axes. */
 399                 for (i = tree->start_axis; i < tree->stop_axis; i++) {
 400                         newminmax = dot_v3v3(&co[k * 3], KDOP_AXES[i]);
 401                         if (newminmax < bv[2 * i])
 402                                 bv[2 * i] = newminmax;
 403                         if (newminmax > bv[(2 * i) + 1])
 404                                 bv[(2 * i) + 1] = newminmax;
 405                 }
 406         }
 407 }
 408
 409 // depends on the fact that the BVH's for each face is already build
 410 static void refit_kdop_hull(BVHTree *tree, BVHNode *node, int start, int end)
 411 {
 412         float newmin, newmax;
 413         int i, j;
 414         float *bv = node->bv;
 415
 416
 417         for (i = tree->start_axis; i < tree->stop_axis; i++) {
 418                 bv[2*i] = FLT_MAX;
 419                 bv[2*i + 1] = -FLT_MAX;
 420         }
 421
 422         for (j = start; j < end; j++) {
 423                 /* for all Axes. */
 424                 for (i = tree->start_axis; i < tree->stop_axis; i++) {
 425                         newmin = tree->nodes[j]->bv[(2 * i)];
 426                         if ((newmin < bv[(2 * i)]))
 427                                 bv[(2 * i)] = newmin;
 428
 429                         newmax = tree->nodes[j]->bv[(2 * i) + 1];
 430                         if ((newmax > bv[(2 * i) + 1]))
 431                                 bv[(2 * i) + 1] = newmax;
 432                 }
 433         }
 434
 435 }
 436
 437 // only supports x,y,z axis in the moment
 438 // but we should use a plain and simple function here for speed sake
 439 static char get_largest_axis(float *bv)
 440 {
 441         float middle_point[3];
 442
 443         middle_point[0] = (bv[1]) - (bv[0]); // x axis
 444         middle_point[1] = (bv[3]) - (bv[2]); // y axis
 445         middle_point[2] = (bv[5]) - (bv[4]); // z axis
 446         if (middle_point[0] > middle_point[1])  {
 447                 if (middle_point[0] > middle_point[2])
 448                         return 1; // max x axis
 449                 else
 450                         return 5; // max z axis
 451         }
 452         else {
 453                 if (middle_point[1] > middle_point[2])
 454                         return 3; // max y axis
 455                 else
 456                         return 5; // max z axis
 457         }
 458 }
 459
 460 // bottom-up update of bvh node BV
 461 // join the children on the parent BV
 462 static void node_join(BVHTree *tree, BVHNode *node)
 463 {
 464         int i, j;
 465
 466         for (i = tree->start_axis; i < tree->stop_axis; i++) {
 467                 node->bv[2*i] = FLT_MAX;
 468                 node->bv[2*i + 1] = -FLT_MAX;
 469         }
 470
 471         for (i = 0; i < tree->tree_type; i++) {
 472                 if (node->children[i])  {
 473                         for (j = tree->start_axis; j < tree->stop_axis; j++) {
 474                                 // update minimum
 475                                 if (node->children[i]->bv[(2 * j)] < node->bv[(2 * j)])
 476                                         node->bv[(2 * j)] = node->children[i]->bv[(2 * j)];
 477
 478                                 // update maximum
 479                                 if (node->children[i]->bv[(2 * j) + 1] > node->bv[(2 * j) + 1])
 480                                         node->bv[(2 * j) + 1] = node->children[i]->bv[(2 * j) + 1];
 481                         }
 482                 }
 483                 else
 484                         break;
 485         }
 486 }
 487
 488 /*
 489  * Debug and information functions
 490  */
 491 #if 0
 492 static void bvhtree_print_tree(BVHTree *tree, BVHNode *node, int depth)
 493 {
 494         int i;
 495         for (i=0; i<depth; i++) printf(" ");
 496         printf(" - %d (%ld): ", node->index, node - tree->nodearray);
 497         for (i=2*tree->start_axis; i<2*tree->stop_axis; i++)
 498                 printf("%.3f ", node->bv[i]);
 499         printf("\n");
 500
 501         for (i=0; i<tree->tree_type; i++)
 502                 if (node->children[i])
 503                         bvhtree_print_tree(tree, node->children[i], depth+1);
 504 }
 505
 506 static void bvhtree_info(BVHTree *tree)
 507 {
 508         printf("BVHTree info\n");
 509         printf("tree_type = %d, axis = %d, epsilon = %f\n", tree->tree_type, tree->axis, tree->epsilon);
 510         printf("nodes = %d, branches = %d, leafs = %d\n", tree->totbranch + tree->totleaf,  tree->totbranch, tree->totleaf);
 511         printf("Memory per node = %ldbytes\n", sizeof(BVHNode) + sizeof(BVHNode*)*tree->tree_type + sizeof(float)*tree->axis);
 512         printf("BV memory = %dbytes\n", MEM_allocN_len(tree->nodebv));
 513
 514         printf("Total memory = %ldbytes\n", sizeof(BVHTree)
 515                 + MEM_allocN_len(tree->nodes)
 516                 + MEM_allocN_len(tree->nodearray)
 517                 + MEM_allocN_len(tree->nodechild)
 518                 + MEM_allocN_len(tree->nodebv)
 519                 );
 520
 521 //      bvhtree_print_tree(tree, tree->nodes[tree->totleaf], 0);
 522 }
 523 #endif
 524
 525 #if 0
 526
 527
 528 static void verify_tree(BVHTree *tree)
 529 {
 530         int i, j, check = 0;
 531
 532         // check the pointer list
 533         for (i = 0; i < tree->totleaf; i++)
 534         {
 535                 if (tree->nodes[i]->parent == NULL)
 536                         printf("Leaf has no parent: %d\n", i);
 537                 else
 538                 {
 539                         for (j = 0; j < tree->tree_type; j++)
 540                         {
 541                                 if (tree->nodes[i]->parent->children[j] == tree->nodes[i])
 542                                         check = 1;
 543                         }
 544                         if (!check)
 545                         {
 546                                 printf("Parent child relationship doesn't match: %d\n", i);
 547                         }
 548                         check = 0;
 549                 }
 550         }
 551
 552         // check the leaf list
 553         for (i = 0; i < tree->totleaf; i++)
 554         {
 555                 if (tree->nodearray[i].parent == NULL)
 556                         printf("Leaf has no parent: %d\n", i);
 557                 else
 558                 {
 559                         for (j = 0; j < tree->tree_type; j++)
 560                         {
 561                                 if (tree->nodearray[i].parent->children[j] == &tree->nodearray[i])
 562                                         check = 1;
 563                         }
 564                         if (!check)
 565                         {
 566                                 printf("Parent child relationship doesn't match: %d\n", i);
 567                         }
 568                         check = 0;
 569                 }
 570         }
 571
 572         printf("branches: %d, leafs: %d, total: %d\n", tree->totbranch, tree->totleaf, tree->totbranch + tree->totleaf);
 573 }
 574 #endif
 575
 576 //Helper data and structures to build a min-leaf generalized implicit tree
 577 //This code can be easily reduced (basicly this is only method to calculate pow(k, n) in O(1).. and stuff like that)
 578 typedef struct BVHBuildHelper
 579 {
 580         int tree_type;                          /* */
 581         int totleafs;                           /* */
 582
 583         int leafs_per_child[32];        /* Min number of leafs that are archievable from a node at depth N */
 584         int branches_on_level[32];      /* Number of nodes at depth N (tree_type^N) */
 585
 586         int remain_leafs;                       /* Number of leafs that are placed on the level that is not 100% filled */
 587
 588 } BVHBuildHelper;
 589
 590 static void build_implicit_tree_helper(BVHTree *tree, BVHBuildHelper *data)
 591 {
 592         int depth = 0;
 593         int remain;
 594         int nnodes;
 595
 596         data->totleafs = tree->totleaf;
 597         data->tree_type= tree->tree_type;
 598
 599         //Calculate the smallest tree_type^n such that tree_type^n >= num_leafs
 600         for (data->leafs_per_child[0] = 1;
 601              data->leafs_per_child[0] <  data->totleafs;
 602              data->leafs_per_child[0] *= data->tree_type)
 603         {
 604                 /* pass */
 605         }
 606
 607         data->branches_on_level[0] = 1;
 608
 609         //We could stop the loop first (but I am lazy to find out when)
 610         for (depth = 1; depth < 32; depth++) {
 611                 data->branches_on_level[depth] = data->branches_on_level[depth-1] * data->tree_type;
 612                 data->leafs_per_child  [depth] = data->leafs_per_child  [depth-1] / data->tree_type;
 613         }
 614
 615         remain = data->totleafs - data->leafs_per_child[1];
 616         nnodes = (remain + data->tree_type - 2) / (data->tree_type - 1);
 617         data->remain_leafs = remain + nnodes;
 618 }
 619
 620 // return the min index of all the leafs archivable with the given branch
 621 static int implicit_leafs_index(BVHBuildHelper *data, int depth, int child_index)
 622 {
 623         int min_leaf_index = child_index * data->leafs_per_child[depth-1];
 624         if (min_leaf_index <= data->remain_leafs)
 625                 return min_leaf_index;
 626         else if (data->leafs_per_child[depth])
 627                 return data->totleafs - (data->branches_on_level[depth-1] - child_index) * data->leafs_per_child[depth];
 628         else
 629                 return data->remain_leafs;
 630 }
 631
 632 /**
 633  * Generalized implicit tree build
 634  *
 635  * An implicit tree is a tree where its structure is implied, thus there is no need to store child pointers or indexs.
 636  * Its possible to find the position of the child or the parent with simple maths (multiplication and adittion). This type
 637  * of tree is for example used on heaps.. where node N has its childs at indexs N*2 and N*2+1.
 638  *
 639  * Although in this case the tree type is general.. and not know until runtime.
 640  * tree_type stands for the maximum number of childs that a tree node can have.
 641  * All tree types >= 2 are supported.
 642  *
 643  * Advantages of the used trees include:
 644  *  - No need to store child/parent relations (they are implicit);
 645  *  - Any node child always has an index greater than the parent;
 646  *  - Brother nodes are sequential in memory;
 647  *
 648  *
 649  * Some math relations derived for general implicit trees:
 650  *
 651  *   K = tree_type, ( 2 <= K )
 652  *   ROOT = 1
 653  *   N child of node A = A * K + (2 - K) + N, (0 <= N < K)
 654  *
 655  * Util methods:
 656  *   TODO...
 657  *    (looping elements, knowing if its a leaf or not.. etc...)
 658  */
 659
 660 // This functions returns the number of branches needed to have the requested number of leafs.
 661 static int implicit_needed_branches(int tree_type, int leafs)
 662 {
 663         return MAX2(1, (leafs + tree_type - 3) / (tree_type-1) );
 664 }
 665
 666 /*
 667  * This function handles the problem of "sorting" the leafs (along the split_axis).
 668  *
 669  * It arranges the elements in the given partitions such that:
 670  *  - any element in partition N is less or equal to any element in partition N+1.
 671  *  - if all elements are different all partition will get the same subset of elements
 672  *    as if the array was sorted.
 673  *
 674  * partition P is described as the elements in the range ( nth[P], nth[P+1] ]
 675  *
 676  * TODO: This can be optimized a bit by doing a specialized nth_element instead of K nth_elements
 677  */
 678 static void split_leafs(BVHNode **leafs_array, int *nth, int partitions, int split_axis)
 679 {
 680         int i;
 681         for (i=0; i < partitions-1; i++) {
 682                 if (nth[i] >= nth[partitions])
 683                         break;
 684
 685                 partition_nth_element(leafs_array, nth[i], nth[partitions], nth[i+1], split_axis);
 686         }
 687 }
 688
 689 /*
 690  * This functions builds an optimal implicit tree from the given leafs.
 691  * Where optimal stands for:
 692  *  - The resulting tree will have the smallest number of branches;
 693  *  - At most only one branch will have NULL childs;
 694  *  - All leafs will be stored at level N or N+1.
 695  *
 696  * This function creates an implicit tree on branches_array, the leafs are given on the leafs_array.
 697  *
 698  * The tree is built per depth levels. First branches at depth 1.. then branches at depth 2.. etc..
 699  * The reason is that we can build level N+1 from level N without any data dependencies.. thus it allows
 700  * to use multithread building.
 701  *
 702  * To archieve this is necessary to find how much leafs are accessible from a certain branch, BVHBuildHelper
 703  * implicit_needed_branches and implicit_leafs_index are auxiliary functions to solve that "optimal-split".
 704  */
 705 static void non_recursive_bvh_div_nodes(BVHTree *tree, BVHNode *branches_array, BVHNode **leafs_array, int num_leafs)
 706 {
 707         int i;
 708
 709         const int tree_type   = tree->tree_type;
 710         const int tree_offset = 2 - tree->tree_type; //this value is 0 (on binary trees) and negative on the others
 711         const int num_branches= implicit_needed_branches(tree_type, num_leafs);
 712
 713         BVHBuildHelper data;
 714         int depth;
 715
 716         // set parent from root node to NULL
 717         BVHNode *tmp = branches_array+0;
 718         tmp->parent = NULL;
 719
 720         //Most of bvhtree code relies on 1-leaf trees having at least one branch
 721         //We handle that special case here
 722         if (num_leafs == 1) {
 723                 BVHNode *root = branches_array+0;
 724                 refit_kdop_hull(tree, root, 0, num_leafs);
 725                 root->main_axis = get_largest_axis(root->bv) / 2;
 726                 root->totnode = 1;
 727                 root->children[0] = leafs_array[0];
 728                 root->children[0]->parent = root;
 729                 return;
 730         }
 731
 732         branches_array--;       //Implicit trees use 1-based indexs
 733
 734         build_implicit_tree_helper(tree, &data);
 735
 736         //Loop tree levels (log N) loops
 737         for (i=1, depth = 1; i <= num_branches; i = i*tree_type + tree_offset, depth++) {
 738                 const int first_of_next_level = i*tree_type + tree_offset;
 739                 const int  end_j = MIN2(first_of_next_level, num_branches + 1); //index of last branch on this level
 740                 int j;
 741
 742                 //Loop all branches on this level
 743 #pragma omp parallel for private(j) schedule(static)
 744                 for (j = i; j < end_j; j++) {
 745                         int k;
 746                         const int parent_level_index= j-i;
 747                         BVHNode* parent = branches_array + j;
 748                         int nth_positions[ MAX_TREETYPE + 1];
 749                         char split_axis;
 750
 751                         int parent_leafs_begin = implicit_leafs_index(&data, depth, parent_level_index);
 752                         int parent_leafs_end   = implicit_leafs_index(&data, depth, parent_level_index+1);
 753
 754                         //This calculates the bounding box of this branch
 755                         //and chooses the largest axis as the axis to divide leafs
 756                         refit_kdop_hull(tree, parent, parent_leafs_begin, parent_leafs_end);
 757                         split_axis = get_largest_axis(parent->bv);
 758
 759                         //Save split axis (this can be used on raytracing to speedup the query time)
 760                         parent->main_axis = split_axis / 2;
 761
 762                         //Split the childs along the split_axis, note: its not needed to sort the whole leafs array
 763                         //Only to assure that the elements are partioned on a way that each child takes the elements
 764                         //it would take in case the whole array was sorted.
 765                         //Split_leafs takes care of that "sort" problem.
 766                         nth_positions[        0] = parent_leafs_begin;
 767                         nth_positions[tree_type] = parent_leafs_end;
 768                         for (k = 1; k < tree_type; k++) {
 769                                 int child_index = j * tree_type + tree_offset + k;
 770                                 int child_level_index = child_index - first_of_next_level; //child level index
 771                                 nth_positions[k] = implicit_leafs_index(&data, depth+1, child_level_index);
 772                         }
 773
 774                         split_leafs(leafs_array, nth_positions, tree_type, split_axis);
 775
 776
 777                         //Setup children and totnode counters
 778                         //Not really needed but currently most of BVH code relies on having an explicit children structure
 779                         for (k = 0; k < tree_type; k++) {
 780                                 int child_index = j * tree_type + tree_offset + k;
 781                                 int child_level_index = child_index - first_of_next_level; //child level index
 782
 783                                 int child_leafs_begin = implicit_leafs_index(&data, depth+1, child_level_index);
 784                                 int child_leafs_end   = implicit_leafs_index(&data, depth+1, child_level_index+1);
 785
 786                                 if (child_leafs_end - child_leafs_begin > 1) {
 787                                         parent->children[k] = branches_array + child_index;
 788                                         parent->children[k]->parent = parent;
 789                                 }
 790                                 else if (child_leafs_end - child_leafs_begin == 1) {
 791                                         parent->children[k] = leafs_array[ child_leafs_begin ];
 792                                         parent->children[k]->parent = parent;
 793                                 }
 794                                 else {
 795                                         break;
 796                                 }
 797
 798                                 parent->totnode = k+1;
 799                         }
 800                 }
 801         }
 802 }
 803
 804
 805 /*
 806  * BLI_bvhtree api
 807  */
 808 BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
 809 {
 810         BVHTree *tree;
 811         int numnodes, i;
 812
 813         // theres not support for trees below binary-trees :P
 814         if (tree_type < 2)
 815                 return NULL;
 816
 817         if (tree_type > MAX_TREETYPE)
 818                 return NULL;
 819
 820         tree = (BVHTree *)MEM_callocN(sizeof(BVHTree), "BVHTree");
 821
 822         //tree epsilon must be >= FLT_EPSILON
 823         //so that tangent rays can still hit a bounding volume..
 824         //this bug would show up when casting a ray aligned with a kdop-axis and with an edge of 2 faces
 825         epsilon = MAX2(FLT_EPSILON, epsilon);
 826
 827         if (tree) {
 828                 tree->epsilon = epsilon;
 829                 tree->tree_type = tree_type;
 830                 tree->axis = axis;
 831
 832                 if (axis == 26) {
 833                         tree->start_axis = 0;
 834                         tree->stop_axis = 13;
 835                 }
 836                 else if (axis == 18) {
 837                         tree->start_axis = 7;
 838                         tree->stop_axis = 13;
 839                 }
 840                 else if (axis == 14) {
 841                         tree->start_axis = 0;
 842                         tree->stop_axis = 7;
 843                 }
 844                 else if (axis == 8) { /* AABB */
 845                         tree->start_axis = 0;
 846                         tree->stop_axis = 4;
 847                 }
 848                 else if (axis == 6) { /* OBB */
 849                         tree->start_axis = 0;
 850                         tree->stop_axis = 3;
 851                 }
 852                 else {
 853                         MEM_freeN(tree);
 854                         return NULL;
 855                 }
 856
 857
 858                 //Allocate arrays
 859                 numnodes = maxsize + implicit_needed_branches(tree_type, maxsize) + tree_type;
 860
 861                 tree->nodes = (BVHNode **)MEM_callocN(sizeof(BVHNode *)*numnodes, "BVHNodes");
 862
 863                 if (!tree->nodes) {
 864                         MEM_freeN(tree);
 865                         return NULL;
 866                 }
 867
 868                 tree->nodebv = (float*)MEM_callocN(sizeof(float)* axis * numnodes, "BVHNodeBV");
 869                 if (!tree->nodebv) {
 870                         MEM_freeN(tree->nodes);
 871                         MEM_freeN(tree);
 872                 }
 873
 874                 tree->nodechild = (BVHNode**)MEM_callocN(sizeof(BVHNode*) * tree_type * numnodes, "BVHNodeBV");
 875                 if (!tree->nodechild) {
 876                         MEM_freeN(tree->nodebv);
 877                         MEM_freeN(tree->nodes);
 878                         MEM_freeN(tree);
 879                 }
 880
 881                 tree->nodearray = (BVHNode *)MEM_callocN(sizeof(BVHNode)* numnodes, "BVHNodeArray");
 882
 883                 if (!tree->nodearray) {
 884                         MEM_freeN(tree->nodechild);
 885                         MEM_freeN(tree->nodebv);
 886                         MEM_freeN(tree->nodes);
 887                         MEM_freeN(tree);
 888                         return NULL;
 889                 }
 890
 891                 //link the dynamic bv and child links
 892                 for (i=0; i< numnodes; i++) {
 893                         tree->nodearray[i].bv = tree->nodebv + i * axis;
 894                         tree->nodearray[i].children = tree->nodechild + i * tree_type;
 895                 }
 896
 897         }
 898
 899         return tree;
 900 }
 901
 902 void BLI_bvhtree_free(BVHTree *tree)
 903 {
 904         if (tree) {
 905                 MEM_freeN(tree->nodes);
 906                 MEM_freeN(tree->nodearray);
 907                 MEM_freeN(tree->nodebv);
 908                 MEM_freeN(tree->nodechild);
 909                 MEM_freeN(tree);
 910         }
 911 }
 912
 913 void BLI_bvhtree_balance(BVHTree *tree)
 914 {
 915         int i;
 916
 917         BVHNode*  branches_array = tree->nodearray + tree->totleaf;
 918         BVHNode** leafs_array    = tree->nodes;
 919
 920         //This function should only be called once (some big bug goes here if its being called more than once per tree)
 921         assert(tree->totbranch == 0);
 922
 923         //Build the implicit tree
 924         non_recursive_bvh_div_nodes(tree, branches_array, leafs_array, tree->totleaf);
 925
 926         //current code expects the branches to be linked to the nodes array
 927         //we perform that linkage here
 928         tree->totbranch = implicit_needed_branches(tree->tree_type, tree->totleaf);
 929         for (i = 0; i < tree->totbranch; i++)
 930                 tree->nodes[tree->totleaf + i] = branches_array + i;
 931
 932         build_skip_links(tree, tree->nodes[tree->totleaf], NULL, NULL);
 933         //bvhtree_info(tree);
 934 }
 935
 936 int BLI_bvhtree_insert(BVHTree *tree, int index, const float *co, int numpoints)
 937 {
 938         int i;
 939         BVHNode *node = NULL;
 940
 941         // insert should only possible as long as tree->totbranch is 0
 942         if (tree->totbranch > 0)
 943                 return 0;
 944
 945         if (tree->totleaf+1 >= MEM_allocN_len(tree->nodes)/sizeof(*(tree->nodes)))
 946                 return 0;
 947
 948         // TODO check if have enough nodes in array
 949
 950         node = tree->nodes[tree->totleaf] = &(tree->nodearray[tree->totleaf]);
 951         tree->totleaf++;
 952
 953         create_kdop_hull(tree, node, co, numpoints, 0);
 954         node->index= index;
 955
 956         // inflate the bv with some epsilon
 957         for (i = tree->start_axis; i < tree->stop_axis; i++) {
 958                 node->bv[(2 * i)] -= tree->epsilon; // minimum
 959                 node->bv[(2 * i) + 1] += tree->epsilon; // maximum
 960         }
 961
 962         return 1;
 963 }
 964
 965
 966 // call before BLI_bvhtree_update_tree()
 967 int BLI_bvhtree_update_node(BVHTree *tree, int index, const float *co, const float *co_moving, int numpoints)
 968 {
 969         int i;
 970         BVHNode *node= NULL;
 971
 972         // check if index exists
 973         if (index > tree->totleaf)
 974                 return 0;
 975
 976         node = tree->nodearray + index;
 977
 978         create_kdop_hull(tree, node, co, numpoints, 0);
 979
 980         if (co_moving)
 981                 create_kdop_hull(tree, node, co_moving, numpoints, 1);
 982
 983         // inflate the bv with some epsilon
 984         for (i = tree->start_axis; i < tree->stop_axis; i++) {
 985                 node->bv[(2 * i)] -= tree->epsilon; // minimum
 986                 node->bv[(2 * i) + 1] += tree->epsilon; // maximum
 987         }
 988
 989         return 1;
 990 }
 991
 992 // call BLI_bvhtree_update_node() first for every node/point/triangle
 993 void BLI_bvhtree_update_tree(BVHTree *tree)
 994 {
 995         //Update bottom=>top
 996         //TRICKY: the way we build the tree all the childs have an index greater than the parent
 997         //This allows us todo a bottom up update by starting on the biger numbered branch
 998
 999         BVHNode** root  = tree->nodes + tree->totleaf;
1000         BVHNode** index = tree->nodes + tree->totleaf + tree->totbranch-1;
1001
1002         for (; index >= root; index--)
1003                 node_join(tree, *index);
1004 }
1005
1006 float BLI_bvhtree_getepsilon(BVHTree *tree)
1007 {
1008         return tree->epsilon;
1009 }
1010
1011
1012 /*
1013  * BLI_bvhtree_overlap
1014  */
1015 // overlap - is it possbile for 2 bv's to collide ?
1016 static int tree_overlap(BVHNode *node1, BVHNode *node2, int start_axis, int stop_axis)
1017 {
1018         float *bv1 = node1->bv;
1019         float *bv2 = node2->bv;
1020
1021         float *bv1_end = bv1 + (stop_axis<<1);
1022
1023         bv1 += start_axis<<1;
1024         bv2 += start_axis<<1;
1025
1026         // test all axis if min + max overlap
1027         for (; bv1 != bv1_end; bv1+=2, bv2+=2) {
1028                 if ((*(bv1) > *(bv2 + 1)) || (*(bv2) > *(bv1 + 1)))
1029                         return 0;
1030         }
1031
1032         return 1;
1033 }
1034
1035 static void traverse(BVHOverlapData *data, BVHNode *node1, BVHNode *node2)
1036 {
1037         int j;
1038
1039         if (tree_overlap(node1, node2, data->start_axis, data->stop_axis)) {
1040                 // check if node1 is a leaf
1041                 if (!node1->totnode) {
1042                         // check if node2 is a leaf
1043                         if (!node2->totnode) {
1044
1045                                 if (node1 == node2) {
1046                                         return;
1047                                 }
1048
1049                                 if (data->i >= data->max_overlap) {
1050                                         // try to make alloc'ed memory bigger
1051                                         data->overlap = realloc(data->overlap, sizeof(BVHTreeOverlap)*data->max_overlap*2);
1052
1053                                         if (!data->overlap) {
1054                                                 printf("Out of Memory in traverse\n");
1055                                                 return;
1056                                         }
1057                                         data->max_overlap *= 2;
1058                                 }
1059
1060                                 // both leafs, insert overlap!
1061                                 data->overlap[data->i].indexA = node1->index;
1062                                 data->overlap[data->i].indexB = node2->index;
1063
1064                                 data->i++;
1065                         }
1066                         else {
1067                                 for (j = 0; j < data->tree2->tree_type; j++) {
1068                                         if (node2->children[j])
1069                                                 traverse(data, node1, node2->children[j]);
1070                                 }
1071                         }
1072                 }
1073                 else {
1074                         for (j = 0; j < data->tree2->tree_type; j++) {
1075                                 if (node1->children[j])
1076                                         traverse(data, node1->children[j], node2);
1077                         }
1078                 }
1079         }
1080         return;
1081 }
1082
1083 BVHTreeOverlap *BLI_bvhtree_overlap(BVHTree *tree1, BVHTree *tree2, unsigned int *result)
1084 {
1085         int j;
1086         unsigned int total = 0;
1087         BVHTreeOverlap *overlap = NULL, *to = NULL;
1088         BVHOverlapData **data;
1089
1090         // check for compatibility of both trees (can't compare 14-DOP with 18-DOP)
1091         if ((tree1->axis != tree2->axis) && (tree1->axis == 14 || tree2->axis == 14) && (tree1->axis == 18 || tree2->axis == 18))
1092                 return NULL;
1093
1094         // fast check root nodes for collision before doing big splitting + traversal
1095         if (!tree_overlap(tree1->nodes[tree1->totleaf], tree2->nodes[tree2->totleaf], MIN2(tree1->start_axis, tree2->start_axis), MIN2(tree1->stop_axis, tree2->stop_axis)))
1096                 return NULL;
1097
1098         data = MEM_callocN(sizeof(BVHOverlapData *)* tree1->tree_type, "BVHOverlapData_star");
1099
1100         for (j = 0; j < tree1->tree_type; j++) {
1101                 data[j] = (BVHOverlapData *)MEM_callocN(sizeof(BVHOverlapData), "BVHOverlapData");
1102
1103                 // init BVHOverlapData
1104                 data[j]->overlap = (BVHTreeOverlap *)malloc(sizeof(BVHTreeOverlap)*MAX2(tree1->totleaf, tree2->totleaf));
1105                 data[j]->tree1 = tree1;
1106                 data[j]->tree2 = tree2;
1107                 data[j]->max_overlap = MAX2(tree1->totleaf, tree2->totleaf);
1108                 data[j]->i = 0;
1109                 data[j]->start_axis = MIN2(tree1->start_axis, tree2->start_axis);
1110                 data[j]->stop_axis  = MIN2(tree1->stop_axis,  tree2->stop_axis );
1111         }
1112
1113 #pragma omp parallel for private(j) schedule(static)
1114         for (j = 0; j < MIN2(tree1->tree_type, tree1->nodes[tree1->totleaf]->totnode); j++) {
1115                 traverse(data[j], tree1->nodes[tree1->totleaf]->children[j], tree2->nodes[tree2->totleaf]);
1116         }
1117
1118         for (j = 0; j < tree1->tree_type; j++)
1119                 total += data[j]->i;
1120
1121         to = overlap = (BVHTreeOverlap *)MEM_callocN(sizeof(BVHTreeOverlap)*total, "BVHTreeOverlap");
1122
1123         for (j = 0; j < tree1->tree_type; j++) {
1124                 memcpy(to, data[j]->overlap, data[j]->i*sizeof(BVHTreeOverlap));
1125                 to+=data[j]->i;
1126         }
1127
1128         for (j = 0; j < tree1->tree_type; j++) {
1129                 free(data[j]->overlap);
1130                 MEM_freeN(data[j]);
1131         }
1132         MEM_freeN(data);
1133
1134         (*result) = total;
1135         return overlap;
1136 }
1137
1138 //Determines the nearest point of the given node BV. Returns the squared distance to that point.
1139 static float calc_nearest_point(const float proj[3], BVHNode *node, float *nearest)
1140 {
1141         int i;
1142         const float *bv = node->bv;
1143
1144         //nearest on AABB hull
1145         for (i=0; i != 3; i++, bv += 2) {
1146                 if (bv[0] > proj[i])
1147                         nearest[i] = bv[0];
1148                 else if (bv[1] < proj[i])
1149                         nearest[i] = bv[1];
1150                 else
1151                         nearest[i] = proj[i];
1152         }
1153
1154 #if 0
1155         //nearest on a general hull
1156         copy_v3_v3(nearest, data->co);
1157         for (i = data->tree->start_axis; i != data->tree->stop_axis; i++, bv+=2)
1158         {
1159                 float proj = dot_v3v3( nearest, KDOP_AXES[i]);
1160                 float dl = bv[0] - proj;
1161                 float du = bv[1] - proj;
1162
1163                 if (dl > 0) {
1164                         madd_v3_v3fl(nearest, KDOP_AXES[i], dl);
1165                 }
1166                 else if (du < 0) {
1167                         madd_v3_v3fl(nearest, KDOP_AXES[i], du);
1168                 }
1169         }
1170 #endif
1171
1172         return len_squared_v3v3(proj, nearest);
1173 }
1174
1175
1176 typedef struct NodeDistance
1177 {
1178         BVHNode *node;
1179         float dist;
1180
1181 } NodeDistance;
1182
1183 #define NodeDistance_priority(a, b)     ( (a).dist < (b).dist )
1184
1185 // TODO: use a priority queue to reduce the number of nodes looked on
1186 static void dfs_find_nearest_dfs(BVHNearestData *data, BVHNode *node)
1187 {
1188         if (node->totnode == 0) {
1189                 if (data->callback)
1190                         data->callback(data->userdata, node->index, data->co, &data->nearest);
1191                 else {
1192                         data->nearest.index     = node->index;
1193                         data->nearest.dist      = calc_nearest_point(data->proj, node, data->nearest.co);
1194                 }
1195         }
1196         else {
1197                 //Better heuristic to pick the closest node to dive on
1198                 int i;
1199                 float nearest[3];
1200
1201                 if (data->proj[ node->main_axis ] <= node->children[0]->bv[node->main_axis*2+1]) {
1202
1203                         for (i=0; i != node->totnode; i++) {
1204                                 if ( calc_nearest_point(data->proj, node->children[i], nearest) >= data->nearest.dist) continue;
1205                                 dfs_find_nearest_dfs(data, node->children[i]);
1206                         }
1207                 }
1208                 else {
1209                         for (i=node->totnode-1; i >= 0 ; i--) {
1210                                 if ( calc_nearest_point(data->proj, node->children[i], nearest) >= data->nearest.dist) continue;
1211                                 dfs_find_nearest_dfs(data, node->children[i]);
1212                         }
1213                 }
1214         }
1215 }
1216
1217 static void dfs_find_nearest_begin(BVHNearestData *data, BVHNode *node)
1218 {
1219         float nearest[3], sdist;
1220         sdist = calc_nearest_point(data->proj, node, nearest);
1221         if (sdist >= data->nearest.dist) return;
1222         dfs_find_nearest_dfs(data, node);
1223 }
1224
1225
1226 #if 0
1227 static void NodeDistance_push_heap(NodeDistance *heap, int heap_size)
1228 PUSH_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size)
1229
1230 static void NodeDistance_pop_heap(NodeDistance *heap, int heap_size)
1231 POP_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size)
1232
1233 //NN function that uses an heap.. this functions leads to an optimal number of min-distance
1234 //but for normal tri-faces and BV 6-dop.. a simple dfs with local heuristics (as implemented
1235 //in source/blender/blenkernel/intern/shrinkwrap.c) works faster.
1236 //
1237 //It may make sense to use this function if the callback queries are very slow.. or if its impossible
1238 //to get a nice heuristic
1239 //
1240 //this function uses "malloc/free" instead of the MEM_* because it intends to be openmp safe
1241 static void bfs_find_nearest(BVHNearestData *data, BVHNode *node)
1242 {
1243         int i;
1244         NodeDistance default_heap[DEFAULT_FIND_NEAREST_HEAP_SIZE];
1245         NodeDistance *heap=default_heap, current;
1246         int heap_size = 0, max_heap_size = sizeof(default_heap)/sizeof(default_heap[0]);
1247         float nearest[3];
1248
1249         int callbacks = 0, push_heaps = 0;
1250
1251         if (node->totnode == 0)
1252         {
1253                 dfs_find_nearest_dfs(data, node);
1254                 return;
1255         }
1256
1257         current.node = node;
1258         current.dist = calc_nearest_point(data->proj, node, nearest);
1259
1260         while (current.dist < data->nearest.dist)
1261         {
1262 //              printf("%f : %f\n", current.dist, data->nearest.dist);
1263                 for (i=0; i< current.node->totnode; i++)
1264                 {
1265                         BVHNode *child = current.node->children[i];
1266                         if (child->totnode == 0)
1267                         {
1268                                 callbacks++;
1269                                 dfs_find_nearest_dfs(data, child);
1270                         }
1271                         else
1272                         {
1273                                 //adjust heap size
1274                                 if ((heap_size >= max_heap_size) &&
1275                                     ADJUST_MEMORY(default_heap, (void**)&heap, heap_size+1, &max_heap_size, sizeof(heap[0])) == FALSE)
1276                                 {
1277                                         printf("WARNING: bvh_find_nearest got out of memory\n");
1278
1279                                         if (heap != default_heap)
1280                                                 free(heap);
1281
1282                                         return;
1283                                 }
1284
1285                                 heap[heap_size].node = current.node->children[i];
1286                                 heap[heap_size].dist = calc_nearest_point(data->proj, current.node->children[i], nearest);
1287
1288                                 if (heap[heap_size].dist >= data->nearest.dist) continue;
1289                                 heap_size++;
1290
1291                                 NodeDistance_push_heap(heap, heap_size);
1292         //                      PUSH_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size);
1293                                 push_heaps++;
1294                         }
1295                 }
1296
1297                 if (heap_size == 0) break;
1298
1299                 current = heap[0];
1300                 NodeDistance_pop_heap(heap, heap_size);
1301 //              POP_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size);
1302                 heap_size--;
1303         }
1304
1305 //      printf("hsize=%d, callbacks=%d, pushs=%d\n", heap_size, callbacks, push_heaps);
1306
1307         if (heap != default_heap)
1308                 free(heap);
1309 }
1310 #endif
1311
1312
1313 int BLI_bvhtree_find_nearest(BVHTree *tree, const float co[3], BVHTreeNearest *nearest, BVHTree_NearestPointCallback callback, void *userdata)
1314 {
1315         int i;
1316
1317         BVHNearestData data;
1318         BVHNode* root = tree->nodes[tree->totleaf];
1319
1320         //init data to search
1321         data.tree = tree;
1322         data.co = co;
1323
1324         data.callback = callback;
1325         data.userdata = userdata;
1326
1327         for (i = data.tree->start_axis; i != data.tree->stop_axis; i++) {
1328                 data.proj[i] = dot_v3v3(data.co, KDOP_AXES[i]);
1329         }
1330
1331         if (nearest) {
1332                 memcpy(&data.nearest, nearest, sizeof(*nearest));
1333         }
1334         else {
1335                 data.nearest.index = -1;
1336                 data.nearest.dist = FLT_MAX;
1337         }
1338
1339         //dfs search
1340         if (root)
1341                 dfs_find_nearest_begin(&data, root);
1342
1343         //copy back results
1344         if (nearest) {
1345                 memcpy(nearest, &data.nearest, sizeof(*nearest));
1346         }
1347
1348         return data.nearest.index;
1349 }
1350
1351
1352 /*
1353  * Raycast - BLI_bvhtree_ray_cast
1354  *
1355  * raycast is done by performing a DFS on the BVHTree and saving the closest hit
1356  */
1357
1358
1359 //Determines the distance that the ray must travel to hit the bounding volume of the given node
1360 static float ray_nearest_hit(BVHRayCastData *data, float *bv)
1361 {
1362         int i;
1363
1364         float low = 0, upper = data->hit.dist;
1365
1366         for (i=0; i != 3; i++, bv += 2) {
1367                 if (data->ray_dot_axis[i] == 0.0f) {
1368                         //axis aligned ray
1369                         if (data->ray.origin[i] < bv[0] - data->ray.radius ||
1370                             data->ray.origin[i] > bv[1] + data->ray.radius)
1371                         {
1372                                 return FLT_MAX;
1373                         }
1374                 }
1375                 else {
1376                         float ll = (bv[0] - data->ray.radius - data->ray.origin[i]) / data->ray_dot_axis[i];
1377                         float lu = (bv[1] + data->ray.radius - data->ray.origin[i]) / data->ray_dot_axis[i];
1378
1379                         if (data->ray_dot_axis[i] > 0.0f) {
1380                                 if (ll > low)   low = ll;
1381                                 if (lu < upper) upper = lu;
1382                         }
1383                         else {
1384                                 if (lu > low)   low = lu;
1385                                 if (ll < upper) upper = ll;
1386                         }
1387
1388                         if (low > upper) return FLT_MAX;
1389                 }
1390         }
1391         return low;
1392 }
1393
1394 //Determines the distance that the ray must travel to hit the bounding volume of the given node
1395 //Based on Tactical Optimization of Ray/Box Intersection, by Graham Fyffe
1396 //[http://tog.acm.org/resources/RTNews/html/rtnv21n1.html#art9]
1397 //
1398 //TODO this doens't has data->ray.radius in consideration
1399 static float fast_ray_nearest_hit(const BVHRayCastData *data, const BVHNode *node)
1400 {
1401         const float *bv = node->bv;
1402         float dist;
1403
1404         float t1x = (bv[data->index[0]] - data->ray.origin[0]) * data->idot_axis[0];
1405         float t2x = (bv[data->index[1]] - data->ray.origin[0]) * data->idot_axis[0];
1406         float t1y = (bv[data->index[2]] - data->ray.origin[1]) * data->idot_axis[1];
1407         float t2y = (bv[data->index[3]] - data->ray.origin[1]) * data->idot_axis[1];
1408         float t1z = (bv[data->index[4]] - data->ray.origin[2]) * data->idot_axis[2];
1409         float t2z = (bv[data->index[5]] - data->ray.origin[2]) * data->idot_axis[2];
1410
1411         if (t1x > t2y || t2x < t1y || t1x > t2z || t2x < t1z || t1y > t2z || t2y < t1z) return FLT_MAX;
1412         if (t2x < 0.0f || t2y < 0.0f || t2z < 0.0f) return FLT_MAX;
1413         if (t1x > data->hit.dist || t1y > data->hit.dist || t1z > data->hit.dist) return FLT_MAX;
1414
1415         dist = t1x;
1416         if (t1y > dist) dist = t1y;
1417         if (t1z > dist) dist = t1z;
1418         return dist;
1419 }
1420
1421 static void dfs_raycast(BVHRayCastData *data, BVHNode *node)
1422 {
1423         int i;
1424
1425         //ray-bv is really fast.. and simple tests revealed its worth to test it
1426         //before calling the ray-primitive functions
1427         /* XXX: temporary solution for particles until fast_ray_nearest_hit supports ray.radius */
1428         float dist = (data->ray.radius > 0.0f) ? ray_nearest_hit(data, node->bv) : fast_ray_nearest_hit(data, node);
1429         if (dist >= data->hit.dist) return;
1430
1431         if (node->totnode == 0) {
1432                 if (data->callback) {
1433                         data->callback(data->userdata, node->index, &data->ray, &data->hit);
1434                 }
1435                 else {
1436                         data->hit.index = node->index;
1437                         data->hit.dist  = dist;
1438                         madd_v3_v3v3fl(data->hit.co, data->ray.origin, data->ray.direction, dist);
1439                 }
1440         }
1441         else {
1442                 //pick loop direction to dive into the tree (based on ray direction and split axis)
1443                 if (data->ray_dot_axis[ (int)node->main_axis ] > 0.0f) {
1444                         for (i=0; i != node->totnode; i++) {
1445                                 dfs_raycast(data, node->children[i]);
1446                         }
1447                 }
1448                 else {
1449                         for (i=node->totnode-1; i >= 0; i--) {
1450                                 dfs_raycast(data, node->children[i]);
1451                         }
1452                 }
1453         }
1454 }
1455
1456 #if 0
1457 static void iterative_raycast(BVHRayCastData *data, BVHNode *node)
1458 {
1459         while (node)
1460         {
1461                 float dist = fast_ray_nearest_hit(data, node);
1462                 if (dist >= data->hit.dist)
1463                 {
1464                         node = node->skip[1];
1465                         continue;
1466                 }
1467
1468                 if (node->totnode == 0)
1469                 {
1470                         if (data->callback)
1471                                 data->callback(data->userdata, node->index, &data->ray, &data->hit);
1472                         else
1473                         {
1474                                 data->hit.index = node->index;
1475                                 data->hit.dist  = dist;
1476                                 madd_v3_v3v3fl(data->hit.co, data->ray.origin, data->ray.direction, dist);
1477                         }
1478
1479                         node = node->skip[1];
1480                 }
1481                 else
1482                 {
1483                         node = node->children[0];
1484                 }
1485         }
1486 }
1487 #endif
1488
1489 int BLI_bvhtree_ray_cast(BVHTree *tree, const float co[3], const float dir[3], float radius, BVHTreeRayHit *hit, BVHTree_RayCastCallback callback, void *userdata)
1490 {
1491         int i;
1492         BVHRayCastData data;
1493         BVHNode * root = tree->nodes[tree->totleaf];
1494
1495         data.tree = tree;
1496
1497         data.callback = callback;
1498         data.userdata = userdata;
1499
1500         copy_v3_v3(data.ray.origin,    co);
1501         copy_v3_v3(data.ray.direction, dir);
1502         data.ray.radius = radius;
1503
1504         normalize_v3(data.ray.direction);
1505
1506         for (i=0; i<3; i++) {
1507                 data.ray_dot_axis[i] = dot_v3v3(data.ray.direction, KDOP_AXES[i]);
1508                 data.idot_axis[i] = 1.0f / data.ray_dot_axis[i];
1509
1510                 if (fabsf(data.ray_dot_axis[i]) < FLT_EPSILON) {
1511                         data.ray_dot_axis[i] = 0.0;
1512                 }
1513                 data.index[2*i] = data.idot_axis[i] < 0.0f ? 1 : 0;
1514                 data.index[2*i+1] = 1 - data.index[2*i];
1515                 data.index[2*i]   += 2*i;
1516                 data.index[2*i+1] += 2*i;
1517         }
1518
1519
1520         if (hit)
1521                 memcpy(&data.hit, hit, sizeof(*hit));
1522         else {
1523                 data.hit.index = -1;
1524                 data.hit.dist = FLT_MAX;
1525         }
1526
1527         if (root) {
1528                 dfs_raycast(&data, root);
1529 //              iterative_raycast(&data, root);
1530          }
1531
1532
1533         if (hit)
1534                 memcpy(hit, &data.hit, sizeof(*hit));
1535
1536         return data.hit.index;
1537 }
1538
1539 float BLI_bvhtree_bb_raycast(float *bv, const float light_start[3], const float light_end[3], float pos[3])
1540 {
1541         BVHRayCastData data;
1542         float dist = 0.0;
1543
1544         data.hit.dist = FLT_MAX;
1545
1546         // get light direction
1547         data.ray.direction[0] = light_end[0] - light_start[0];
1548         data.ray.direction[1] = light_end[1] - light_start[1];
1549         data.ray.direction[2] = light_end[2] - light_start[2];
1550
1551         data.ray.radius = 0.0;
1552
1553         data.ray.origin[0] = light_start[0];
1554         data.ray.origin[1] = light_start[1];
1555         data.ray.origin[2] = light_start[2];
1556
1557         normalize_v3(data.ray.direction);
1558         copy_v3_v3(data.ray_dot_axis, data.ray.direction);
1559
1560         dist = ray_nearest_hit(&data, bv);
1561
1562         if (dist > 0.0f) {
1563                 madd_v3_v3v3fl(pos, light_start, data.ray.direction, dist);
1564         }
1565         return dist;
1566
1567 }
1568
1569 /*
1570  * Range Query - as request by broken :P
1571  *
1572  * Allocs and fills an array with the indexs of node that are on the given spherical range (center, radius)
1573  * Returns the size of the array.
1574  */
1575 typedef struct RangeQueryData
1576 {
1577         BVHTree *tree;
1578         const float *center;
1579         float radius;                   //squared radius
1580
1581         int hits;
1582
1583         BVHTree_RangeQuery callback;
1584         void *userdata;
1585
1586
1587 } RangeQueryData;
1588
1589
1590 static void dfs_range_query(RangeQueryData *data, BVHNode *node)
1591 {
1592         if (node->totnode == 0) {
1593 #if 0   /*UNUSED*/
1594                 //Calculate the node min-coords (if the node was a point then this is the point coordinates)
1595                 float co[3];
1596                 co[0] = node->bv[0];
1597                 co[1] = node->bv[2];
1598                 co[2] = node->bv[4];
1599 #endif
1600         }
1601         else {
1602                 int i;
1603                 for (i=0; i != node->totnode; i++) {
1604                         float nearest[3];
1605                         float dist = calc_nearest_point(data->center, node->children[i], nearest);
1606                         if (dist < data->radius) {
1607                                 //Its a leaf.. call the callback
1608                                 if (node->children[i]->totnode == 0) {
1609                                         data->hits++;
1610                                         data->callback(data->userdata, node->children[i]->index, dist);
1611                                 }
1612                                 else
1613                                         dfs_range_query(data, node->children[i]);
1614                         }
1615                 }
1616         }
1617 }
1618
1619 int BLI_bvhtree_range_query(BVHTree *tree, const float co[3], float radius, BVHTree_RangeQuery callback, void *userdata)
1620 {
1621         BVHNode * root = tree->nodes[tree->totleaf];
1622
1623         RangeQueryData data;
1624         data.tree = tree;
1625         data.center = co;
1626         data.radius = radius*radius;
1627         data.hits = 0;
1628
1629         data.callback = callback;
1630         data.userdata = userdata;
1631
1632         if (root != NULL) {
1633                 float nearest[3];
1634                 float dist = calc_nearest_point(data.center, root, nearest);
1635                 if (dist < data.radius) {
1636                         /* Its a leaf.. call the callback */
1637                         if (root->totnode == 0) {
1638                                 data.hits++;
1639                                 data.callback(data.userdata, root->index, dist);
1640                         }
1641                         else
1642                                 dfs_range_query(&data, root);
1643                 }
1644         }
1645
1646         return data.hits;
1647 }