c1187fdd8bb54c4e8fa9b1c0594f85c4ee15b786
[blender-staging.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 + 1);
861         }
862 }
863
864 /**
865  * This functions builds an optimal implicit tree from the given leafs.
866  * Where optimal stands for:
867  *  - The resulting tree will have the smallest number of branches;
868  *  - At most only one branch will have NULL childs;
869  *  - All leafs will be stored at level N or N+1.
870  *
871  * This function creates an implicit tree on branches_array, the leafs are given on the leafs_array.
872  *
873  * The tree is built per depth levels. First branches at depth 1.. then branches at depth 2.. etc..
874  * The reason is that we can build level N+1 from level N without any data dependencies.. thus it allows
875  * to use multithread building.
876  *
877  * To archive this is necessary to find how much leafs are accessible from a certain branch, BVHBuildHelper
878  * #implicit_needed_branches and #implicit_leafs_index are auxiliary functions to solve that "optimal-split".
879  */
880 static void non_recursive_bvh_div_nodes(
881         const BVHTree *tree, BVHNode *branches_array, BVHNode **leafs_array, int num_leafs)
882 {
883         int i;
884
885         const int tree_type   = tree->tree_type;
886         const int tree_offset = 2 - tree->tree_type; /* this value is 0 (on binary trees) and negative on the others */
887         const int num_branches = implicit_needed_branches(tree_type, num_leafs);
888
889         BVHBuildHelper data;
890         int depth;
891
892         {
893                 /* set parent from root node to NULL */
894                 BVHNode *root = &branches_array[1];
895                 root->parent = NULL;
896
897                 /* Most of bvhtree code relies on 1-leaf trees having at least one branch
898                  * We handle that special case here */
899                 if (num_leafs == 1) {
900                         refit_kdop_hull(tree, root, 0, num_leafs);
901                         root->main_axis = get_largest_axis(root->bv) / 2;
902                         root->totnode = 1;
903                         root->children[0] = leafs_array[0];
904                         root->children[0]->parent = root;
905                         return;
906                 }
907         }
908
909         build_implicit_tree_helper(tree, &data);
910
911         BVHDivNodesData cb_data = {
912                 .tree = tree, .branches_array = branches_array, .leafs_array = leafs_array,
913                 .tree_type = tree_type, .tree_offset = tree_offset, .data = &data,
914                 .first_of_next_level = 0, .depth = 0, .i = 0,
915         };
916
917         /* Loop tree levels (log N) loops */
918         for (i = 1, depth = 1; i <= num_branches; i = i * tree_type + tree_offset, depth++) {
919                 const int first_of_next_level = i * tree_type + tree_offset;
920                 const int i_stop = min_ii(first_of_next_level, num_branches + 1);  /* index of last branch on this level */
921
922                 /* Loop all branches on this level */
923                 cb_data.first_of_next_level = first_of_next_level;
924                 cb_data.i = i;
925                 cb_data.depth = depth;
926
927                 if (true) {
928                         ParallelRangeSettings settings;
929                         BLI_parallel_range_settings_defaults(&settings);
930                         settings.use_threading = (num_leafs > KDOPBVH_THREAD_LEAF_THRESHOLD);
931                         BLI_task_parallel_range(
932                                 i, i_stop,
933                                 &cb_data,
934                                 non_recursive_bvh_div_nodes_task_cb,
935                                 &settings);
936                 }
937                 else {
938                         /* Less hassle for debugging. */
939                         ParallelRangeTLS tls = {0};
940                         for (int i_task = i; i_task < i_stop; i_task++) {
941                                 non_recursive_bvh_div_nodes_task_cb(&cb_data, i_task, &tls);
942                         }
943                 }
944         }
945 }
946
947 /** \} */
948
949
950 /* -------------------------------------------------------------------- */
951
952 /** \name BLI_bvhtree API
953  * \{ */
954
955 /**
956  * \note many callers don't check for ``NULL`` return.
957  */
958 BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
959 {
960         BVHTree *tree;
961         int numnodes, i;
962
963         BLI_assert(tree_type >= 2 && tree_type <= MAX_TREETYPE);
964
965         tree = MEM_callocN(sizeof(BVHTree), "BVHTree");
966
967         /* tree epsilon must be >= FLT_EPSILON
968          * so that tangent rays can still hit a bounding volume..
969          * this bug would show up when casting a ray aligned with a kdop-axis and with an edge of 2 faces */
970         epsilon = max_ff(FLT_EPSILON, epsilon);
971
972         if (tree) {
973                 tree->epsilon = epsilon;
974                 tree->tree_type = tree_type;
975                 tree->axis = axis;
976
977                 if (axis == 26) {
978                         tree->start_axis = 0;
979                         tree->stop_axis = 13;
980                 }
981                 else if (axis == 18) {
982                         tree->start_axis = 7;
983                         tree->stop_axis = 13;
984                 }
985                 else if (axis == 14) {
986                         tree->start_axis = 0;
987                         tree->stop_axis = 7;
988                 }
989                 else if (axis == 8) { /* AABB */
990                         tree->start_axis = 0;
991                         tree->stop_axis = 4;
992                 }
993                 else if (axis == 6) { /* OBB */
994                         tree->start_axis = 0;
995                         tree->stop_axis = 3;
996                 }
997                 else {
998                         /* should never happen! */
999                         BLI_assert(0);
1000
1001                         goto fail;
1002                 }
1003
1004
1005                 /* Allocate arrays */
1006                 numnodes = maxsize + implicit_needed_branches(tree_type, maxsize) + tree_type;
1007
1008                 tree->nodes = MEM_callocN(sizeof(BVHNode *) * (size_t)numnodes, "BVHNodes");
1009                 tree->nodebv = MEM_callocN(sizeof(float) * (size_t)(axis * numnodes), "BVHNodeBV");
1010                 tree->nodechild = MEM_callocN(sizeof(BVHNode *) * (size_t)(tree_type * numnodes), "BVHNodeBV");
1011                 tree->nodearray = MEM_callocN(sizeof(BVHNode) * (size_t)numnodes, "BVHNodeArray");
1012                 
1013                 if (UNLIKELY((!tree->nodes) ||
1014                              (!tree->nodebv) ||
1015                              (!tree->nodechild) ||
1016                              (!tree->nodearray)))
1017                 {
1018                         goto fail;
1019                 }
1020
1021                 /* link the dynamic bv and child links */
1022                 for (i = 0; i < numnodes; i++) {
1023                         tree->nodearray[i].bv = &tree->nodebv[i * axis];
1024                         tree->nodearray[i].children = &tree->nodechild[i * tree_type];
1025                 }
1026                 
1027         }
1028         return tree;
1029
1030
1031 fail:
1032         MEM_SAFE_FREE(tree->nodes);
1033         MEM_SAFE_FREE(tree->nodebv);
1034         MEM_SAFE_FREE(tree->nodechild);
1035         MEM_SAFE_FREE(tree->nodearray);
1036
1037         MEM_freeN(tree);
1038
1039         return NULL;
1040 }
1041
1042 void BLI_bvhtree_free(BVHTree *tree)
1043 {
1044         if (tree) {
1045                 MEM_freeN(tree->nodes);
1046                 MEM_freeN(tree->nodearray);
1047                 MEM_freeN(tree->nodebv);
1048                 MEM_freeN(tree->nodechild);
1049                 MEM_freeN(tree);
1050         }
1051 }
1052
1053 void BLI_bvhtree_balance(BVHTree *tree)
1054 {
1055         BVHNode **leafs_array    = tree->nodes;
1056
1057         /* This function should only be called once
1058          * (some big bug goes here if its being called more than once per tree) */
1059         BLI_assert(tree->totbranch == 0);
1060
1061         /* Build the implicit tree */
1062         non_recursive_bvh_div_nodes(tree, tree->nodearray + (tree->totleaf - 1), leafs_array, tree->totleaf);
1063
1064         /* current code expects the branches to be linked to the nodes array
1065          * we perform that linkage here */
1066         tree->totbranch = implicit_needed_branches(tree->tree_type, tree->totleaf);
1067         for (int i = 0; i < tree->totbranch; i++) {
1068                 tree->nodes[tree->totleaf + i] = &tree->nodearray[tree->totleaf + i];
1069         }
1070
1071 #ifdef USE_SKIP_LINKS
1072         build_skip_links(tree, tree->nodes[tree->totleaf], NULL, NULL);
1073 #endif
1074
1075 #ifdef USE_VERIFY_TREE
1076         bvhtree_verify(tree);
1077 #endif
1078
1079 #ifdef USE_PRINT_TREE
1080         bvhtree_info(tree);
1081 #endif
1082 }
1083
1084 void BLI_bvhtree_insert(BVHTree *tree, int index, const float co[3], int numpoints)
1085 {
1086         axis_t axis_iter;
1087         BVHNode *node = NULL;
1088
1089         /* insert should only possible as long as tree->totbranch is 0 */
1090         BLI_assert(tree->totbranch <= 0);
1091         BLI_assert((size_t)tree->totleaf < MEM_allocN_len(tree->nodes) / sizeof(*(tree->nodes)));
1092
1093         node = tree->nodes[tree->totleaf] = &(tree->nodearray[tree->totleaf]);
1094         tree->totleaf++;
1095
1096         create_kdop_hull(tree, node, co, numpoints, 0);
1097         node->index = index;
1098
1099         /* inflate the bv with some epsilon */
1100         for (axis_iter = tree->start_axis; axis_iter < tree->stop_axis; axis_iter++) {
1101                 node->bv[(2 * axis_iter)] -= tree->epsilon; /* minimum */
1102                 node->bv[(2 * axis_iter) + 1] += tree->epsilon; /* maximum */
1103         }
1104 }
1105
1106
1107 /* call before BLI_bvhtree_update_tree() */
1108 bool BLI_bvhtree_update_node(BVHTree *tree, int index, const float co[3], const float co_moving[3], int numpoints)
1109 {
1110         BVHNode *node = NULL;
1111         axis_t axis_iter;
1112         
1113         /* check if index exists */
1114         if (index > tree->totleaf)
1115                 return false;
1116         
1117         node = tree->nodearray + index;
1118         
1119         create_kdop_hull(tree, node, co, numpoints, 0);
1120         
1121         if (co_moving)
1122                 create_kdop_hull(tree, node, co_moving, numpoints, 1);
1123         
1124         /* inflate the bv with some epsilon */
1125         for (axis_iter = tree->start_axis; axis_iter < tree->stop_axis; axis_iter++) {
1126                 node->bv[(2 * axis_iter)]     -= tree->epsilon; /* minimum */
1127                 node->bv[(2 * axis_iter) + 1] += tree->epsilon; /* maximum */
1128         }
1129
1130         return true;
1131 }
1132
1133 /* call BLI_bvhtree_update_node() first for every node/point/triangle */
1134 void BLI_bvhtree_update_tree(BVHTree *tree)
1135 {
1136         /* Update bottom=>top
1137          * TRICKY: the way we build the tree all the childs have an index greater than the parent
1138          * This allows us todo a bottom up update by starting on the bigger numbered branch */
1139
1140         BVHNode **root  = tree->nodes + tree->totleaf;
1141         BVHNode **index = tree->nodes + tree->totleaf + tree->totbranch - 1;
1142
1143         for (; index >= root; index--)
1144                 node_join(tree, *index);
1145 }
1146 /**
1147  * Number of times #BLI_bvhtree_insert has been called.
1148  * mainly useful for asserts functions to check we added the correct number.
1149  */
1150 int BLI_bvhtree_get_len(const BVHTree *tree)
1151 {
1152         return tree->totleaf;
1153 }
1154
1155 float BLI_bvhtree_get_epsilon(const BVHTree *tree)
1156 {
1157         return tree->epsilon;
1158 }
1159
1160 /** \} */
1161
1162
1163 /* -------------------------------------------------------------------- */
1164
1165 /** \name BLI_bvhtree_overlap
1166  * \{ */
1167
1168 /**
1169  * overlap - is it possible for 2 bv's to collide ?
1170  */
1171 static bool tree_overlap_test(const BVHNode *node1, const BVHNode *node2, axis_t start_axis, axis_t stop_axis)
1172 {
1173         const float *bv1     = node1->bv + (start_axis << 1);
1174         const float *bv2     = node2->bv + (start_axis << 1);
1175         const float *bv1_end = node1->bv + (stop_axis  << 1);
1176         
1177         /* test all axis if min + max overlap */
1178         for (; bv1 != bv1_end; bv1 += 2, bv2 += 2) {
1179                 if ((bv1[0] > bv2[1]) || (bv2[0] > bv1[1])) {
1180                         return 0;
1181                 }
1182         }
1183
1184         return 1;
1185 }
1186
1187 static void tree_overlap_traverse(
1188         BVHOverlapData_Thread *data_thread,
1189         const BVHNode *node1, const BVHNode *node2)
1190 {
1191         BVHOverlapData_Shared *data = data_thread->shared;
1192         int j;
1193
1194         if (tree_overlap_test(node1, node2, data->start_axis, data->stop_axis)) {
1195                 /* check if node1 is a leaf */
1196                 if (!node1->totnode) {
1197                         /* check if node2 is a leaf */
1198                         if (!node2->totnode) {
1199                                 BVHTreeOverlap *overlap;
1200
1201                                 if (UNLIKELY(node1 == node2)) {
1202                                         return;
1203                                 }
1204
1205                                 /* both leafs, insert overlap! */
1206                                 overlap = BLI_stack_push_r(data_thread->overlap);
1207                                 overlap->indexA = node1->index;
1208                                 overlap->indexB = node2->index;
1209                         }
1210                         else {
1211                                 for (j = 0; j < data->tree2->tree_type; j++) {
1212                                         if (node2->children[j]) {
1213                                                 tree_overlap_traverse(data_thread, node1, node2->children[j]);
1214                                         }
1215                                 }
1216                         }
1217                 }
1218                 else {
1219                         for (j = 0; j < data->tree2->tree_type; j++) {
1220                                 if (node1->children[j]) {
1221                                         tree_overlap_traverse(data_thread, node1->children[j], node2);
1222                                 }
1223                         }
1224                 }
1225         }
1226 }
1227
1228 /**
1229  * a version of #tree_overlap_traverse that runs a callback to check if the nodes really intersect.
1230  */
1231 static void tree_overlap_traverse_cb(
1232         BVHOverlapData_Thread *data_thread,
1233         const BVHNode *node1, const BVHNode *node2)
1234 {
1235         BVHOverlapData_Shared *data = data_thread->shared;
1236         int j;
1237
1238         if (tree_overlap_test(node1, node2, data->start_axis, data->stop_axis)) {
1239                 /* check if node1 is a leaf */
1240                 if (!node1->totnode) {
1241                         /* check if node2 is a leaf */
1242                         if (!node2->totnode) {
1243                                 BVHTreeOverlap *overlap;
1244
1245                                 if (UNLIKELY(node1 == node2)) {
1246                                         return;
1247                                 }
1248
1249                                 /* only difference to tree_overlap_traverse! */
1250                                 if (data->callback(data->userdata, node1->index, node2->index, data_thread->thread)) {
1251                                         /* both leafs, insert overlap! */
1252                                         overlap = BLI_stack_push_r(data_thread->overlap);
1253                                         overlap->indexA = node1->index;
1254                                         overlap->indexB = node2->index;
1255                                 }
1256                         }
1257                         else {
1258                                 for (j = 0; j < data->tree2->tree_type; j++) {
1259                                         if (node2->children[j]) {
1260                                                 tree_overlap_traverse_cb(data_thread, node1, node2->children[j]);
1261                                         }
1262                                 }
1263                         }
1264                 }
1265                 else {
1266                         for (j = 0; j < data->tree2->tree_type; j++) {
1267                                 if (node1->children[j]) {
1268                                         tree_overlap_traverse_cb(data_thread, node1->children[j], node2);
1269                                 }
1270                         }
1271                 }
1272         }
1273 }
1274
1275 /**
1276  * Use to check the total number of threads #BLI_bvhtree_overlap will use.
1277  *
1278  * \warning Must be the first tree passed to #BLI_bvhtree_overlap!
1279  */
1280 int BLI_bvhtree_overlap_thread_num(const BVHTree *tree)
1281 {
1282         return (int)MIN2(tree->tree_type, tree->nodes[tree->totleaf]->totnode);
1283 }
1284
1285 static void bvhtree_overlap_task_cb(
1286         void *__restrict userdata,
1287         const int j,
1288         const ParallelRangeTLS *__restrict UNUSED(tls))
1289 {
1290         BVHOverlapData_Thread *data = &((BVHOverlapData_Thread *)userdata)[j];
1291         BVHOverlapData_Shared *data_shared = data->shared;
1292
1293         if (data_shared->callback) {
1294                 tree_overlap_traverse_cb(
1295                             data, data_shared->tree1->nodes[data_shared->tree1->totleaf]->children[j],
1296                             data_shared->tree2->nodes[data_shared->tree2->totleaf]);
1297         }
1298         else {
1299                 tree_overlap_traverse(
1300                             data, data_shared->tree1->nodes[data_shared->tree1->totleaf]->children[j],
1301                             data_shared->tree2->nodes[data_shared->tree2->totleaf]);
1302         }
1303 }
1304
1305 BVHTreeOverlap *BLI_bvhtree_overlap(
1306         const BVHTree *tree1, const BVHTree *tree2, uint *r_overlap_tot,
1307         /* optional callback to test the overlap before adding (must be thread-safe!) */
1308         BVHTree_OverlapCallback callback, void *userdata)
1309 {
1310         const int thread_num = BLI_bvhtree_overlap_thread_num(tree1);
1311         int j;
1312         size_t total = 0;
1313         BVHTreeOverlap *overlap = NULL, *to = NULL;
1314         BVHOverlapData_Shared data_shared;
1315         BVHOverlapData_Thread *data = BLI_array_alloca(data, (size_t)thread_num);
1316         axis_t start_axis, stop_axis;
1317         
1318         /* check for compatibility of both trees (can't compare 14-DOP with 18-DOP) */
1319         if (UNLIKELY((tree1->axis != tree2->axis) &&
1320                      (tree1->axis == 14 || tree2->axis == 14) &&
1321                      (tree1->axis == 18 || tree2->axis == 18)))
1322         {
1323                 BLI_assert(0);
1324                 return NULL;
1325         }
1326
1327         start_axis = min_axis(tree1->start_axis, tree2->start_axis);
1328         stop_axis  = min_axis(tree1->stop_axis,  tree2->stop_axis);
1329         
1330         /* fast check root nodes for collision before doing big splitting + traversal */
1331         if (!tree_overlap_test(tree1->nodes[tree1->totleaf], tree2->nodes[tree2->totleaf], start_axis, stop_axis)) {
1332                 return NULL;
1333         }
1334
1335         data_shared.tree1 = tree1;
1336         data_shared.tree2 = tree2;
1337         data_shared.start_axis = start_axis;
1338         data_shared.stop_axis = stop_axis;
1339
1340         /* can be NULL */
1341         data_shared.callback = callback;
1342         data_shared.userdata = userdata;
1343
1344         for (j = 0; j < thread_num; j++) {
1345                 /* init BVHOverlapData_Thread */
1346                 data[j].shared = &data_shared;
1347                 data[j].overlap = BLI_stack_new(sizeof(BVHTreeOverlap), __func__);
1348
1349                 /* for callback */
1350                 data[j].thread = j;
1351         }
1352
1353         ParallelRangeSettings settings;
1354         BLI_parallel_range_settings_defaults(&settings);
1355         settings.use_threading = (tree1->totleaf > KDOPBVH_THREAD_LEAF_THRESHOLD);
1356         BLI_task_parallel_range(
1357                     0, thread_num,
1358                     data,
1359                     bvhtree_overlap_task_cb,
1360                     &settings);
1361         
1362         for (j = 0; j < thread_num; j++)
1363                 total += BLI_stack_count(data[j].overlap);
1364         
1365         to = overlap = MEM_mallocN(sizeof(BVHTreeOverlap) * total, "BVHTreeOverlap");
1366         
1367         for (j = 0; j < thread_num; j++) {
1368                 uint count = (uint)BLI_stack_count(data[j].overlap);
1369                 BLI_stack_pop_n(data[j].overlap, to, count);
1370                 BLI_stack_free(data[j].overlap);
1371                 to += count;
1372         }
1373
1374         *r_overlap_tot = (uint)total;
1375         return overlap;
1376 }
1377
1378 /** \} */
1379
1380
1381 /* -------------------------------------------------------------------- */
1382
1383 /** \name BLI_bvhtree_find_nearest
1384  * \{ */
1385
1386 /* Determines the nearest point of the given node BV. Returns the squared distance to that point. */
1387 static float calc_nearest_point_squared(const float proj[3], BVHNode *node, float nearest[3])
1388 {
1389         int i;
1390         const float *bv = node->bv;
1391
1392         /* nearest on AABB hull */
1393         for (i = 0; i != 3; i++, bv += 2) {
1394                 if (bv[0] > proj[i])
1395                         nearest[i] = bv[0];
1396                 else if (bv[1] < proj[i])
1397                         nearest[i] = bv[1];
1398                 else
1399                         nearest[i] = proj[i]; 
1400         }
1401
1402 #if 0
1403         /* nearest on a general hull */
1404         copy_v3_v3(nearest, data->co);
1405         for (i = data->tree->start_axis; i != data->tree->stop_axis; i++, bv += 2) {
1406                 float proj = dot_v3v3(nearest, bvhtree_kdop_axes[i]);
1407                 float dl = bv[0] - proj;
1408                 float du = bv[1] - proj;
1409
1410                 if (dl > 0) {
1411                         madd_v3_v3fl(nearest, bvhtree_kdop_axes[i], dl);
1412                 }
1413                 else if (du < 0) {
1414                         madd_v3_v3fl(nearest, bvhtree_kdop_axes[i], du);
1415                 }
1416         }
1417 #endif
1418
1419         return len_squared_v3v3(proj, nearest);
1420 }
1421
1422 /* TODO: use a priority queue to reduce the number of nodes looked on */
1423 static void dfs_find_nearest_dfs(BVHNearestData *data, BVHNode *node)
1424 {
1425         if (node->totnode == 0) {
1426                 if (data->callback)
1427                         data->callback(data->userdata, node->index, data->co, &data->nearest);
1428                 else {
1429                         data->nearest.index = node->index;
1430                         data->nearest.dist_sq = calc_nearest_point_squared(data->proj, node, data->nearest.co);
1431                 }
1432         }
1433         else {
1434                 /* Better heuristic to pick the closest node to dive on */
1435                 int i;
1436                 float nearest[3];
1437
1438                 if (data->proj[node->main_axis] <= node->children[0]->bv[node->main_axis * 2 + 1]) {
1439
1440                         for (i = 0; i != node->totnode; i++) {
1441                                 if (calc_nearest_point_squared(data->proj, node->children[i], nearest) >= data->nearest.dist_sq)
1442                                         continue;
1443                                 dfs_find_nearest_dfs(data, node->children[i]);
1444                         }
1445                 }
1446                 else {
1447                         for (i = node->totnode - 1; i >= 0; 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         }
1454 }
1455
1456 static void dfs_find_nearest_begin(BVHNearestData *data, BVHNode *node)
1457 {
1458         float nearest[3], dist_sq;
1459         dist_sq = calc_nearest_point_squared(data->proj, node, nearest);
1460         if (dist_sq >= data->nearest.dist_sq) {
1461                 return;
1462         }
1463         dfs_find_nearest_dfs(data, node);
1464 }
1465
1466
1467 #if 0
1468
1469 typedef struct NodeDistance {
1470         BVHNode *node;
1471         float dist;
1472
1473 } NodeDistance;
1474
1475 #define DEFAULT_FIND_NEAREST_HEAP_SIZE 1024
1476
1477 #define NodeDistance_priority(a, b) ((a).dist < (b).dist)
1478
1479 static void NodeDistance_push_heap(NodeDistance *heap, int heap_size)
1480 PUSH_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size)
1481
1482 static void NodeDistance_pop_heap(NodeDistance *heap, int heap_size)
1483 POP_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size)
1484
1485 /* NN function that uses an heap.. this functions leads to an optimal number of min-distance
1486  * but for normal tri-faces and BV 6-dop.. a simple dfs with local heuristics (as implemented
1487  * in source/blender/blenkernel/intern/shrinkwrap.c) works faster.
1488  *
1489  * It may make sense to use this function if the callback queries are very slow.. or if its impossible
1490  * to get a nice heuristic
1491  *
1492  * this function uses "malloc/free" instead of the MEM_* because it intends to be thread safe */
1493 static void bfs_find_nearest(BVHNearestData *data, BVHNode *node)
1494 {
1495         int i;
1496         NodeDistance default_heap[DEFAULT_FIND_NEAREST_HEAP_SIZE];
1497         NodeDistance *heap = default_heap, current;
1498         int heap_size = 0, max_heap_size = sizeof(default_heap) / sizeof(default_heap[0]);
1499         float nearest[3];
1500
1501         int callbacks = 0, push_heaps = 0;
1502
1503         if (node->totnode == 0) {
1504                 dfs_find_nearest_dfs(data, node);
1505                 return;
1506         }
1507
1508         current.node = node;
1509         current.dist = calc_nearest_point(data->proj, node, nearest);
1510
1511         while (current.dist < data->nearest.dist) {
1512 //              printf("%f : %f\n", current.dist, data->nearest.dist);
1513                 for (i = 0; i < current.node->totnode; i++) {
1514                         BVHNode *child = current.node->children[i];
1515                         if (child->totnode == 0) {
1516                                 callbacks++;
1517                                 dfs_find_nearest_dfs(data, child);
1518                         }
1519                         else {
1520                                 /* adjust heap size */
1521                                 if ((heap_size >= max_heap_size) &&
1522                                     ADJUST_MEMORY(default_heap, (void **)&heap,
1523                                                   heap_size + 1, &max_heap_size, sizeof(heap[0])) == false)
1524                                 {
1525                                         printf("WARNING: bvh_find_nearest got out of memory\n");
1526
1527                                         if (heap != default_heap)
1528                                                 free(heap);
1529
1530                                         return;
1531                                 }
1532
1533                                 heap[heap_size].node = current.node->children[i];
1534                                 heap[heap_size].dist = calc_nearest_point(data->proj, current.node->children[i], nearest);
1535
1536                                 if (heap[heap_size].dist >= data->nearest.dist) continue;
1537                                 heap_size++;
1538
1539                                 NodeDistance_push_heap(heap, heap_size);
1540                                 //                      PUSH_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size);
1541                                 push_heaps++;
1542                         }
1543                 }
1544                 
1545                 if (heap_size == 0) break;
1546
1547                 current = heap[0];
1548                 NodeDistance_pop_heap(heap, heap_size);
1549 //              POP_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size);
1550                 heap_size--;
1551         }
1552
1553 //      printf("hsize=%d, callbacks=%d, pushs=%d\n", heap_size, callbacks, push_heaps);
1554
1555         if (heap != default_heap)
1556                 free(heap);
1557 }
1558 #endif
1559
1560
1561 int BLI_bvhtree_find_nearest(
1562         BVHTree *tree, const float co[3], BVHTreeNearest *nearest,
1563         BVHTree_NearestPointCallback callback, void *userdata)
1564 {
1565         axis_t axis_iter;
1566
1567         BVHNearestData data;
1568         BVHNode *root = tree->nodes[tree->totleaf];
1569
1570         /* init data to search */
1571         data.tree = tree;
1572         data.co = co;
1573
1574         data.callback = callback;
1575         data.userdata = userdata;
1576
1577         for (axis_iter = data.tree->start_axis; axis_iter != data.tree->stop_axis; axis_iter++) {
1578                 data.proj[axis_iter] = dot_v3v3(data.co, bvhtree_kdop_axes[axis_iter]);
1579         }
1580
1581         if (nearest) {
1582                 memcpy(&data.nearest, nearest, sizeof(*nearest));
1583         }
1584         else {
1585                 data.nearest.index = -1;
1586                 data.nearest.dist_sq = FLT_MAX;
1587         }
1588
1589         /* dfs search */
1590         if (root)
1591                 dfs_find_nearest_begin(&data, root);
1592
1593         /* copy back results */
1594         if (nearest) {
1595                 memcpy(nearest, &data.nearest, sizeof(*nearest));
1596         }
1597
1598         return data.nearest.index;
1599 }
1600
1601 /** \} */
1602
1603
1604 /* -------------------------------------------------------------------- */
1605
1606 /** \name BLI_bvhtree_ray_cast
1607  *
1608  * raycast is done by performing a DFS on the BVHTree and saving the closest hit.
1609  *
1610  * \{ */
1611
1612
1613 /* Determines the distance that the ray must travel to hit the bounding volume of the given node */
1614 static float ray_nearest_hit(const BVHRayCastData *data, const float bv[6])
1615 {
1616         int i;
1617
1618         float low = 0, upper = data->hit.dist;
1619
1620         for (i = 0; i != 3; i++, bv += 2) {
1621                 if (data->ray_dot_axis[i] == 0.0f) {
1622                         /* axis aligned ray */
1623                         if (data->ray.origin[i] < bv[0] - data->ray.radius ||
1624                             data->ray.origin[i] > bv[1] + data->ray.radius)
1625                         {
1626                                 return FLT_MAX;
1627                         }
1628                 }
1629                 else {
1630                         float ll = (bv[0] - data->ray.radius - data->ray.origin[i]) / data->ray_dot_axis[i];
1631                         float lu = (bv[1] + data->ray.radius - data->ray.origin[i]) / data->ray_dot_axis[i];
1632
1633                         if (data->ray_dot_axis[i] > 0.0f) {
1634                                 if (ll > low) low = ll;
1635                                 if (lu < upper) upper = lu;
1636                         }
1637                         else {
1638                                 if (lu > low) low = lu;
1639                                 if (ll < upper) upper = ll;
1640                         }
1641         
1642                         if (low > upper) return FLT_MAX;
1643                 }
1644         }
1645         return low;
1646 }
1647
1648 /**
1649  * Determines the distance that the ray must travel to hit the bounding volume of the given node
1650  * Based on Tactical Optimization of Ray/Box Intersection, by Graham Fyffe
1651  * [http://tog.acm.org/resources/RTNews/html/rtnv21n1.html#art9]
1652  *
1653  * TODO this doesn't take data->ray.radius into consideration */
1654 static float fast_ray_nearest_hit(const BVHRayCastData *data, const BVHNode *node)
1655 {
1656         const float *bv = node->bv;
1657         
1658         float t1x = (bv[data->index[0]] - data->ray.origin[0]) * data->idot_axis[0];
1659         float t2x = (bv[data->index[1]] - data->ray.origin[0]) * data->idot_axis[0];
1660         float t1y = (bv[data->index[2]] - data->ray.origin[1]) * data->idot_axis[1];
1661         float t2y = (bv[data->index[3]] - data->ray.origin[1]) * data->idot_axis[1];
1662         float t1z = (bv[data->index[4]] - data->ray.origin[2]) * data->idot_axis[2];
1663         float t2z = (bv[data->index[5]] - data->ray.origin[2]) * data->idot_axis[2];
1664
1665         if ((t1x > t2y || t2x < t1y || t1x > t2z || t2x < t1z || t1y > t2z || t2y < t1z) ||
1666             (t2x < 0.0f || t2y < 0.0f || t2z < 0.0f) ||
1667             (t1x > data->hit.dist || t1y > data->hit.dist || t1z > data->hit.dist))
1668         {
1669                 return FLT_MAX;
1670         }
1671         else {
1672                 return max_fff(t1x, t1y, t1z);
1673         }
1674 }
1675
1676 static void dfs_raycast(BVHRayCastData *data, BVHNode *node)
1677 {
1678         int i;
1679
1680         /* ray-bv is really fast.. and simple tests revealed its worth to test it
1681          * before calling the ray-primitive functions */
1682         /* XXX: temporary solution for particles until fast_ray_nearest_hit supports ray.radius */
1683         float dist = (data->ray.radius == 0.0f) ? fast_ray_nearest_hit(data, node) : ray_nearest_hit(data, node->bv);
1684         if (dist >= data->hit.dist) {
1685                 return;
1686         }
1687
1688         if (node->totnode == 0) {
1689                 if (data->callback) {
1690                         data->callback(data->userdata, node->index, &data->ray, &data->hit);
1691                 }
1692                 else {
1693                         data->hit.index = node->index;
1694                         data->hit.dist  = dist;
1695                         madd_v3_v3v3fl(data->hit.co, data->ray.origin, data->ray.direction, dist);
1696                 }
1697         }
1698         else {
1699                 /* pick loop direction to dive into the tree (based on ray direction and split axis) */
1700                 if (data->ray_dot_axis[node->main_axis] > 0.0f) {
1701                         for (i = 0; i != node->totnode; i++) {
1702                                 dfs_raycast(data, node->children[i]);
1703                         }
1704                 }
1705                 else {
1706                         for (i = node->totnode - 1; i >= 0; i--) {
1707                                 dfs_raycast(data, node->children[i]);
1708                         }
1709                 }
1710         }
1711 }
1712
1713 /**
1714  * A version of #dfs_raycast with minor changes to reset the index & dist each ray cast.
1715  */
1716 static void dfs_raycast_all(BVHRayCastData *data, BVHNode *node)
1717 {
1718         int i;
1719
1720         /* ray-bv is really fast.. and simple tests revealed its worth to test it
1721          * before calling the ray-primitive functions */
1722         /* XXX: temporary solution for particles until fast_ray_nearest_hit supports ray.radius */
1723         float dist = (data->ray.radius == 0.0f) ? fast_ray_nearest_hit(data, node) : ray_nearest_hit(data, node->bv);
1724         if (dist >= data->hit.dist) {
1725                 return;
1726         }
1727
1728         if (node->totnode == 0) {
1729                 /* no need to check for 'data->callback' (using 'all' only makes sense with a callback). */
1730                 dist = data->hit.dist;
1731                 data->callback(data->userdata, node->index, &data->ray, &data->hit);
1732                 data->hit.index = -1;
1733                 data->hit.dist = dist;
1734         }
1735         else {
1736                 /* pick loop direction to dive into the tree (based on ray direction and split axis) */
1737                 if (data->ray_dot_axis[node->main_axis] > 0.0f) {
1738                         for (i = 0; i != node->totnode; i++) {
1739                                 dfs_raycast_all(data, node->children[i]);
1740                         }
1741                 }
1742                 else {
1743                         for (i = node->totnode - 1; i >= 0; i--) {
1744                                 dfs_raycast_all(data, node->children[i]);
1745                         }
1746                 }
1747         }
1748 }
1749
1750 #if 0
1751 static void iterative_raycast(BVHRayCastData *data, BVHNode *node)
1752 {
1753         while (node) {
1754                 float dist = fast_ray_nearest_hit(data, node);
1755                 if (dist >= data->hit.dist) {
1756                         node = node->skip[1];
1757                         continue;
1758                 }
1759
1760                 if (node->totnode == 0) {
1761                         if (data->callback) {
1762                                 data->callback(data->userdata, node->index, &data->ray, &data->hit);
1763                         }
1764                         else {
1765                                 data->hit.index = node->index;
1766                                 data->hit.dist  = dist;
1767                                 madd_v3_v3v3fl(data->hit.co, data->ray.origin, data->ray.direction, dist);
1768                         }
1769                         
1770                         node = node->skip[1];
1771                 }
1772                 else {
1773                         node = node->children[0];
1774                 }
1775         }
1776 }
1777 #endif
1778
1779 static void bvhtree_ray_cast_data_precalc(BVHRayCastData *data, int flag)
1780 {
1781         int i;
1782
1783         for (i = 0; i < 3; i++) {
1784                 data->ray_dot_axis[i] = dot_v3v3(data->ray.direction, bvhtree_kdop_axes[i]);
1785                 data->idot_axis[i] = 1.0f / data->ray_dot_axis[i];
1786
1787                 if (fabsf(data->ray_dot_axis[i]) < FLT_EPSILON) {
1788                         data->ray_dot_axis[i] = 0.0;
1789                 }
1790                 data->index[2 * i] = data->idot_axis[i] < 0.0f ? 1 : 0;
1791                 data->index[2 * i + 1] = 1 - data->index[2 * i];
1792                 data->index[2 * i]   += 2 * i;
1793                 data->index[2 * i + 1] += 2 * i;
1794         }
1795
1796 #ifdef USE_KDOPBVH_WATERTIGHT
1797         if (flag & BVH_RAYCAST_WATERTIGHT) {
1798                 isect_ray_tri_watertight_v3_precalc(&data->isect_precalc, data->ray.direction);
1799                 data->ray.isect_precalc = &data->isect_precalc;
1800         }
1801         else {
1802                 data->ray.isect_precalc = NULL;
1803         }
1804 #else
1805         UNUSED_VARS(flag);
1806 #endif
1807 }
1808
1809 int BLI_bvhtree_ray_cast_ex(
1810         BVHTree *tree, const float co[3], const float dir[3], float radius, BVHTreeRayHit *hit,
1811         BVHTree_RayCastCallback callback, void *userdata,
1812         int flag)
1813 {
1814         BVHRayCastData data;
1815         BVHNode *root = tree->nodes[tree->totleaf];
1816
1817         BLI_ASSERT_UNIT_V3(dir);
1818
1819         data.tree = tree;
1820
1821         data.callback = callback;
1822         data.userdata = userdata;
1823
1824         copy_v3_v3(data.ray.origin,    co);
1825         copy_v3_v3(data.ray.direction, dir);
1826         data.ray.radius = radius;
1827
1828         bvhtree_ray_cast_data_precalc(&data, flag);
1829
1830         if (hit) {
1831                 memcpy(&data.hit, hit, sizeof(*hit));
1832         }
1833         else {
1834                 data.hit.index = -1;
1835                 data.hit.dist = BVH_RAYCAST_DIST_MAX;
1836         }
1837
1838         if (root) {
1839                 dfs_raycast(&data, root);
1840 //              iterative_raycast(&data, root);
1841         }
1842
1843
1844         if (hit)
1845                 memcpy(hit, &data.hit, sizeof(*hit));
1846
1847         return data.hit.index;
1848 }
1849
1850 int BLI_bvhtree_ray_cast(
1851         BVHTree *tree, const float co[3], const float dir[3], float radius, BVHTreeRayHit *hit,
1852         BVHTree_RayCastCallback callback, void *userdata)
1853 {
1854         return BLI_bvhtree_ray_cast_ex(tree, co, dir, radius, hit, callback, userdata, BVH_RAYCAST_DEFAULT);
1855 }
1856
1857 float BLI_bvhtree_bb_raycast(const float bv[6], const float light_start[3], const float light_end[3], float pos[3])
1858 {
1859         BVHRayCastData data;
1860         float dist;
1861
1862         data.hit.dist = BVH_RAYCAST_DIST_MAX;
1863         
1864         /* get light direction */
1865         sub_v3_v3v3(data.ray.direction, light_end, light_start);
1866         
1867         data.ray.radius = 0.0;
1868         
1869         copy_v3_v3(data.ray.origin, light_start);
1870
1871         normalize_v3(data.ray.direction);
1872         copy_v3_v3(data.ray_dot_axis, data.ray.direction);
1873         
1874         dist = ray_nearest_hit(&data, bv);
1875
1876         madd_v3_v3v3fl(pos, light_start, data.ray.direction, dist);
1877
1878         return dist;
1879         
1880 }
1881
1882 /**
1883  * Calls the callback for every ray intersection
1884  *
1885  * \note Using a \a callback which resets or never sets the #BVHTreeRayHit index & dist works too,
1886  * however using this function means existing generic callbacks can be used from custom callbacks without
1887  * having to handle resetting the hit beforehand.
1888  * It also avoid redundant argument and return value which aren't meaningful when collecting multiple hits.
1889  */
1890 void BLI_bvhtree_ray_cast_all_ex(
1891         BVHTree *tree, const float co[3], const float dir[3], float radius, float hit_dist,
1892         BVHTree_RayCastCallback callback, void *userdata,
1893         int flag)
1894 {
1895         BVHRayCastData data;
1896         BVHNode *root = tree->nodes[tree->totleaf];
1897
1898         BLI_ASSERT_UNIT_V3(dir);
1899         BLI_assert(callback != NULL);
1900
1901         data.tree = tree;
1902
1903         data.callback = callback;
1904         data.userdata = userdata;
1905
1906         copy_v3_v3(data.ray.origin,    co);
1907         copy_v3_v3(data.ray.direction, dir);
1908         data.ray.radius = radius;
1909
1910         bvhtree_ray_cast_data_precalc(&data, flag);
1911
1912         data.hit.index = -1;
1913         data.hit.dist = hit_dist;
1914
1915         if (root) {
1916                 dfs_raycast_all(&data, root);
1917         }
1918 }
1919
1920 void BLI_bvhtree_ray_cast_all(
1921         BVHTree *tree, const float co[3], const float dir[3], float radius, float hit_dist,
1922         BVHTree_RayCastCallback callback, void *userdata)
1923 {
1924         BLI_bvhtree_ray_cast_all_ex(tree, co, dir, radius, hit_dist, callback, userdata, BVH_RAYCAST_DEFAULT);
1925 }
1926
1927 /** \} */
1928
1929 /* -------------------------------------------------------------------- */
1930
1931 /** \name BLI_bvhtree_range_query
1932  *
1933  * Allocs and fills an array with the indexs of node that are on the given spherical range (center, radius).
1934  * Returns the size of the array.
1935  *
1936  * \{ */
1937
1938 typedef struct RangeQueryData {
1939         BVHTree *tree;
1940         const float *center;
1941         float radius_sq;  /* squared radius */
1942
1943         int hits;
1944
1945         BVHTree_RangeQuery callback;
1946         void *userdata;
1947 } RangeQueryData;
1948
1949
1950 static void dfs_range_query(RangeQueryData *data, BVHNode *node)
1951 {
1952         if (node->totnode == 0) {
1953 #if 0   /*UNUSED*/
1954                 /* Calculate the node min-coords (if the node was a point then this is the point coordinates) */
1955                 float co[3];
1956                 co[0] = node->bv[0];
1957                 co[1] = node->bv[2];
1958                 co[2] = node->bv[4];
1959 #endif
1960         }
1961         else {
1962                 int i;
1963                 for (i = 0; i != node->totnode; i++) {
1964                         float nearest[3];
1965                         float dist_sq = calc_nearest_point_squared(data->center, node->children[i], nearest);
1966                         if (dist_sq < data->radius_sq) {
1967                                 /* Its a leaf.. call the callback */
1968                                 if (node->children[i]->totnode == 0) {
1969                                         data->hits++;
1970                                         data->callback(data->userdata, node->children[i]->index, data->center, dist_sq);
1971                                 }
1972                                 else
1973                                         dfs_range_query(data, node->children[i]);
1974                         }
1975                 }
1976         }
1977 }
1978
1979 int BLI_bvhtree_range_query(
1980         BVHTree *tree, const float co[3], float radius,
1981         BVHTree_RangeQuery callback, void *userdata)
1982 {
1983         BVHNode *root = tree->nodes[tree->totleaf];
1984
1985         RangeQueryData data;
1986         data.tree = tree;
1987         data.center = co;
1988         data.radius_sq = radius * radius;
1989         data.hits = 0;
1990
1991         data.callback = callback;
1992         data.userdata = userdata;
1993
1994         if (root != NULL) {
1995                 float nearest[3];
1996                 float dist_sq = calc_nearest_point_squared(data.center, root, nearest);
1997                 if (dist_sq < data.radius_sq) {
1998                         /* Its a leaf.. call the callback */
1999                         if (root->totnode == 0) {
2000                                 data.hits++;
2001                                 data.callback(data.userdata, root->index, co, dist_sq);
2002                         }
2003                         else
2004                                 dfs_range_query(&data, root);
2005                 }
2006         }
2007
2008         return data.hits;
2009 }
2010
2011 /** \} */
2012
2013
2014 /* -------------------------------------------------------------------- */
2015
2016 /** \name BLI_bvhtree_walk_dfs
2017  * \{ */
2018
2019 /**
2020  * Runs first among nodes children of the first node before going to the next node in the same layer.
2021  *
2022  * \return false to break out of the search early.
2023  */
2024 static bool bvhtree_walk_dfs_recursive(
2025         BVHTree_WalkParentCallback walk_parent_cb,
2026         BVHTree_WalkLeafCallback walk_leaf_cb,
2027         BVHTree_WalkOrderCallback walk_order_cb,
2028         const BVHNode *node, void *userdata)
2029 {
2030         if (node->totnode == 0) {
2031                 return walk_leaf_cb((const BVHTreeAxisRange *)node->bv, node->index, userdata);
2032         }
2033         else {
2034                 /* First pick the closest node to recurse into */
2035                 if (walk_order_cb((const BVHTreeAxisRange *)node->bv, node->main_axis, userdata)) {
2036                         for (int i = 0; i != node->totnode; i++) {
2037                                 if (walk_parent_cb((const BVHTreeAxisRange *)node->children[i]->bv, userdata)) {
2038                                         if (!bvhtree_walk_dfs_recursive(
2039                                                 walk_parent_cb, walk_leaf_cb, walk_order_cb,
2040                                                 node->children[i], userdata))
2041                                         {
2042                                                 return false;
2043                                         }
2044                                 }
2045                         }
2046                 }
2047                 else {
2048                         for (int i = node->totnode - 1; i >= 0; i--) {
2049                                 if (walk_parent_cb((const BVHTreeAxisRange *)node->children[i]->bv, userdata)) {
2050                                         if (!bvhtree_walk_dfs_recursive(
2051                                                 walk_parent_cb, walk_leaf_cb, walk_order_cb,
2052                                                 node->children[i], userdata))
2053                                         {
2054                                                 return false;
2055                                         }
2056                                 }
2057                         }
2058                 }
2059         }
2060         return true;
2061 }
2062
2063 /**
2064  * This is a generic function to perform a depth first search on the BVHTree
2065  * where the search order and nodes traversed depend on callbacks passed in.
2066  *
2067  * \param tree: Tree to walk.
2068  * \param walk_parent_cb: Callback on a parents bound-box to test if it should be traversed.
2069  * \param walk_leaf_cb: Callback to test leaf nodes, callback must store its own result,
2070  * returning false exits early.
2071  * \param walk_order_cb: Callback that indicates which direction to search,
2072  * either from the node with the lower or higher k-dop axis value.
2073  * \param userdata: Argument passed to all callbacks.
2074  */
2075 void BLI_bvhtree_walk_dfs(
2076         BVHTree *tree,
2077         BVHTree_WalkParentCallback walk_parent_cb,
2078         BVHTree_WalkLeafCallback walk_leaf_cb,
2079         BVHTree_WalkOrderCallback walk_order_cb, void *userdata)
2080 {
2081         const BVHNode *root = tree->nodes[tree->totleaf];
2082         if (root != NULL) {
2083                 /* first make sure the bv of root passes in the test too */
2084                 if (walk_parent_cb((const BVHTreeAxisRange *)root->bv, userdata)) {
2085                         bvhtree_walk_dfs_recursive(walk_parent_cb, walk_leaf_cb, walk_order_cb, root, userdata);
2086                 }
2087         }
2088 }
2089
2090 /** \} */