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