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