source/blender/blenlib/intern/BLI_kdopbvh.c

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