027c6e084f5d0923a402220ac7d9e1e61e512b41
[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  *  \brief BVH-tree implementation.
31  *
32  * k-DOP BVH (Discrete Oriented Polytope, Bounding Volume Hierarchy).
33  * A k-DOP is represented as k/2 pairs of min , max values for k/2 directions (intervals, "slabs").
34  *
35  * See: http://www.gris.uni-tuebingen.de/people/staff/jmezger/papers/bvh.pdf
36  *
37  * implements a bvh-tree structure with support for:
38  *
39  * - Ray-cast:
40  *   #BLI_bvhtree_ray_cast, #BVHRayCastData
41  * - Nearest point on surface:
42  *   #BLI_bvhtree_find_nearest, #BVHNearestData
43  * - Overlapping 2 trees:
44  *   #BLI_bvhtree_overlap, #BVHOverlapData_Shared, #BVHOverlapData_Thread
45  * - Range Query:
46  *   #BLI_bvhtree_range_query
47  */
48
49 #include <assert.h>
50
51 #include "MEM_guardedalloc.h"
52
53 #include "BLI_utildefines.h"
54 #include "BLI_alloca.h"
55 #include "BLI_stack.h"
56 #include "BLI_kdopbvh.h"
57 #include "BLI_math.h"
58 #include "BLI_task.h"
59
60 #include "BLI_strict_flags.h"
61
62 /* used for iterative_raycast */
63 // #define USE_SKIP_LINKS
64
65 /* Use to print balanced output. */
66 // #define USE_PRINT_TREE
67
68 /* Check tree is valid. */
69 // #define USE_VERIFY_TREE
70
71
72 #define MAX_TREETYPE 32
73
74 /* Setting zero so we can catch bugs in BLI_task/KDOPBVH.
75  * TODO(sergey): Deduplicate the limits with PBVH from BKE.
76  */
77 #ifdef DEBUG
78 #  define KDOPBVH_THREAD_LEAF_THRESHOLD 0
79 #else
80 #  define KDOPBVH_THREAD_LEAF_THRESHOLD 1024
81 #endif
82
83
84 /* -------------------------------------------------------------------- */
85
86 /** \name Struct Definitions
87  * \{ */
88
89 typedef unsigned char axis_t;
90
91 typedef struct BVHNode {
92         struct BVHNode **children;
93         struct BVHNode *parent; /* some user defined traversed need that */
94 #ifdef USE_SKIP_LINKS
95         struct BVHNode *skip[2];
96 #endif
97         float *bv;      /* Bounding volume of all nodes, max 13 axis */
98         int index;      /* face, edge, vertex index */
99         char totnode;   /* how many nodes are used, used for speedup */
100         char main_axis; /* Axis used to split this node */
101 } BVHNode;
102
103 /* keep under 26 bytes for speed purposes */
104 struct BVHTree {
105         BVHNode **nodes;
106         BVHNode *nodearray;     /* pre-alloc branch nodes */
107         BVHNode **nodechild;    /* pre-alloc childs for nodes */
108         float   *nodebv;        /* pre-alloc bounding-volumes for nodes */
109         float epsilon;          /* epslion is used for inflation of the k-dop      */
110         int totleaf;            /* leafs */
111         int totbranch;
112         axis_t start_axis, stop_axis;  /* bvhtree_kdop_axes array indices according to axis */
113         axis_t axis;                   /* kdop type (6 => OBB, 7 => AABB, ...) */
114         char tree_type;                /* type of tree (4 => quadtree) */
115 };
116
117 /* optimization, ensure we stay small */
118 BLI_STATIC_ASSERT((sizeof(void *) == 8 && sizeof(BVHTree) <= 48) ||
119                   (sizeof(void *) == 4 && sizeof(BVHTree) <= 32),
120                   "over sized")
121
122 /* avoid duplicating vars in BVHOverlapData_Thread */
123 typedef struct BVHOverlapData_Shared {
124         const BVHTree *tree1, *tree2;
125         axis_t start_axis, stop_axis;
126
127         /* use for callbacks */
128         BVHTree_OverlapCallback callback;
129         void *userdata;
130 } BVHOverlapData_Shared;
131
132 typedef struct BVHOverlapData_Thread {
133         BVHOverlapData_Shared *shared;
134         struct BLI_Stack *overlap;  /* store BVHTreeOverlap */
135         /* use for callbacks */
136         int thread;
137 } BVHOverlapData_Thread;
138
139 typedef struct BVHNearestData {
140         const BVHTree *tree;
141         const float *co;
142         BVHTree_NearestPointCallback callback;
143         void    *userdata;
144         float proj[13];         /* coordinates projection over axis */
145         BVHTreeNearest nearest;
146
147 } BVHNearestData;
148
149 typedef struct BVHRayCastData {
150         const BVHTree *tree;
151
152         BVHTree_RayCastCallback callback;
153         void    *userdata;
154
155
156         BVHTreeRay ray;
157
158 #ifdef USE_KDOPBVH_WATERTIGHT
159         struct IsectRayPrecalc isect_precalc;
160 #endif
161
162         /* initialized by bvhtree_ray_cast_data_precalc */
163         float ray_dot_axis[13];
164         float idot_axis[13];
165         int index[6];
166
167         BVHTreeRayHit hit;
168 } BVHRayCastData;
169
170 /** \} */
171
172
173 /**
174  * Bounding Volume Hierarchy Definition
175  *
176  * Notes: From OBB until 26-DOP --> all bounding volumes possible, just choose type below
177  * Notes: You have to choose the type at compile time ITM
178  * Notes: You can choose the tree type --> binary, quad, octree, choose below
179  */
180
181 const float bvhtree_kdop_axes[13][3] = {
182         {1.0, 0, 0}, {0, 1.0, 0}, {0, 0, 1.0},
183         {1.0, 1.0, 1.0}, {1.0, -1.0, 1.0}, {1.0, 1.0, -1.0}, {1.0, -1.0, -1.0},
184         {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}, {0, 1.0, -1.0}
185 };
186
187
188 /* -------------------------------------------------------------------- */
189
190 /** \name Utility Functions
191  * \{ */
192
193 MINLINE axis_t min_axis(axis_t a, axis_t b)
194 {
195         return (a < b) ? a : b;
196 }
197 #if 0
198 MINLINE axis_t max_axis(axis_t a, axis_t b)
199 {
200         return (b < a) ? a : b;
201 }
202 #endif
203
204 #if 0
205
206 /*
207  * Generic push and pop heap
208  */
209 #define PUSH_HEAP_BODY(HEAP_TYPE, PRIORITY, heap, heap_size)                  \
210         {                                                                         \
211                 HEAP_TYPE element = heap[heap_size - 1];                              \
212                 int child = heap_size - 1;                                            \
213                 while (child != 0) {                                                  \
214                         int parent = (child - 1) / 2;                                     \
215                         if (PRIORITY(element, heap[parent])) {                            \
216                                 heap[child] = heap[parent];                                   \
217                                 child = parent;                                               \
218                         }                                                                 \
219                         else {                                                            \
220                                 break;                                                        \
221                         }                                                                 \
222                 }                                                                     \
223                 heap[child] = element;                                                \
224         } (void)0
225
226 #define POP_HEAP_BODY(HEAP_TYPE, PRIORITY, heap, heap_size)                   \
227         {                                                                         \
228                 HEAP_TYPE element = heap[heap_size - 1];                              \
229                 int parent = 0;                                                       \
230                 while (parent < (heap_size - 1) / 2) {                                \
231                         int child2 = (parent + 1) * 2;                                    \
232                         if (PRIORITY(heap[child2 - 1], heap[child2])) {                   \
233                                 child2--;                                                     \
234                         }                                                                 \
235                         if (PRIORITY(element, heap[child2])) {                            \
236                                 break;                                                        \
237                         }                                                                 \
238                         heap[parent] = heap[child2];                                      \
239                         parent = child2;                                                  \
240                 }                                                                     \
241                 heap[parent] = element;                                               \
242         } (void)0
243
244 static bool ADJUST_MEMORY(void *local_memblock, void **memblock, int new_size, int *max_size, int size_per_item)
245 {
246         int new_max_size = *max_size * 2;
247         void *new_memblock = NULL;
248
249         if (new_size <= *max_size) {
250                 return true;
251         }
252
253         if (*memblock == local_memblock) {
254                 new_memblock = malloc(size_per_item * new_max_size);
255                 memcpy(new_memblock, *memblock, size_per_item * *max_size);
256         }
257         else {
258                 new_memblock = realloc(*memblock, size_per_item * new_max_size);
259         }
260
261         if (new_memblock) {
262                 *memblock = new_memblock;
263                 *max_size = new_max_size;
264                 return true;
265         }
266         else {
267                 return false;
268         }
269 }
270 #endif
271
272 /**
273  * Introsort
274  * with permission deriven from the following Java code:
275  * http://ralphunden.net/content/tutorials/a-guide-to-introsort/
276  * and he derived it from the SUN STL
277  */
278
279 //static int size_threshold = 16;
280
281 #if 0
282 /**
283  * Common methods for all algorithms
284  */
285 static int floor_lg(int a)
286 {
287         return (int)(floor(log(a) / log(2)));
288 }
289 #endif
290
291 static void node_minmax_init(const BVHTree *tree, BVHNode *node)
292 {
293         axis_t axis_iter;
294         float (*bv)[2] = (float (*)[2])node->bv;
295
296         for (axis_iter = tree->start_axis; axis_iter != tree->stop_axis; axis_iter++) {
297                 bv[axis_iter][0] =  FLT_MAX;
298                 bv[axis_iter][1] = -FLT_MAX;
299         }
300 }
301
302 /** \} */
303
304
305 /* -------------------------------------------------------------------- */
306
307 /** \name Balance Utility Functions
308  * \{ */
309
310 /**
311  * Insertion sort algorithm
312  */
313 static void bvh_insertionsort(BVHNode **a, int lo, int hi, int axis)
314 {
315         int i, j;
316         BVHNode *t;
317         for (i = lo; i < hi; i++) {
318                 j = i;
319                 t = a[i];
320                 while ((j != lo) && (t->bv[axis] < (a[j - 1])->bv[axis])) {
321                         a[j] = a[j - 1];
322                         j--;
323                 }
324                 a[j] = t;
325         }
326 }
327
328 static int bvh_partition(BVHNode **a, int lo, int hi, BVHNode *x, int axis)
329 {
330         int i = lo, j = hi;
331         while (1) {
332                 while (a[i]->bv[axis] < x->bv[axis]) {
333                         i++;
334                 }
335                 j--;
336                 while (x->bv[axis] < a[j]->bv[axis]) {
337                         j--;
338                 }
339                 if (!(i < j)) {
340                         return i;
341                 }
342                 SWAP(BVHNode *, a[i], a[j]);
343                 i++;
344         }
345 }
346
347 #if 0
348 /**
349  * Heapsort algorithm
350  */
351 static void bvh_downheap(BVHNode **a, int i, int n, int lo, int axis)
352 {
353         BVHNode *d = a[lo + i - 1];
354         int child;
355         while (i <= n / 2) {
356                 child = 2 * i;
357                 if ((child < n) && ((a[lo + child - 1])->bv[axis] < (a[lo + child])->bv[axis])) {
358                         child++;
359                 }
360                 if (!(d->bv[axis] < (a[lo + child - 1])->bv[axis])) break;
361                 a[lo + i - 1] = a[lo + child - 1];
362                 i = child;
363         }
364         a[lo + i - 1] = d;
365 }
366
367 static void bvh_heapsort(BVHNode **a, int lo, int hi, int axis)
368 {
369         int n = hi - lo, i;
370         for (i = n / 2; i >= 1; i = i - 1) {
371                 bvh_downheap(a, i, n, lo, axis);
372         }
373         for (i = n; i > 1; i = i - 1) {
374                 SWAP(BVHNode *, a[lo], a[lo + i - 1]);
375                 bvh_downheap(a, 1, i - 1, lo, axis);
376         }
377 }
378 #endif
379
380 static BVHNode *bvh_medianof3(BVHNode **a, int lo, int mid, int hi, int axis)  /* returns Sortable */
381 {
382         if ((a[mid])->bv[axis] < (a[lo])->bv[axis]) {
383                 if ((a[hi])->bv[axis] < (a[mid])->bv[axis])
384                         return a[mid];
385                 else {
386                         if ((a[hi])->bv[axis] < (a[lo])->bv[axis])
387                                 return a[hi];
388                         else
389                                 return a[lo];
390                 }
391         }
392         else {
393                 if ((a[hi])->bv[axis] < (a[mid])->bv[axis]) {
394                         if ((a[hi])->bv[axis] < (a[lo])->bv[axis])
395                                 return a[lo];
396                         else
397                                 return a[hi];
398                 }
399                 else
400                         return a[mid];
401         }
402 }
403
404 #if 0
405 /*
406  * Quicksort algorithm modified for Introsort
407  */
408 static void bvh_introsort_loop(BVHNode **a, int lo, int hi, int depth_limit, int axis)
409 {
410         int p;
411
412         while (hi - lo > size_threshold) {
413                 if (depth_limit == 0) {
414                         bvh_heapsort(a, lo, hi, axis);
415                         return;
416                 }
417                 depth_limit = depth_limit - 1;
418                 p = bvh_partition(a, lo, hi, bvh_medianof3(a, lo, lo + ((hi - lo) / 2) + 1, hi - 1, axis), axis);
419                 bvh_introsort_loop(a, p, hi, depth_limit, axis);
420                 hi = p;
421         }
422 }
423
424 static void sort(BVHNode **a0, int begin, int end, int axis)
425 {
426         if (begin < end) {
427                 BVHNode **a = a0;
428                 bvh_introsort_loop(a, begin, end, 2 * floor_lg(end - begin), axis);
429                 bvh_insertionsort(a, begin, end, axis);
430         }
431 }
432
433 static void sort_along_axis(BVHTree *tree, int start, int end, int axis)
434 {
435         sort(tree->nodes, start, end, axis);
436 }
437 #endif
438
439 /**
440  * \note after a call to this function you can expect one of:
441  * - every node to left of a[n] are smaller or equal to it
442  * - every node to the right of a[n] are greater or equal to it */
443 static void partition_nth_element(BVHNode **a, int begin, int end, const int n, const int axis)
444 {
445         while (end - begin > 3) {
446                 const int cut = bvh_partition(a, begin, end, bvh_medianof3(a, begin, (begin + end) / 2, end - 1, axis), axis);
447                 if (cut <= n) {
448                         begin = cut;
449                 }
450                 else {
451                         end = cut;
452                 }
453         }
454         bvh_insertionsort(a, begin, end, axis);
455 }
456
457 #ifdef USE_SKIP_LINKS
458 static void build_skip_links(BVHTree *tree, BVHNode *node, BVHNode *left, BVHNode *right)
459 {
460         int i;
461         
462         node->skip[0] = left;
463         node->skip[1] = right;
464         
465         for (i = 0; i < node->totnode; i++) {
466                 if (i + 1 < node->totnode)
467                         build_skip_links(tree, node->children[i], left, node->children[i + 1]);
468                 else
469                         build_skip_links(tree, node->children[i], left, right);
470
471                 left = node->children[i];
472         }
473 }
474 #endif
475
476 /*
477  * BVHTree bounding volumes functions
478  */
479 static void create_kdop_hull(const BVHTree *tree, BVHNode *node, const float *co, int numpoints, int moving)
480 {
481         float newminmax;
482         float *bv = node->bv;
483         int k;
484         axis_t axis_iter;
485         
486         /* don't init boudings for the moving case */
487         if (!moving) {
488                 node_minmax_init(tree, node);
489         }
490
491         for (k = 0; k < numpoints; k++) {
492                 /* for all Axes. */
493                 for (axis_iter = tree->start_axis; axis_iter < tree->stop_axis; axis_iter++) {
494                         newminmax = dot_v3v3(&co[k * 3], bvhtree_kdop_axes[axis_iter]);
495                         if (newminmax < bv[2 * axis_iter])
496                                 bv[2 * axis_iter] = newminmax;
497                         if (newminmax > bv[(2 * axis_iter) + 1])
498                                 bv[(2 * axis_iter) + 1] = newminmax;
499                 }
500         }
501 }
502
503 /**
504  * \note depends on the fact that the BVH's for each face is already build
505  */
506 static void refit_kdop_hull(const BVHTree *tree, BVHNode *node, int start, int end)
507 {
508         float newmin, newmax;
509         float *bv = node->bv;
510         int j;
511         axis_t axis_iter;
512
513         node_minmax_init(tree, node);
514
515         for (j = start; j < end; j++) {
516                 /* for all Axes. */
517                 for (axis_iter = tree->start_axis; axis_iter < tree->stop_axis; axis_iter++) {
518                         newmin = tree->nodes[j]->bv[(2 * axis_iter)];
519                         if ((newmin < bv[(2 * axis_iter)]))
520                                 bv[(2 * axis_iter)] = newmin;
521
522                         newmax = tree->nodes[j]->bv[(2 * axis_iter) + 1];
523                         if ((newmax > bv[(2 * axis_iter) + 1]))
524                                 bv[(2 * axis_iter) + 1] = newmax;
525                 }
526         }
527
528 }
529
530 /**
531  * only supports x,y,z axis in the moment
532  * but we should use a plain and simple function here for speed sake */
533 static char get_largest_axis(const float *bv)
534 {
535         float middle_point[3];
536
537         middle_point[0] = (bv[1]) - (bv[0]); /* x axis */
538         middle_point[1] = (bv[3]) - (bv[2]); /* y axis */
539         middle_point[2] = (bv[5]) - (bv[4]); /* z axis */
540         if (middle_point[0] > middle_point[1]) {
541                 if (middle_point[0] > middle_point[2])
542                         return 1;  /* max x axis */
543                 else
544                         return 5;  /* max z axis */
545         }
546         else {
547                 if (middle_point[1] > middle_point[2])
548                         return 3;  /* max y axis */
549                 else
550                         return 5;  /* max z axis */
551         }
552 }
553
554 /**
555  * bottom-up update of bvh node BV
556  * join the children on the parent BV */
557 static void node_join(BVHTree *tree, BVHNode *node)
558 {
559         int i;
560         axis_t axis_iter;
561
562         node_minmax_init(tree, node);
563         
564         for (i = 0; i < tree->tree_type; i++) {
565                 if (node->children[i]) {
566                         for (axis_iter = tree->start_axis; axis_iter < tree->stop_axis; axis_iter++) {
567                                 /* update minimum */
568                                 if (node->children[i]->bv[(2 * axis_iter)] < node->bv[(2 * axis_iter)])
569                                         node->bv[(2 * axis_iter)] = node->children[i]->bv[(2 * axis_iter)];
570
571                                 /* update maximum */
572                                 if (node->children[i]->bv[(2 * axis_iter) + 1] > node->bv[(2 * axis_iter) + 1])
573                                         node->bv[(2 * axis_iter) + 1] = node->children[i]->bv[(2 * axis_iter) + 1];
574                         }
575                 }
576                 else
577                         break;
578         }
579 }
580
581 #ifdef USE_PRINT_TREE
582
583 /**
584  * Debug and information functions
585  */
586
587 static void bvhtree_print_tree(BVHTree *tree, BVHNode *node, int depth)
588 {
589         int i;
590         axis_t axis_iter;
591
592         for (i = 0; i < depth; i++) printf(" ");
593         printf(" - %d (%ld): ", node->index, (long int)(node - tree->nodearray));
594         for (axis_iter = (axis_t)(2 * tree->start_axis);
595              axis_iter < (axis_t)(2 * tree->stop_axis);
596              axis_iter++)
597         {
598                 printf("%.3f ", node->bv[axis_iter]);
599         }
600         printf("\n");
601
602         for (i = 0; i < tree->tree_type; i++)
603                 if (node->children[i])
604                         bvhtree_print_tree(tree, node->children[i], depth + 1);
605 }
606
607 static void bvhtree_info(BVHTree *tree)
608 {
609         printf("BVHTree Info: tree_type = %d, axis = %d, epsilon = %f\n",
610                tree->tree_type, tree->axis, tree->epsilon);
611         printf("nodes = %d, branches = %d, leafs = %d\n",
612                tree->totbranch + tree->totleaf,  tree->totbranch, tree->totleaf);
613         printf("Memory per node = %ubytes\n",
614                (uint)(sizeof(BVHNode) + sizeof(BVHNode *) * tree->tree_type + sizeof(float) * tree->axis));
615         printf("BV memory = %ubytes\n",
616                (uint)MEM_allocN_len(tree->nodebv));
617
618         printf("Total memory = %ubytes\n",
619                (uint)(sizeof(BVHTree) +
620                       MEM_allocN_len(tree->nodes) +
621                       MEM_allocN_len(tree->nodearray) +
622                       MEM_allocN_len(tree->nodechild) +
623                       MEM_allocN_len(tree->nodebv)));
624
625         bvhtree_print_tree(tree, tree->nodes[tree->totleaf], 0);
626 }
627 #endif  /* USE_PRINT_TREE */
628
629 #ifdef USE_VERIFY_TREE
630
631 static void bvhtree_verify(BVHTree *tree)
632 {
633         int i, j, check = 0;
634         
635         /* check the pointer list */
636         for (i = 0; i < tree->totleaf; i++) {
637                 if (tree->nodes[i]->parent == NULL) {
638                         printf("Leaf has no parent: %d\n", i);
639                 }
640                 else {
641                         for (j = 0; j < tree->tree_type; j++) {
642                                 if (tree->nodes[i]->parent->children[j] == tree->nodes[i])
643                                         check = 1;
644                         }
645                         if (!check) {
646                                 printf("Parent child relationship doesn't match: %d\n", i);
647                         }
648                         check = 0;
649                 }
650         }
651         
652         /* check the leaf list */
653         for (i = 0; i < tree->totleaf; i++) {
654                 if (tree->nodearray[i].parent == NULL) {
655                         printf("Leaf has no parent: %d\n", i);
656                 }
657                 else {
658                         for (j = 0; j < tree->tree_type; j++) {
659                                 if (tree->nodearray[i].parent->children[j] == &tree->nodearray[i])
660                                         check = 1;
661                         }
662                         if (!check) {
663                                 printf("Parent child relationship doesn't match: %d\n", i);
664                         }
665                         check = 0;
666                 }
667         }
668         
669         printf("branches: %d, leafs: %d, total: %d\n",
670                tree->totbranch, tree->totleaf, tree->totbranch + tree->totleaf);
671 }
672 #endif  /* USE_VERIFY_TREE */
673
674 /* Helper data and structures to build a min-leaf generalized implicit tree
675  * This code can be easily reduced
676  * (basicly this is only method to calculate pow(k, n) in O(1).. and stuff like that) */
677 typedef struct BVHBuildHelper {
678         int tree_type;              /* */
679         int totleafs;               /* */
680
681         int leafs_per_child[32];    /* Min number of leafs that are archievable from a node at depth N */
682         int branches_on_level[32];  /* Number of nodes at depth N (tree_type^N) */
683
684         int remain_leafs;           /* Number of leafs that are placed on the level that is not 100% filled */
685
686 } BVHBuildHelper;
687
688 static void build_implicit_tree_helper(const BVHTree *tree, BVHBuildHelper *data)
689 {
690         int depth = 0;
691         int remain;
692         int nnodes;
693
694         data->totleafs = tree->totleaf;
695         data->tree_type = tree->tree_type;
696
697         /* Calculate the smallest tree_type^n such that tree_type^n >= num_leafs */
698         for (data->leafs_per_child[0] = 1;
699              data->leafs_per_child[0] <  data->totleafs;
700              data->leafs_per_child[0] *= data->tree_type)
701         {
702                 /* pass */
703         }
704
705         data->branches_on_level[0] = 1;
706
707         for (depth = 1; (depth < 32) && data->leafs_per_child[depth - 1]; depth++) {
708                 data->branches_on_level[depth] = data->branches_on_level[depth - 1] * data->tree_type;
709                 data->leafs_per_child[depth] = data->leafs_per_child[depth - 1] / data->tree_type;
710         }
711
712         remain = data->totleafs - data->leafs_per_child[1];
713         nnodes = (remain + data->tree_type - 2) / (data->tree_type - 1);
714         data->remain_leafs = remain + nnodes;
715 }
716
717 // return the min index of all the leafs archivable with the given branch
718 static int implicit_leafs_index(const BVHBuildHelper *data, const int depth, const int child_index)
719 {
720         int min_leaf_index = child_index * data->leafs_per_child[depth - 1];
721         if (min_leaf_index <= data->remain_leafs)
722                 return min_leaf_index;
723         else if (data->leafs_per_child[depth])
724                 return data->totleafs - (data->branches_on_level[depth - 1] - child_index) * data->leafs_per_child[depth];
725         else
726                 return data->remain_leafs;
727 }
728
729 /**
730  * Generalized implicit tree build
731  *
732  * An implicit tree is a tree where its structure is implied, thus there is no need to store child pointers or indexs.
733  * Its possible to find the position of the child or the parent with simple maths (multiplication and adittion).
734  * This type of tree is for example used on heaps.. where node N has its childs at indexs N*2 and N*2+1.
735  *
736  * Although in this case the tree type is general.. and not know until runtime.
737  * tree_type stands for the maximum number of childs that a tree node can have.
738  * All tree types >= 2 are supported.
739  *
740  * Advantages of the used trees include:
741  *  - No need to store child/parent relations (they are implicit);
742  *  - Any node child always has an index greater than the parent;
743  *  - Brother nodes are sequential in memory;
744  *
745  *
746  * Some math relations derived for general implicit trees:
747  *
748  *   K = tree_type, ( 2 <= K )
749  *   ROOT = 1
750  *   N child of node A = A * K + (2 - K) + N, (0 <= N < K)
751  *
752  * Util methods:
753  *   TODO...
754  *    (looping elements, knowing if its a leaf or not.. etc...)
755  */
756
757 /* This functions returns the number of branches needed to have the requested number of leafs. */
758 static int implicit_needed_branches(int tree_type, int leafs)
759 {
760         return max_ii(1, (leafs + tree_type - 3) / (tree_type - 1));
761 }
762
763 /**
764  * This function handles the problem of "sorting" the leafs (along the split_axis).
765  *
766  * It arranges the elements in the given partitions such that:
767  *  - any element in partition N is less or equal to any element in partition N+1.
768  *  - if all elements are different all partition will get the same subset of elements
769  *    as if the array was sorted.
770  *
771  * partition P is described as the elements in the range ( nth[P], nth[P+1] ]
772  *
773  * TODO: This can be optimized a bit by doing a specialized nth_element instead of K nth_elements
774  */
775 static void split_leafs(BVHNode **leafs_array, const int nth[], const int partitions, const int split_axis)
776 {
777         int i;
778         for (i = 0; i < partitions - 1; i++) {
779                 if (nth[i] >= nth[partitions])
780                         break;
781
782                 partition_nth_element(leafs_array, nth[i], nth[partitions], nth[i + 1], split_axis);
783         }
784 }
785
786 typedef struct BVHDivNodesData {
787         const BVHTree *tree;
788         BVHNode *branches_array;
789         BVHNode **leafs_array;
790
791         int tree_type;
792         int tree_offset;
793
794         const BVHBuildHelper *data;
795
796         int depth;
797         int i;
798         int first_of_next_level;
799 } BVHDivNodesData;
800
801 static void non_recursive_bvh_div_nodes_task_cb(
802         void *__restrict userdata,
803         const int j,
804         const ParallelRangeTLS *__restrict UNUSED(tls))
805 {
806         BVHDivNodesData *data = userdata;
807
808         int k;
809         const int parent_level_index = j - data->i;
810         BVHNode *parent = &data->branches_array[j];
811         int nth_positions[MAX_TREETYPE + 1];
812         char split_axis;
813
814         int parent_leafs_begin = implicit_leafs_index(data->data, data->depth, parent_level_index);
815         int parent_leafs_end   = implicit_leafs_index(data->data, data->depth, parent_level_index + 1);
816
817         /* This calculates the bounding box of this branch
818          * and chooses the largest axis as the axis to divide leafs */
819         refit_kdop_hull(data->tree, parent, parent_leafs_begin, parent_leafs_end);
820         split_axis = get_largest_axis(parent->bv);
821
822         /* Save split axis (this can be used on raytracing to speedup the query time) */
823         parent->main_axis = split_axis / 2;
824
825         /* Split the childs along the split_axis, note: its not needed to sort the whole leafs array
826          * Only to assure that the elements are partitioned on a way that each child takes the elements
827          * it would take in case the whole array was sorted.
828          * Split_leafs takes care of that "sort" problem. */
829         nth_positions[0] = parent_leafs_begin;
830         nth_positions[data->tree_type] = parent_leafs_end;
831         for (k = 1; k < data->tree_type; k++) {
832                 const int child_index = j * data->tree_type + data->tree_offset + k;
833                 const int child_level_index = child_index - data->first_of_next_level; /* child level index */
834                 nth_positions[k] = implicit_leafs_index(data->data, data->depth + 1, child_level_index);
835         }
836
837         split_leafs(data->leafs_array, nth_positions, data->tree_type, split_axis);
838
839         /* Setup children and totnode counters
840          * Not really needed but currently most of BVH code relies on having an explicit children structure */
841         for (k = 0; k < data->tree_type; k++) {
842                 const int child_index = j * data->tree_type + data->tree_offset + k;
843                 const int child_level_index = child_index - data->first_of_next_level; /* child level index */
844
845                 const int child_leafs_begin = implicit_leafs_index(data->data, data->depth + 1, child_level_index);
846                 const int child_leafs_end   = implicit_leafs_index(data->data, data->depth + 1, child_level_index + 1);
847
848                 if (child_leafs_end - child_leafs_begin > 1) {
849                         parent->children[k] = &data->branches_array[child_index];
850                         parent->children[k]->parent = parent;
851                 }
852                 else if (child_leafs_end - child_leafs_begin == 1) {
853                         parent->children[k] = data->leafs_array[child_leafs_begin];
854                         parent->children[k]->parent = parent;
855                 }
856                 else {
857                         break;
858                 }
859         }
860         parent->totnode = (char)k;
861 }
862
863 /**
864  * This functions builds an optimal implicit tree from the given leafs.
865  * Where optimal stands for:
866  *  - The resulting tree will have the smallest number of branches;
867  *  - At most only one branch will have NULL childs;
868  *  - All leafs will be stored at level N or N+1.
869  *
870  * This function creates an implicit tree on branches_array, the leafs are given on the leafs_array.
871  *
872  * The tree is built per depth levels. First branches at depth 1.. then branches at depth 2.. etc..
873  * The reason is that we can build level N+1 from level N without any data dependencies.. thus it allows
874  * to use multithread building.
875  *
876  * To archive this is necessary to find how much leafs are accessible from a certain branch, BVHBuildHelper
877  * #implicit_needed_branches and #implicit_leafs_index are auxiliary functions to solve that "optimal-split".
878  */
879 static void non_recursive_bvh_div_nodes(
880         const BVHTree *tree, BVHNode *branches_array, BVHNode **leafs_array, int num_leafs)
881 {
882         int i;
883
884         const int tree_type   = tree->tree_type;
885         const int tree_offset = 2 - tree->tree_type; /* this value is 0 (on binary trees) and negative on the others */
886         const int num_branches = implicit_needed_branches(tree_type, num_leafs);
887
888         BVHBuildHelper data;
889         int depth;
890
891         {
892                 /* set parent from root node to NULL */
893                 BVHNode *root = &branches_array[1];
894                 root->parent = NULL;
895
896                 /* Most of bvhtree code relies on 1-leaf trees having at least one branch
897                  * We handle that special case here */
898                 if (num_leafs == 1) {
899                         refit_kdop_hull(tree, root, 0, num_leafs);
900                         root->main_axis = get_largest_axis(root->bv) / 2;
901                         root->totnode = 1;
902                         root->children[0] = leafs_array[0];
903                         root->children[0]->parent = root;
904                         return;
905                 }
906         }
907
908         build_implicit_tree_helper(tree, &data);
909
910         BVHDivNodesData cb_data = {
911                 .tree = tree, .branches_array = branches_array, .leafs_array = leafs_array,
912                 .tree_type = tree_type, .tree_offset = tree_offset, .data = &data,
913                 .first_of_next_level = 0, .depth = 0, .i = 0,
914         };
915
916         /* Loop tree levels (log N) loops */
917         for (i = 1, depth = 1; i <= num_branches; i = i * tree_type + tree_offset, depth++) {
918                 const int first_of_next_level = i * tree_type + tree_offset;
919                 const int i_stop = min_ii(first_of_next_level, num_branches + 1);  /* index of last branch on this level */
920
921                 /* Loop all branches on this level */
922                 cb_data.first_of_next_level = first_of_next_level;
923                 cb_data.i = i;
924                 cb_data.depth = depth;
925
926                 if (true) {
927                         ParallelRangeSettings settings;
928                         BLI_parallel_range_settings_defaults(&settings);
929                         settings.use_threading = (num_leafs > KDOPBVH_THREAD_LEAF_THRESHOLD);
930                         BLI_task_parallel_range(
931                                 i, i_stop,
932                                 &cb_data,
933                                 non_recursive_bvh_div_nodes_task_cb,
934                                 &settings);
935                 }
936                 else {
937                         /* Less hassle for debugging. */
938                         ParallelRangeTLS tls = {0};
939                         for (int i_task = i; i_task < i_stop; i_task++) {
940                                 non_recursive_bvh_div_nodes_task_cb(&cb_data, i_task, &tls);
941                         }
942                 }
943         }
944 }
945
946 /** \} */
947
948
949 /* -------------------------------------------------------------------- */
950
951 /** \name BLI_bvhtree API
952  * \{ */
953
954 /**
955  * \note many callers don't check for ``NULL`` return.
956  */
957 BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
958 {
959         BVHTree *tree;
960         int numnodes, i;
961
962         BLI_assert(tree_type >= 2 && tree_type <= MAX_TREETYPE);
963
964         tree = MEM_callocN(sizeof(BVHTree), "BVHTree");
965
966         /* tree epsilon must be >= FLT_EPSILON
967          * so that tangent rays can still hit a bounding volume..
968          * this bug would show up when casting a ray aligned with a kdop-axis and with an edge of 2 faces */
969         epsilon = max_ff(FLT_EPSILON, epsilon);
970
971         if (tree) {
972                 tree->epsilon = epsilon;
973                 tree->tree_type = tree_type;
974                 tree->axis = axis;
975
976                 if (axis == 26) {
977                         tree->start_axis = 0;
978                         tree->stop_axis = 13;
979                 }
980                 else if (axis == 18) {
981                         tree->start_axis = 7;
982                         tree->stop_axis = 13;
983                 }
984                 else if (axis == 14) {
985                         tree->start_axis = 0;
986                         tree->stop_axis = 7;
987                 }
988                 else if (axis == 8) { /* AABB */
989                         tree->start_axis = 0;
990                         tree->stop_axis = 4;
991                 }
992                 else if (axis == 6) { /* OBB */
993                         tree->start_axis = 0;
994                         tree->stop_axis = 3;
995                 }
996                 else {
997                         /* should never happen! */
998                         BLI_assert(0);
999
1000                         goto fail;
1001                 }
1002
1003
1004                 /* Allocate arrays */
1005                 numnodes = maxsize + implicit_needed_branches(tree_type, maxsize) + tree_type;
1006
1007                 tree->nodes = MEM_callocN(sizeof(BVHNode *) * (size_t)numnodes, "BVHNodes");
1008                 tree->nodebv = MEM_callocN(sizeof(float) * (size_t)(axis * numnodes), "BVHNodeBV");
1009                 tree->nodechild = MEM_callocN(sizeof(BVHNode *) * (size_t)(tree_type * numnodes), "BVHNodeBV");
1010                 tree->nodearray = MEM_callocN(sizeof(BVHNode) * (size_t)numnodes, "BVHNodeArray");
1011                 
1012                 if (UNLIKELY((!tree->nodes) ||
1013                              (!tree->nodebv) ||
1014                              (!tree->nodechild) ||
1015                              (!tree->nodearray)))
1016                 {
1017                         goto fail;
1018                 }
1019
1020                 /* link the dynamic bv and child links */
1021                 for (i = 0; i < numnodes; i++) {
1022                         tree->nodearray[i].bv = &tree->nodebv[i * axis];
1023                         tree->nodearray[i].children = &tree->nodechild[i * tree_type];
1024                 }
1025                 
1026         }
1027         return tree;
1028
1029
1030 fail:
1031         MEM_SAFE_FREE(tree->nodes);
1032         MEM_SAFE_FREE(tree->nodebv);
1033         MEM_SAFE_FREE(tree->nodechild);
1034         MEM_SAFE_FREE(tree->nodearray);
1035
1036         MEM_freeN(tree);
1037
1038         return NULL;
1039 }
1040
1041 void BLI_bvhtree_free(BVHTree *tree)
1042 {
1043         if (tree) {
1044                 MEM_freeN(tree->nodes);
1045                 MEM_freeN(tree->nodearray);
1046                 MEM_freeN(tree->nodebv);
1047                 MEM_freeN(tree->nodechild);
1048                 MEM_freeN(tree);
1049         }
1050 }
1051
1052 void BLI_bvhtree_balance(BVHTree *tree)
1053 {
1054         BVHNode **leafs_array    = tree->nodes;
1055
1056         /* This function should only be called once
1057          * (some big bug goes here if its being called more than once per tree) */
1058         BLI_assert(tree->totbranch == 0);
1059
1060         /* Build the implicit tree */
1061         non_recursive_bvh_div_nodes(tree, tree->nodearray + (tree->totleaf - 1), leafs_array, tree->totleaf);
1062
1063         /* current code expects the branches to be linked to the nodes array
1064          * we perform that linkage here */
1065         tree->totbranch = implicit_needed_branches(tree->tree_type, tree->totleaf);
1066         for (int i = 0; i < tree->totbranch; i++) {
1067                 tree->nodes[tree->totleaf + i] = &tree->nodearray[tree->totleaf + i];
1068         }
1069
1070 #ifdef USE_SKIP_LINKS
1071         build_skip_links(tree, tree->nodes[tree->totleaf], NULL, NULL);
1072 #endif
1073
1074 #ifdef USE_VERIFY_TREE
1075         bvhtree_verify(tree);
1076 #endif
1077
1078 #ifdef USE_PRINT_TREE
1079         bvhtree_info(tree);
1080 #endif
1081 }
1082
1083 void BLI_bvhtree_insert(BVHTree *tree, int index, const float co[3], int numpoints)
1084 {
1085         axis_t axis_iter;
1086         BVHNode *node = NULL;
1087
1088         /* insert should only possible as long as tree->totbranch is 0 */
1089         BLI_assert(tree->totbranch <= 0);
1090         BLI_assert((size_t)tree->totleaf < MEM_allocN_len(tree->nodes) / sizeof(*(tree->nodes)));
1091
1092         node = tree->nodes[tree->totleaf] = &(tree->nodearray[tree->totleaf]);
1093         tree->totleaf++;
1094
1095         create_kdop_hull(tree, node, co, numpoints, 0);
1096         node->index = index;
1097
1098         /* inflate the bv with some epsilon */
1099         for (axis_iter = tree->start_axis; axis_iter < tree->stop_axis; axis_iter++) {
1100                 node->bv[(2 * axis_iter)] -= tree->epsilon; /* minimum */
1101                 node->bv[(2 * axis_iter) + 1] += tree->epsilon; /* maximum */
1102         }
1103 }
1104
1105
1106 /* call before BLI_bvhtree_update_tree() */
1107 bool BLI_bvhtree_update_node(BVHTree *tree, int index, const float co[3], const float co_moving[3], int numpoints)
1108 {
1109         BVHNode *node = NULL;
1110         axis_t axis_iter;
1111         
1112         /* check if index exists */
1113         if (index > tree->totleaf)
1114                 return false;
1115         
1116         node = tree->nodearray + index;
1117         
1118         create_kdop_hull(tree, node, co, numpoints, 0);
1119         
1120         if (co_moving)
1121                 create_kdop_hull(tree, node, co_moving, numpoints, 1);
1122         
1123         /* inflate the bv with some epsilon */
1124         for (axis_iter = tree->start_axis; axis_iter < tree->stop_axis; axis_iter++) {
1125                 node->bv[(2 * axis_iter)]     -= tree->epsilon; /* minimum */
1126                 node->bv[(2 * axis_iter) + 1] += tree->epsilon; /* maximum */
1127         }
1128
1129         return true;
1130 }
1131
1132 /* call BLI_bvhtree_update_node() first for every node/point/triangle */
1133 void BLI_bvhtree_update_tree(BVHTree *tree)
1134 {
1135         /* Update bottom=>top
1136          * TRICKY: the way we build the tree all the childs have an index greater than the parent
1137          * This allows us todo a bottom up update by starting on the bigger numbered branch */
1138
1139         BVHNode **root  = tree->nodes + tree->totleaf;
1140         BVHNode **index = tree->nodes + tree->totleaf + tree->totbranch - 1;
1141
1142         for (; index >= root; index--)
1143                 node_join(tree, *index);
1144 }
1145 /**
1146  * Number of times #BLI_bvhtree_insert has been called.
1147  * mainly useful for asserts functions to check we added the correct number.
1148  */
1149 int BLI_bvhtree_get_len(const BVHTree *tree)
1150 {
1151         return tree->totleaf;
1152 }
1153
1154 /**
1155  * Maximum number of children that a node can have.
1156  */
1157 int BLI_bvhtree_get_tree_type(const BVHTree *tree)
1158 {
1159         return tree->tree_type;
1160 }
1161
1162 float BLI_bvhtree_get_epsilon(const BVHTree *tree)
1163 {
1164         return tree->epsilon;
1165 }
1166
1167 /** \} */
1168
1169
1170 /* -------------------------------------------------------------------- */
1171
1172 /** \name BLI_bvhtree_overlap
1173  * \{ */
1174
1175 /**
1176  * overlap - is it possible for 2 bv's to collide ?
1177  */
1178 static bool tree_overlap_test(const BVHNode *node1, const BVHNode *node2, axis_t start_axis, axis_t stop_axis)
1179 {
1180         const float *bv1     = node1->bv + (start_axis << 1);
1181         const float *bv2     = node2->bv + (start_axis << 1);
1182         const float *bv1_end = node1->bv + (stop_axis  << 1);
1183         
1184         /* test all axis if min + max overlap */
1185         for (; bv1 != bv1_end; bv1 += 2, bv2 += 2) {
1186                 if ((bv1[0] > bv2[1]) || (bv2[0] > bv1[1])) {
1187                         return 0;
1188                 }
1189         }
1190
1191         return 1;
1192 }
1193
1194 static void tree_overlap_traverse(
1195         BVHOverlapData_Thread *data_thread,
1196         const BVHNode *node1, const BVHNode *node2)
1197 {
1198         BVHOverlapData_Shared *data = data_thread->shared;
1199         int j;
1200
1201         if (tree_overlap_test(node1, node2, data->start_axis, data->stop_axis)) {
1202                 /* check if node1 is a leaf */
1203                 if (!node1->totnode) {
1204                         /* check if node2 is a leaf */
1205                         if (!node2->totnode) {
1206                                 BVHTreeOverlap *overlap;
1207
1208                                 if (UNLIKELY(node1 == node2)) {
1209                                         return;
1210                                 }
1211
1212                                 /* both leafs, insert overlap! */
1213                                 overlap = BLI_stack_push_r(data_thread->overlap);
1214                                 overlap->indexA = node1->index;
1215                                 overlap->indexB = node2->index;
1216                         }
1217                         else {
1218                                 for (j = 0; j < data->tree2->tree_type; j++) {
1219                                         if (node2->children[j]) {
1220                                                 tree_overlap_traverse(data_thread, node1, node2->children[j]);
1221                                         }
1222                                 }
1223                         }
1224                 }
1225                 else {
1226                         for (j = 0; j < data->tree2->tree_type; j++) {
1227                                 if (node1->children[j]) {
1228                                         tree_overlap_traverse(data_thread, node1->children[j], node2);
1229                                 }
1230                         }
1231                 }
1232         }
1233 }
1234
1235 /**
1236  * a version of #tree_overlap_traverse that runs a callback to check if the nodes really intersect.
1237  */
1238 static void tree_overlap_traverse_cb(
1239         BVHOverlapData_Thread *data_thread,
1240         const BVHNode *node1, const BVHNode *node2)
1241 {
1242         BVHOverlapData_Shared *data = data_thread->shared;
1243         int j;
1244
1245         if (tree_overlap_test(node1, node2, data->start_axis, data->stop_axis)) {
1246                 /* check if node1 is a leaf */
1247                 if (!node1->totnode) {
1248                         /* check if node2 is a leaf */
1249                         if (!node2->totnode) {
1250                                 BVHTreeOverlap *overlap;
1251
1252                                 if (UNLIKELY(node1 == node2)) {
1253                                         return;
1254                                 }
1255
1256                                 /* only difference to tree_overlap_traverse! */
1257                                 if (data->callback(data->userdata, node1->index, node2->index, data_thread->thread)) {
1258                                         /* both leafs, insert overlap! */
1259                                         overlap = BLI_stack_push_r(data_thread->overlap);
1260                                         overlap->indexA = node1->index;
1261                                         overlap->indexB = node2->index;
1262                                 }
1263                         }
1264                         else {
1265                                 for (j = 0; j < data->tree2->tree_type; j++) {
1266                                         if (node2->children[j]) {
1267                                                 tree_overlap_traverse_cb(data_thread, node1, node2->children[j]);
1268                                         }
1269                                 }
1270                         }
1271                 }
1272                 else {
1273                         for (j = 0; j < data->tree2->tree_type; j++) {
1274                                 if (node1->children[j]) {
1275                                         tree_overlap_traverse_cb(data_thread, node1->children[j], node2);
1276                                 }
1277                         }
1278                 }
1279         }
1280 }
1281
1282 /**
1283  * Use to check the total number of threads #BLI_bvhtree_overlap will use.
1284  *
1285  * \warning Must be the first tree passed to #BLI_bvhtree_overlap!
1286  */
1287 int BLI_bvhtree_overlap_thread_num(const BVHTree *tree)
1288 {
1289         return (int)MIN2(tree->tree_type, tree->nodes[tree->totleaf]->totnode);
1290 }
1291
1292 static void bvhtree_overlap_task_cb(
1293         void *__restrict userdata,
1294         const int j,
1295         const ParallelRangeTLS *__restrict UNUSED(tls))
1296 {
1297         BVHOverlapData_Thread *data = &((BVHOverlapData_Thread *)userdata)[j];
1298         BVHOverlapData_Shared *data_shared = data->shared;
1299
1300         if (data_shared->callback) {
1301                 tree_overlap_traverse_cb(
1302                             data, data_shared->tree1->nodes[data_shared->tree1->totleaf]->children[j],
1303                             data_shared->tree2->nodes[data_shared->tree2->totleaf]);
1304         }
1305         else {
1306                 tree_overlap_traverse(
1307                             data, data_shared->tree1->nodes[data_shared->tree1->totleaf]->children[j],
1308                             data_shared->tree2->nodes[data_shared->tree2->totleaf]);
1309         }
1310 }
1311
1312 BVHTreeOverlap *BLI_bvhtree_overlap(
1313         const BVHTree *tree1, const BVHTree *tree2, uint *r_overlap_tot,
1314         /* optional callback to test the overlap before adding (must be thread-safe!) */
1315         BVHTree_OverlapCallback callback, void *userdata)
1316 {
1317         const int thread_num = BLI_bvhtree_overlap_thread_num(tree1);
1318         int j;
1319         size_t total = 0;
1320         BVHTreeOverlap *overlap = NULL, *to = NULL;
1321         BVHOverlapData_Shared data_shared;
1322         BVHOverlapData_Thread *data = BLI_array_alloca(data, (size_t)thread_num);
1323         axis_t start_axis, stop_axis;
1324         
1325         /* check for compatibility of both trees (can't compare 14-DOP with 18-DOP) */
1326         if (UNLIKELY((tree1->axis != tree2->axis) &&
1327                      (tree1->axis == 14 || tree2->axis == 14) &&
1328                      (tree1->axis == 18 || tree2->axis == 18)))
1329         {
1330                 BLI_assert(0);
1331                 return NULL;
1332         }
1333
1334         start_axis = min_axis(tree1->start_axis, tree2->start_axis);
1335         stop_axis  = min_axis(tree1->stop_axis,  tree2->stop_axis);
1336         
1337         /* fast check root nodes for collision before doing big splitting + traversal */
1338         if (!tree_overlap_test(tree1->nodes[tree1->totleaf], tree2->nodes[tree2->totleaf], start_axis, stop_axis)) {
1339                 return NULL;
1340         }
1341
1342         data_shared.tree1 = tree1;
1343         data_shared.tree2 = tree2;
1344         data_shared.start_axis = start_axis;
1345         data_shared.stop_axis = stop_axis;
1346
1347         /* can be NULL */
1348         data_shared.callback = callback;
1349         data_shared.userdata = userdata;
1350
1351         for (j = 0; j < thread_num; j++) {
1352                 /* init BVHOverlapData_Thread */
1353                 data[j].shared = &data_shared;
1354                 data[j].overlap = BLI_stack_new(sizeof(BVHTreeOverlap), __func__);
1355
1356                 /* for callback */
1357                 data[j].thread = j;
1358         }
1359
1360         ParallelRangeSettings settings;
1361         BLI_parallel_range_settings_defaults(&settings);
1362         settings.use_threading = (tree1->totleaf > KDOPBVH_THREAD_LEAF_THRESHOLD);
1363         BLI_task_parallel_range(
1364                     0, thread_num,
1365                     data,
1366                     bvhtree_overlap_task_cb,
1367                     &settings);
1368         
1369         for (j = 0; j < thread_num; j++)
1370                 total += BLI_stack_count(data[j].overlap);
1371         
1372         to = overlap = MEM_mallocN(sizeof(BVHTreeOverlap) * total, "BVHTreeOverlap");
1373         
1374         for (j = 0; j < thread_num; j++) {
1375                 uint count = (uint)BLI_stack_count(data[j].overlap);
1376                 BLI_stack_pop_n(data[j].overlap, to, count);
1377                 BLI_stack_free(data[j].overlap);
1378                 to += count;
1379         }
1380
1381         *r_overlap_tot = (uint)total;
1382         return overlap;
1383 }
1384
1385 /** \} */
1386
1387
1388 /* -------------------------------------------------------------------- */
1389
1390 /** \name BLI_bvhtree_find_nearest
1391  * \{ */
1392
1393 /* Determines the nearest point of the given node BV. Returns the squared distance to that point. */
1394 static float calc_nearest_point_squared(const float proj[3], BVHNode *node, float nearest[3])
1395 {
1396         int i;
1397         const float *bv = node->bv;
1398
1399         /* nearest on AABB hull */
1400         for (i = 0; i != 3; i++, bv += 2) {
1401                 if (bv[0] > proj[i])
1402                         nearest[i] = bv[0];
1403                 else if (bv[1] < proj[i])
1404                         nearest[i] = bv[1];
1405                 else
1406                         nearest[i] = proj[i]; 
1407         }
1408
1409 #if 0
1410         /* nearest on a general hull */
1411         copy_v3_v3(nearest, data->co);
1412         for (i = data->tree->start_axis; i != data->tree->stop_axis; i++, bv += 2) {
1413                 float proj = dot_v3v3(nearest, bvhtree_kdop_axes[i]);
1414                 float dl = bv[0] - proj;
1415                 float du = bv[1] - proj;
1416
1417                 if (dl > 0) {
1418                         madd_v3_v3fl(nearest, bvhtree_kdop_axes[i], dl);
1419                 }
1420                 else if (du < 0) {
1421                         madd_v3_v3fl(nearest, bvhtree_kdop_axes[i], du);
1422                 }
1423         }
1424 #endif
1425
1426         return len_squared_v3v3(proj, nearest);
1427 }
1428
1429 /* TODO: use a priority queue to reduce the number of nodes looked on */
1430 static void dfs_find_nearest_dfs(BVHNearestData *data, BVHNode *node)
1431 {
1432         if (node->totnode == 0) {
1433                 if (data->callback)
1434                         data->callback(data->userdata, node->index, data->co, &data->nearest);
1435                 else {
1436                         data->nearest.index = node->index;
1437                         data->nearest.dist_sq = calc_nearest_point_squared(data->proj, node, data->nearest.co);
1438                 }
1439         }
1440         else {
1441                 /* Better heuristic to pick the closest node to dive on */
1442                 int i;
1443                 float nearest[3];
1444
1445                 if (data->proj[node->main_axis] <= node->children[0]->bv[node->main_axis * 2 + 1]) {
1446
1447                         for (i = 0; i != node->totnode; i++) {
1448                                 if (calc_nearest_point_squared(data->proj, node->children[i], nearest) >= data->nearest.dist_sq)
1449                                         continue;
1450                                 dfs_find_nearest_dfs(data, node->children[i]);
1451                         }
1452                 }
1453                 else {
1454                         for (i = node->totnode - 1; i >= 0; i--) {
1455                                 if (calc_nearest_point_squared(data->proj, node->children[i], nearest) >= data->nearest.dist_sq)
1456                                         continue;
1457                                 dfs_find_nearest_dfs(data, node->children[i]);
1458                         }
1459                 }
1460         }
1461 }
1462
1463 static void dfs_find_nearest_begin(BVHNearestData *data, BVHNode *node)
1464 {
1465         float nearest[3], dist_sq;
1466         dist_sq = calc_nearest_point_squared(data->proj, node, nearest);
1467         if (dist_sq >= data->nearest.dist_sq) {
1468                 return;
1469         }
1470         dfs_find_nearest_dfs(data, node);
1471 }
1472
1473
1474 #if 0
1475
1476 typedef struct NodeDistance {
1477         BVHNode *node;
1478         float dist;
1479
1480 } NodeDistance;
1481
1482 #define DEFAULT_FIND_NEAREST_HEAP_SIZE 1024
1483
1484 #define NodeDistance_priority(a, b) ((a).dist < (b).dist)
1485
1486 static void NodeDistance_push_heap(NodeDistance *heap, int heap_size)
1487 PUSH_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size)
1488
1489 static void NodeDistance_pop_heap(NodeDistance *heap, int heap_size)
1490 POP_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size)
1491
1492 /* NN function that uses an heap.. this functions leads to an optimal number of min-distance
1493  * but for normal tri-faces and BV 6-dop.. a simple dfs with local heuristics (as implemented
1494  * in source/blender/blenkernel/intern/shrinkwrap.c) works faster.
1495  *
1496  * It may make sense to use this function if the callback queries are very slow.. or if its impossible
1497  * to get a nice heuristic
1498  *
1499  * this function uses "malloc/free" instead of the MEM_* because it intends to be thread safe */
1500 static void bfs_find_nearest(BVHNearestData *data, BVHNode *node)
1501 {
1502         int i;
1503         NodeDistance default_heap[DEFAULT_FIND_NEAREST_HEAP_SIZE];
1504         NodeDistance *heap = default_heap, current;
1505         int heap_size = 0, max_heap_size = sizeof(default_heap) / sizeof(default_heap[0]);
1506         float nearest[3];
1507
1508         int callbacks = 0, push_heaps = 0;
1509
1510         if (node->totnode == 0) {
1511                 dfs_find_nearest_dfs(data, node);
1512                 return;
1513         }
1514
1515         current.node = node;
1516         current.dist = calc_nearest_point(data->proj, node, nearest);
1517
1518         while (current.dist < data->nearest.dist) {
1519 //              printf("%f : %f\n", current.dist, data->nearest.dist);
1520                 for (i = 0; i < current.node->totnode; i++) {
1521                         BVHNode *child = current.node->children[i];
1522                         if (child->totnode == 0) {
1523                                 callbacks++;
1524                                 dfs_find_nearest_dfs(data, child);
1525                         }
1526                         else {
1527                                 /* adjust heap size */
1528                                 if ((heap_size >= max_heap_size) &&
1529                                     ADJUST_MEMORY(default_heap, (void **)&heap,
1530                                                   heap_size + 1, &max_heap_size, sizeof(heap[0])) == false)
1531                                 {
1532                                         printf("WARNING: bvh_find_nearest got out of memory\n");
1533
1534                                         if (heap != default_heap)
1535                                                 free(heap);
1536
1537                                         return;
1538                                 }
1539
1540                                 heap[heap_size].node = current.node->children[i];
1541                                 heap[heap_size].dist = calc_nearest_point(data->proj, current.node->children[i], nearest);
1542
1543                                 if (heap[heap_size].dist >= data->nearest.dist) continue;
1544                                 heap_size++;
1545
1546                                 NodeDistance_push_heap(heap, heap_size);
1547                                 //                      PUSH_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size);
1548                                 push_heaps++;
1549                         }
1550                 }
1551                 
1552                 if (heap_size == 0) break;
1553
1554                 current = heap[0];
1555                 NodeDistance_pop_heap(heap, heap_size);
1556 //              POP_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size);
1557                 heap_size--;
1558         }
1559
1560 //      printf("hsize=%d, callbacks=%d, pushs=%d\n", heap_size, callbacks, push_heaps);
1561
1562         if (heap != default_heap)
1563                 free(heap);
1564 }
1565 #endif
1566
1567
1568 int BLI_bvhtree_find_nearest(
1569         BVHTree *tree, const float co[3], BVHTreeNearest *nearest,
1570         BVHTree_NearestPointCallback callback, void *userdata)
1571 {
1572         axis_t axis_iter;
1573
1574         BVHNearestData data;
1575         BVHNode *root = tree->nodes[tree->totleaf];
1576
1577         /* init data to search */
1578         data.tree = tree;
1579         data.co = co;
1580
1581         data.callback = callback;
1582         data.userdata = userdata;
1583
1584         for (axis_iter = data.tree->start_axis; axis_iter != data.tree->stop_axis; axis_iter++) {
1585                 data.proj[axis_iter] = dot_v3v3(data.co, bvhtree_kdop_axes[axis_iter]);
1586         }
1587
1588         if (nearest) {
1589                 memcpy(&data.nearest, nearest, sizeof(*nearest));
1590         }
1591         else {
1592                 data.nearest.index = -1;
1593                 data.nearest.dist_sq = FLT_MAX;
1594         }
1595
1596         /* dfs search */
1597         if (root)
1598                 dfs_find_nearest_begin(&data, root);
1599
1600         /* copy back results */
1601         if (nearest) {
1602                 memcpy(nearest, &data.nearest, sizeof(*nearest));
1603         }
1604
1605         return data.nearest.index;
1606 }
1607
1608 /** \} */
1609
1610
1611 /* -------------------------------------------------------------------- */
1612
1613 /** \name BLI_bvhtree_ray_cast
1614  *
1615  * raycast is done by performing a DFS on the BVHTree and saving the closest hit.
1616  *
1617  * \{ */
1618
1619
1620 /* Determines the distance that the ray must travel to hit the bounding volume of the given node */
1621 static float ray_nearest_hit(const BVHRayCastData *data, const float bv[6])
1622 {
1623         int i;
1624
1625         float low = 0, upper = data->hit.dist;
1626
1627         for (i = 0; i != 3; i++, bv += 2) {
1628                 if (data->ray_dot_axis[i] == 0.0f) {
1629                         /* axis aligned ray */
1630                         if (data->ray.origin[i] < bv[0] - data->ray.radius ||
1631                             data->ray.origin[i] > bv[1] + data->ray.radius)
1632                         {
1633                                 return FLT_MAX;
1634                         }
1635                 }
1636                 else {
1637                         float ll = (bv[0] - data->ray.radius - data->ray.origin[i]) / data->ray_dot_axis[i];
1638                         float lu = (bv[1] + data->ray.radius - data->ray.origin[i]) / data->ray_dot_axis[i];
1639
1640                         if (data->ray_dot_axis[i] > 0.0f) {
1641                                 if (ll > low) low = ll;
1642                                 if (lu < upper) upper = lu;
1643                         }
1644                         else {
1645                                 if (lu > low) low = lu;
1646                                 if (ll < upper) upper = ll;
1647                         }
1648         
1649                         if (low > upper) return FLT_MAX;
1650                 }
1651         }
1652         return low;
1653 }
1654
1655 /**
1656  * Determines the distance that the ray must travel to hit the bounding volume of the given node
1657  * Based on Tactical Optimization of Ray/Box Intersection, by Graham Fyffe
1658  * [http://tog.acm.org/resources/RTNews/html/rtnv21n1.html#art9]
1659  *
1660  * TODO this doesn't take data->ray.radius into consideration */
1661 static float fast_ray_nearest_hit(const BVHRayCastData *data, const BVHNode *node)
1662 {
1663         const float *bv = node->bv;
1664         
1665         float t1x = (bv[data->index[0]] - data->ray.origin[0]) * data->idot_axis[0];
1666         float t2x = (bv[data->index[1]] - data->ray.origin[0]) * data->idot_axis[0];
1667         float t1y = (bv[data->index[2]] - data->ray.origin[1]) * data->idot_axis[1];
1668         float t2y = (bv[data->index[3]] - data->ray.origin[1]) * data->idot_axis[1];
1669         float t1z = (bv[data->index[4]] - data->ray.origin[2]) * data->idot_axis[2];
1670         float t2z = (bv[data->index[5]] - data->ray.origin[2]) * data->idot_axis[2];
1671
1672         if ((t1x > t2y || t2x < t1y || t1x > t2z || t2x < t1z || t1y > t2z || t2y < t1z) ||
1673             (t2x < 0.0f || t2y < 0.0f || t2z < 0.0f) ||
1674             (t1x > data->hit.dist || t1y > data->hit.dist || t1z > data->hit.dist))
1675         {
1676                 return FLT_MAX;
1677         }
1678         else {
1679                 return max_fff(t1x, t1y, t1z);
1680         }
1681 }
1682
1683 static void dfs_raycast(BVHRayCastData *data, BVHNode *node)
1684 {
1685         int i;
1686
1687         /* ray-bv is really fast.. and simple tests revealed its worth to test it
1688          * before calling the ray-primitive functions */
1689         /* XXX: temporary solution for particles until fast_ray_nearest_hit supports ray.radius */
1690         float dist = (data->ray.radius == 0.0f) ? fast_ray_nearest_hit(data, node) : ray_nearest_hit(data, node->bv);
1691         if (dist >= data->hit.dist) {
1692                 return;
1693         }
1694
1695         if (node->totnode == 0) {
1696                 if (data->callback) {
1697                         data->callback(data->userdata, node->index, &data->ray, &data->hit);
1698                 }
1699                 else {
1700                         data->hit.index = node->index;
1701                         data->hit.dist  = dist;
1702                         madd_v3_v3v3fl(data->hit.co, data->ray.origin, data->ray.direction, dist);
1703                 }
1704         }
1705         else {
1706                 /* pick loop direction to dive into the tree (based on ray direction and split axis) */
1707                 if (data->ray_dot_axis[node->main_axis] > 0.0f) {
1708                         for (i = 0; i != node->totnode; i++) {
1709                                 dfs_raycast(data, node->children[i]);
1710                         }
1711                 }
1712                 else {
1713                         for (i = node->totnode - 1; i >= 0; i--) {
1714                                 dfs_raycast(data, node->children[i]);
1715                         }
1716                 }
1717         }
1718 }
1719
1720 /**
1721  * A version of #dfs_raycast with minor changes to reset the index & dist each ray cast.
1722  */
1723 static void dfs_raycast_all(BVHRayCastData *data, BVHNode *node)
1724 {
1725         int i;
1726
1727         /* ray-bv is really fast.. and simple tests revealed its worth to test it
1728          * before calling the ray-primitive functions */
1729         /* XXX: temporary solution for particles until fast_ray_nearest_hit supports ray.radius */
1730         float dist = (data->ray.radius == 0.0f) ? fast_ray_nearest_hit(data, node) : ray_nearest_hit(data, node->bv);
1731         if (dist >= data->hit.dist) {
1732                 return;
1733         }
1734
1735         if (node->totnode == 0) {
1736                 /* no need to check for 'data->callback' (using 'all' only makes sense with a callback). */
1737                 dist = data->hit.dist;
1738                 data->callback(data->userdata, node->index, &data->ray, &data->hit);
1739                 data->hit.index = -1;
1740                 data->hit.dist = dist;
1741         }
1742         else {
1743                 /* pick loop direction to dive into the tree (based on ray direction and split axis) */
1744                 if (data->ray_dot_axis[node->main_axis] > 0.0f) {
1745                         for (i = 0; i != node->totnode; i++) {
1746                                 dfs_raycast_all(data, node->children[i]);
1747                         }
1748                 }
1749                 else {
1750                         for (i = node->totnode - 1; i >= 0; i--) {
1751                                 dfs_raycast_all(data, node->children[i]);
1752                         }
1753                 }
1754         }
1755 }
1756
1757 #if 0
1758 static void iterative_raycast(BVHRayCastData *data, BVHNode *node)
1759 {
1760         while (node) {
1761                 float dist = fast_ray_nearest_hit(data, node);
1762                 if (dist >= data->hit.dist) {
1763                         node = node->skip[1];
1764                         continue;
1765                 }
1766
1767                 if (node->totnode == 0) {
1768                         if (data->callback) {
1769                                 data->callback(data->userdata, node->index, &data->ray, &data->hit);
1770                         }
1771                         else {
1772                                 data->hit.index = node->index;
1773                                 data->hit.dist  = dist;
1774                                 madd_v3_v3v3fl(data->hit.co, data->ray.origin, data->ray.direction, dist);
1775                         }
1776                         
1777                         node = node->skip[1];
1778                 }
1779                 else {
1780                         node = node->children[0];
1781                 }
1782         }
1783 }
1784 #endif
1785
1786 static void bvhtree_ray_cast_data_precalc(BVHRayCastData *data, int flag)
1787 {
1788         int i;
1789
1790         for (i = 0; i < 3; i++) {
1791                 data->ray_dot_axis[i] = dot_v3v3(data->ray.direction, bvhtree_kdop_axes[i]);
1792                 data->idot_axis[i] = 1.0f / data->ray_dot_axis[i];
1793
1794                 if (fabsf(data->ray_dot_axis[i]) < FLT_EPSILON) {
1795                         data->ray_dot_axis[i] = 0.0;
1796                 }
1797                 data->index[2 * i] = data->idot_axis[i] < 0.0f ? 1 : 0;
1798                 data->index[2 * i + 1] = 1 - data->index[2 * i];
1799                 data->index[2 * i]   += 2 * i;
1800                 data->index[2 * i + 1] += 2 * i;
1801         }
1802
1803 #ifdef USE_KDOPBVH_WATERTIGHT
1804         if (flag & BVH_RAYCAST_WATERTIGHT) {
1805                 isect_ray_tri_watertight_v3_precalc(&data->isect_precalc, data->ray.direction);
1806                 data->ray.isect_precalc = &data->isect_precalc;
1807         }
1808         else {
1809                 data->ray.isect_precalc = NULL;
1810         }
1811 #else
1812         UNUSED_VARS(flag);
1813 #endif
1814 }
1815
1816 int BLI_bvhtree_ray_cast_ex(
1817         BVHTree *tree, const float co[3], const float dir[3], float radius, BVHTreeRayHit *hit,
1818         BVHTree_RayCastCallback callback, void *userdata,
1819         int flag)
1820 {
1821         BVHRayCastData data;
1822         BVHNode *root = tree->nodes[tree->totleaf];
1823
1824         BLI_ASSERT_UNIT_V3(dir);
1825
1826         data.tree = tree;
1827
1828         data.callback = callback;
1829         data.userdata = userdata;
1830
1831         copy_v3_v3(data.ray.origin,    co);
1832         copy_v3_v3(data.ray.direction, dir);
1833         data.ray.radius = radius;
1834
1835         bvhtree_ray_cast_data_precalc(&data, flag);
1836
1837         if (hit) {
1838                 memcpy(&data.hit, hit, sizeof(*hit));
1839         }
1840         else {
1841                 data.hit.index = -1;
1842                 data.hit.dist = BVH_RAYCAST_DIST_MAX;
1843         }
1844
1845         if (root) {
1846                 dfs_raycast(&data, root);
1847 //              iterative_raycast(&data, root);
1848         }
1849
1850
1851         if (hit)
1852                 memcpy(hit, &data.hit, sizeof(*hit));
1853
1854         return data.hit.index;
1855 }
1856
1857 int BLI_bvhtree_ray_cast(
1858         BVHTree *tree, const float co[3], const float dir[3], float radius, BVHTreeRayHit *hit,
1859         BVHTree_RayCastCallback callback, void *userdata)
1860 {
1861         return BLI_bvhtree_ray_cast_ex(tree, co, dir, radius, hit, callback, userdata, BVH_RAYCAST_DEFAULT);
1862 }
1863
1864 float BLI_bvhtree_bb_raycast(const float bv[6], const float light_start[3], const float light_end[3], float pos[3])
1865 {
1866         BVHRayCastData data;
1867         float dist;
1868
1869         data.hit.dist = BVH_RAYCAST_DIST_MAX;
1870         
1871         /* get light direction */
1872         sub_v3_v3v3(data.ray.direction, light_end, light_start);
1873         
1874         data.ray.radius = 0.0;
1875         
1876         copy_v3_v3(data.ray.origin, light_start);
1877
1878         normalize_v3(data.ray.direction);
1879         copy_v3_v3(data.ray_dot_axis, data.ray.direction);
1880         
1881         dist = ray_nearest_hit(&data, bv);
1882
1883         madd_v3_v3v3fl(pos, light_start, data.ray.direction, dist);
1884
1885         return dist;
1886         
1887 }
1888
1889 /**
1890  * Calls the callback for every ray intersection
1891  *
1892  * \note Using a \a callback which resets or never sets the #BVHTreeRayHit index & dist works too,
1893  * however using this function means existing generic callbacks can be used from custom callbacks without
1894  * having to handle resetting the hit beforehand.
1895  * It also avoid redundant argument and return value which aren't meaningful when collecting multiple hits.
1896  */
1897 void BLI_bvhtree_ray_cast_all_ex(
1898         BVHTree *tree, const float co[3], const float dir[3], float radius, float hit_dist,
1899         BVHTree_RayCastCallback callback, void *userdata,
1900         int flag)
1901 {
1902         BVHRayCastData data;
1903         BVHNode *root = tree->nodes[tree->totleaf];
1904
1905         BLI_ASSERT_UNIT_V3(dir);
1906         BLI_assert(callback != NULL);
1907
1908         data.tree = tree;
1909
1910         data.callback = callback;
1911         data.userdata = userdata;
1912
1913         copy_v3_v3(data.ray.origin,    co);
1914         copy_v3_v3(data.ray.direction, dir);
1915         data.ray.radius = radius;
1916
1917         bvhtree_ray_cast_data_precalc(&data, flag);
1918
1919         data.hit.index = -1;
1920         data.hit.dist = hit_dist;
1921
1922         if (root) {
1923                 dfs_raycast_all(&data, root);
1924         }
1925 }
1926
1927 void BLI_bvhtree_ray_cast_all(
1928         BVHTree *tree, const float co[3], const float dir[3], float radius, float hit_dist,
1929         BVHTree_RayCastCallback callback, void *userdata)
1930 {
1931         BLI_bvhtree_ray_cast_all_ex(tree, co, dir, radius, hit_dist, callback, userdata, BVH_RAYCAST_DEFAULT);
1932 }
1933
1934 /** \} */
1935
1936 /* -------------------------------------------------------------------- */
1937
1938 /** \name BLI_bvhtree_range_query
1939  *
1940  * Allocs and fills an array with the indexs of node that are on the given spherical range (center, radius).
1941  * Returns the size of the array.
1942  *
1943  * \{ */
1944
1945 typedef struct RangeQueryData {
1946         BVHTree *tree;
1947         const float *center;
1948         float radius_sq;  /* squared radius */
1949
1950         int hits;
1951
1952         BVHTree_RangeQuery callback;
1953         void *userdata;
1954 } RangeQueryData;
1955
1956
1957 static void dfs_range_query(RangeQueryData *data, BVHNode *node)
1958 {
1959         if (node->totnode == 0) {
1960 #if 0   /*UNUSED*/
1961                 /* Calculate the node min-coords (if the node was a point then this is the point coordinates) */
1962                 float co[3];
1963                 co[0] = node->bv[0];
1964                 co[1] = node->bv[2];
1965                 co[2] = node->bv[4];
1966 #endif
1967         }
1968         else {
1969                 int i;
1970                 for (i = 0; i != node->totnode; i++) {
1971                         float nearest[3];
1972                         float dist_sq = calc_nearest_point_squared(data->center, node->children[i], nearest);
1973                         if (dist_sq < data->radius_sq) {
1974                                 /* Its a leaf.. call the callback */
1975                                 if (node->children[i]->totnode == 0) {
1976                                         data->hits++;
1977                                         data->callback(data->userdata, node->children[i]->index, data->center, dist_sq);
1978                                 }
1979                                 else
1980                                         dfs_range_query(data, node->children[i]);
1981                         }
1982                 }
1983         }
1984 }
1985
1986 int BLI_bvhtree_range_query(
1987         BVHTree *tree, const float co[3], float radius,
1988         BVHTree_RangeQuery callback, void *userdata)
1989 {
1990         BVHNode *root = tree->nodes[tree->totleaf];
1991
1992         RangeQueryData data;
1993         data.tree = tree;
1994         data.center = co;
1995         data.radius_sq = radius * radius;
1996         data.hits = 0;
1997
1998         data.callback = callback;
1999         data.userdata = userdata;
2000
2001         if (root != NULL) {
2002                 float nearest[3];
2003                 float dist_sq = calc_nearest_point_squared(data.center, root, nearest);
2004                 if (dist_sq < data.radius_sq) {
2005                         /* Its a leaf.. call the callback */
2006                         if (root->totnode == 0) {
2007                                 data.hits++;
2008                                 data.callback(data.userdata, root->index, co, dist_sq);
2009                         }
2010                         else
2011                                 dfs_range_query(&data, root);
2012                 }
2013         }
2014
2015         return data.hits;
2016 }
2017
2018 /** \} */
2019
2020
2021 /* -------------------------------------------------------------------- */
2022
2023 /** \name BLI_bvhtree_walk_dfs
2024  * \{ */
2025
2026 typedef struct BVHTree_WalkData {
2027         BVHTree_WalkParentCallback walk_parent_cb;
2028         BVHTree_WalkLeafCallback walk_leaf_cb;
2029         BVHTree_WalkOrderCallback walk_order_cb;
2030         void *userdata;
2031 } BVHTree_WalkData;
2032
2033 /**
2034  * Runs first among nodes children of the first node before going to the next node in the same layer.
2035  *
2036  * \return false to break out of the search early.
2037  */
2038 static bool bvhtree_walk_dfs_recursive(
2039         BVHTree_WalkData *walk_data,
2040         const BVHNode *node)
2041 {
2042         if (node->totnode == 0) {
2043                 return walk_data->walk_leaf_cb((const BVHTreeAxisRange *)node->bv, node->index, walk_data->userdata);
2044         }
2045         else {
2046                 /* First pick the closest node to recurse into */
2047                 if (walk_data->walk_order_cb((const BVHTreeAxisRange *)node->bv, node->main_axis, walk_data->userdata)) {
2048                         for (int i = 0; i != node->totnode; i++) {
2049                                 if (walk_data->walk_parent_cb((const BVHTreeAxisRange *)node->children[i]->bv, walk_data->userdata)) {
2050                                         if (!bvhtree_walk_dfs_recursive(walk_data, node->children[i])) {
2051                                                 return false;
2052                                         }
2053                                 }
2054                         }
2055                 }
2056                 else {
2057                         for (int i = node->totnode - 1; i >= 0; i--) {
2058                                 if (walk_data->walk_parent_cb((const BVHTreeAxisRange *)node->children[i]->bv, walk_data->userdata)) {
2059                                         if (!bvhtree_walk_dfs_recursive(walk_data, node->children[i])) {
2060                                                 return false;
2061                                         }
2062                                 }
2063                         }
2064                 }
2065         }
2066         return true;
2067 }
2068
2069 /**
2070  * This is a generic function to perform a depth first search on the BVHTree
2071  * where the search order and nodes traversed depend on callbacks passed in.
2072  *
2073  * \param tree: Tree to walk.
2074  * \param walk_parent_cb: Callback on a parents bound-box to test if it should be traversed.
2075  * \param walk_leaf_cb: Callback to test leaf nodes, callback must store its own result,
2076  * returning false exits early.
2077  * \param walk_order_cb: Callback that indicates which direction to search,
2078  * either from the node with the lower or higher k-dop axis value.
2079  * \param userdata: Argument passed to all callbacks.
2080  */
2081 void BLI_bvhtree_walk_dfs(
2082         BVHTree *tree,
2083         BVHTree_WalkParentCallback walk_parent_cb,
2084         BVHTree_WalkLeafCallback walk_leaf_cb,
2085         BVHTree_WalkOrderCallback walk_order_cb, void *userdata)
2086 {
2087         const BVHNode *root = tree->nodes[tree->totleaf];
2088         if (root != NULL) {
2089                 BVHTree_WalkData walk_data = {walk_parent_cb, walk_leaf_cb, walk_order_cb, userdata};
2090                 /* first make sure the bv of root passes in the test too */
2091                 if (walk_parent_cb((const BVHTreeAxisRange *)root->bv, userdata)) {
2092                         bvhtree_walk_dfs_recursive(&walk_data, root);
2093                 }
2094         }
2095 }
2096
2097 /** \} */