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