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