Cleanup: kdopbvh, only set parent nodes once
[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;
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 float BLI_bvhtree_get_epsilon(const BVHTree *tree)
1155 {
1156         return tree->epsilon;
1157 }
1158
1159 /** \} */
1160
1161
1162 /* -------------------------------------------------------------------- */
1163
1164 /** \name BLI_bvhtree_overlap
1165  * \{ */
1166
1167 /**
1168  * overlap - is it possible for 2 bv's to collide ?
1169  */
1170 static bool tree_overlap_test(const BVHNode *node1, const BVHNode *node2, axis_t start_axis, axis_t stop_axis)
1171 {
1172         const float *bv1     = node1->bv + (start_axis << 1);
1173         const float *bv2     = node2->bv + (start_axis << 1);
1174         const float *bv1_end = node1->bv + (stop_axis  << 1);
1175         
1176         /* test all axis if min + max overlap */
1177         for (; bv1 != bv1_end; bv1 += 2, bv2 += 2) {
1178                 if ((bv1[0] > bv2[1]) || (bv2[0] > bv1[1])) {
1179                         return 0;
1180                 }
1181         }
1182
1183         return 1;
1184 }
1185
1186 static void tree_overlap_traverse(
1187         BVHOverlapData_Thread *data_thread,
1188         const BVHNode *node1, const BVHNode *node2)
1189 {
1190         BVHOverlapData_Shared *data = data_thread->shared;
1191         int j;
1192
1193         if (tree_overlap_test(node1, node2, data->start_axis, data->stop_axis)) {
1194                 /* check if node1 is a leaf */
1195                 if (!node1->totnode) {
1196                         /* check if node2 is a leaf */
1197                         if (!node2->totnode) {
1198                                 BVHTreeOverlap *overlap;
1199
1200                                 if (UNLIKELY(node1 == node2)) {
1201                                         return;
1202                                 }
1203
1204                                 /* both leafs, insert overlap! */
1205                                 overlap = BLI_stack_push_r(data_thread->overlap);
1206                                 overlap->indexA = node1->index;
1207                                 overlap->indexB = node2->index;
1208                         }
1209                         else {
1210                                 for (j = 0; j < data->tree2->tree_type; j++) {
1211                                         if (node2->children[j]) {
1212                                                 tree_overlap_traverse(data_thread, node1, node2->children[j]);
1213                                         }
1214                                 }
1215                         }
1216                 }
1217                 else {
1218                         for (j = 0; j < data->tree2->tree_type; j++) {
1219                                 if (node1->children[j]) {
1220                                         tree_overlap_traverse(data_thread, node1->children[j], node2);
1221                                 }
1222                         }
1223                 }
1224         }
1225 }
1226
1227 /**
1228  * a version of #tree_overlap_traverse that runs a callback to check if the nodes really intersect.
1229  */
1230 static void tree_overlap_traverse_cb(
1231         BVHOverlapData_Thread *data_thread,
1232         const BVHNode *node1, const BVHNode *node2)
1233 {
1234         BVHOverlapData_Shared *data = data_thread->shared;
1235         int j;
1236
1237         if (tree_overlap_test(node1, node2, data->start_axis, data->stop_axis)) {
1238                 /* check if node1 is a leaf */
1239                 if (!node1->totnode) {
1240                         /* check if node2 is a leaf */
1241                         if (!node2->totnode) {
1242                                 BVHTreeOverlap *overlap;
1243
1244                                 if (UNLIKELY(node1 == node2)) {
1245                                         return;
1246                                 }
1247
1248                                 /* only difference to tree_overlap_traverse! */
1249                                 if (data->callback(data->userdata, node1->index, node2->index, data_thread->thread)) {
1250                                         /* both leafs, insert overlap! */
1251                                         overlap = BLI_stack_push_r(data_thread->overlap);
1252                                         overlap->indexA = node1->index;
1253                                         overlap->indexB = node2->index;
1254                                 }
1255                         }
1256                         else {
1257                                 for (j = 0; j < data->tree2->tree_type; j++) {
1258                                         if (node2->children[j]) {
1259                                                 tree_overlap_traverse_cb(data_thread, node1, node2->children[j]);
1260                                         }
1261                                 }
1262                         }
1263                 }
1264                 else {
1265                         for (j = 0; j < data->tree2->tree_type; j++) {
1266                                 if (node1->children[j]) {
1267                                         tree_overlap_traverse_cb(data_thread, node1->children[j], node2);
1268                                 }
1269                         }
1270                 }
1271         }
1272 }
1273
1274 /**
1275  * Use to check the total number of threads #BLI_bvhtree_overlap will use.
1276  *
1277  * \warning Must be the first tree passed to #BLI_bvhtree_overlap!
1278  */
1279 int BLI_bvhtree_overlap_thread_num(const BVHTree *tree)
1280 {
1281         return (int)MIN2(tree->tree_type, tree->nodes[tree->totleaf]->totnode);
1282 }
1283
1284 static void bvhtree_overlap_task_cb(
1285         void *__restrict userdata,
1286         const int j,
1287         const ParallelRangeTLS *__restrict UNUSED(tls))
1288 {
1289         BVHOverlapData_Thread *data = &((BVHOverlapData_Thread *)userdata)[j];
1290         BVHOverlapData_Shared *data_shared = data->shared;
1291
1292         if (data_shared->callback) {
1293                 tree_overlap_traverse_cb(
1294                             data, data_shared->tree1->nodes[data_shared->tree1->totleaf]->children[j],
1295                             data_shared->tree2->nodes[data_shared->tree2->totleaf]);
1296         }
1297         else {
1298                 tree_overlap_traverse(
1299                             data, data_shared->tree1->nodes[data_shared->tree1->totleaf]->children[j],
1300                             data_shared->tree2->nodes[data_shared->tree2->totleaf]);
1301         }
1302 }
1303
1304 BVHTreeOverlap *BLI_bvhtree_overlap(
1305         const BVHTree *tree1, const BVHTree *tree2, uint *r_overlap_tot,
1306         /* optional callback to test the overlap before adding (must be thread-safe!) */
1307         BVHTree_OverlapCallback callback, void *userdata)
1308 {
1309         const int thread_num = BLI_bvhtree_overlap_thread_num(tree1);
1310         int j;
1311         size_t total = 0;
1312         BVHTreeOverlap *overlap = NULL, *to = NULL;
1313         BVHOverlapData_Shared data_shared;
1314         BVHOverlapData_Thread *data = BLI_array_alloca(data, (size_t)thread_num);
1315         axis_t start_axis, stop_axis;
1316         
1317         /* check for compatibility of both trees (can't compare 14-DOP with 18-DOP) */
1318         if (UNLIKELY((tree1->axis != tree2->axis) &&
1319                      (tree1->axis == 14 || tree2->axis == 14) &&
1320                      (tree1->axis == 18 || tree2->axis == 18)))
1321         {
1322                 BLI_assert(0);
1323                 return NULL;
1324         }
1325
1326         start_axis = min_axis(tree1->start_axis, tree2->start_axis);
1327         stop_axis  = min_axis(tree1->stop_axis,  tree2->stop_axis);
1328         
1329         /* fast check root nodes for collision before doing big splitting + traversal */
1330         if (!tree_overlap_test(tree1->nodes[tree1->totleaf], tree2->nodes[tree2->totleaf], start_axis, stop_axis)) {
1331                 return NULL;
1332         }
1333
1334         data_shared.tree1 = tree1;
1335         data_shared.tree2 = tree2;
1336         data_shared.start_axis = start_axis;
1337         data_shared.stop_axis = stop_axis;
1338
1339         /* can be NULL */
1340         data_shared.callback = callback;
1341         data_shared.userdata = userdata;
1342
1343         for (j = 0; j < thread_num; j++) {
1344                 /* init BVHOverlapData_Thread */
1345                 data[j].shared = &data_shared;
1346                 data[j].overlap = BLI_stack_new(sizeof(BVHTreeOverlap), __func__);
1347
1348                 /* for callback */
1349                 data[j].thread = j;
1350         }
1351
1352         ParallelRangeSettings settings;
1353         BLI_parallel_range_settings_defaults(&settings);
1354         settings.use_threading = (tree1->totleaf > KDOPBVH_THREAD_LEAF_THRESHOLD);
1355         BLI_task_parallel_range(
1356                     0, thread_num,
1357                     data,
1358                     bvhtree_overlap_task_cb,
1359                     &settings);
1360         
1361         for (j = 0; j < thread_num; j++)
1362                 total += BLI_stack_count(data[j].overlap);
1363         
1364         to = overlap = MEM_mallocN(sizeof(BVHTreeOverlap) * total, "BVHTreeOverlap");
1365         
1366         for (j = 0; j < thread_num; j++) {
1367                 uint count = (uint)BLI_stack_count(data[j].overlap);
1368                 BLI_stack_pop_n(data[j].overlap, to, count);
1369                 BLI_stack_free(data[j].overlap);
1370                 to += count;
1371         }
1372
1373         *r_overlap_tot = (uint)total;
1374         return overlap;
1375 }
1376
1377 /** \} */
1378
1379
1380 /* -------------------------------------------------------------------- */
1381
1382 /** \name BLI_bvhtree_find_nearest
1383  * \{ */
1384
1385 /* Determines the nearest point of the given node BV. Returns the squared distance to that point. */
1386 static float calc_nearest_point_squared(const float proj[3], BVHNode *node, float nearest[3])
1387 {
1388         int i;
1389         const float *bv = node->bv;
1390
1391         /* nearest on AABB hull */
1392         for (i = 0; i != 3; i++, bv += 2) {
1393                 if (bv[0] > proj[i])
1394                         nearest[i] = bv[0];
1395                 else if (bv[1] < proj[i])
1396                         nearest[i] = bv[1];
1397                 else
1398                         nearest[i] = proj[i]; 
1399         }
1400
1401 #if 0
1402         /* nearest on a general hull */
1403         copy_v3_v3(nearest, data->co);
1404         for (i = data->tree->start_axis; i != data->tree->stop_axis; i++, bv += 2) {
1405                 float proj = dot_v3v3(nearest, bvhtree_kdop_axes[i]);
1406                 float dl = bv[0] - proj;
1407                 float du = bv[1] - proj;
1408
1409                 if (dl > 0) {
1410                         madd_v3_v3fl(nearest, bvhtree_kdop_axes[i], dl);
1411                 }
1412                 else if (du < 0) {
1413                         madd_v3_v3fl(nearest, bvhtree_kdop_axes[i], du);
1414                 }
1415         }
1416 #endif
1417
1418         return len_squared_v3v3(proj, nearest);
1419 }
1420
1421 /* TODO: use a priority queue to reduce the number of nodes looked on */
1422 static void dfs_find_nearest_dfs(BVHNearestData *data, BVHNode *node)
1423 {
1424         if (node->totnode == 0) {
1425                 if (data->callback)
1426                         data->callback(data->userdata, node->index, data->co, &data->nearest);
1427                 else {
1428                         data->nearest.index = node->index;
1429                         data->nearest.dist_sq = calc_nearest_point_squared(data->proj, node, data->nearest.co);
1430                 }
1431         }
1432         else {
1433                 /* Better heuristic to pick the closest node to dive on */
1434                 int i;
1435                 float nearest[3];
1436
1437                 if (data->proj[node->main_axis] <= node->children[0]->bv[node->main_axis * 2 + 1]) {
1438
1439                         for (i = 0; i != node->totnode; i++) {
1440                                 if (calc_nearest_point_squared(data->proj, node->children[i], nearest) >= data->nearest.dist_sq)
1441                                         continue;
1442                                 dfs_find_nearest_dfs(data, node->children[i]);
1443                         }
1444                 }
1445                 else {
1446                         for (i = node->totnode - 1; i >= 0; i--) {
1447                                 if (calc_nearest_point_squared(data->proj, node->children[i], nearest) >= data->nearest.dist_sq)
1448                                         continue;
1449                                 dfs_find_nearest_dfs(data, node->children[i]);
1450                         }
1451                 }
1452         }
1453 }
1454
1455 static void dfs_find_nearest_begin(BVHNearestData *data, BVHNode *node)
1456 {
1457         float nearest[3], dist_sq;
1458         dist_sq = calc_nearest_point_squared(data->proj, node, nearest);
1459         if (dist_sq >= data->nearest.dist_sq) {
1460                 return;
1461         }
1462         dfs_find_nearest_dfs(data, node);
1463 }
1464
1465
1466 #if 0
1467
1468 typedef struct NodeDistance {
1469         BVHNode *node;
1470         float dist;
1471
1472 } NodeDistance;
1473
1474 #define DEFAULT_FIND_NEAREST_HEAP_SIZE 1024
1475
1476 #define NodeDistance_priority(a, b) ((a).dist < (b).dist)
1477
1478 static void NodeDistance_push_heap(NodeDistance *heap, int heap_size)
1479 PUSH_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size)
1480
1481 static void NodeDistance_pop_heap(NodeDistance *heap, int heap_size)
1482 POP_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size)
1483
1484 /* NN function that uses an heap.. this functions leads to an optimal number of min-distance
1485  * but for normal tri-faces and BV 6-dop.. a simple dfs with local heuristics (as implemented
1486  * in source/blender/blenkernel/intern/shrinkwrap.c) works faster.
1487  *
1488  * It may make sense to use this function if the callback queries are very slow.. or if its impossible
1489  * to get a nice heuristic
1490  *
1491  * this function uses "malloc/free" instead of the MEM_* because it intends to be thread safe */
1492 static void bfs_find_nearest(BVHNearestData *data, BVHNode *node)
1493 {
1494         int i;
1495         NodeDistance default_heap[DEFAULT_FIND_NEAREST_HEAP_SIZE];
1496         NodeDistance *heap = default_heap, current;
1497         int heap_size = 0, max_heap_size = sizeof(default_heap) / sizeof(default_heap[0]);
1498         float nearest[3];
1499
1500         int callbacks = 0, push_heaps = 0;
1501
1502         if (node->totnode == 0) {
1503                 dfs_find_nearest_dfs(data, node);
1504                 return;
1505         }
1506
1507         current.node = node;
1508         current.dist = calc_nearest_point(data->proj, node, nearest);
1509
1510         while (current.dist < data->nearest.dist) {
1511 //              printf("%f : %f\n", current.dist, data->nearest.dist);
1512                 for (i = 0; i < current.node->totnode; i++) {
1513                         BVHNode *child = current.node->children[i];
1514                         if (child->totnode == 0) {
1515                                 callbacks++;
1516                                 dfs_find_nearest_dfs(data, child);
1517                         }
1518                         else {
1519                                 /* adjust heap size */
1520                                 if ((heap_size >= max_heap_size) &&
1521                                     ADJUST_MEMORY(default_heap, (void **)&heap,
1522                                                   heap_size + 1, &max_heap_size, sizeof(heap[0])) == false)
1523                                 {
1524                                         printf("WARNING: bvh_find_nearest got out of memory\n");
1525
1526                                         if (heap != default_heap)
1527                                                 free(heap);
1528
1529                                         return;
1530                                 }
1531
1532                                 heap[heap_size].node = current.node->children[i];
1533                                 heap[heap_size].dist = calc_nearest_point(data->proj, current.node->children[i], nearest);
1534
1535                                 if (heap[heap_size].dist >= data->nearest.dist) continue;
1536                                 heap_size++;
1537
1538                                 NodeDistance_push_heap(heap, heap_size);
1539                                 //                      PUSH_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size);
1540                                 push_heaps++;
1541                         }
1542                 }
1543                 
1544                 if (heap_size == 0) break;
1545
1546                 current = heap[0];
1547                 NodeDistance_pop_heap(heap, heap_size);
1548 //              POP_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size);
1549                 heap_size--;
1550         }
1551
1552 //      printf("hsize=%d, callbacks=%d, pushs=%d\n", heap_size, callbacks, push_heaps);
1553
1554         if (heap != default_heap)
1555                 free(heap);
1556 }
1557 #endif
1558
1559
1560 int BLI_bvhtree_find_nearest(
1561         BVHTree *tree, const float co[3], BVHTreeNearest *nearest,
1562         BVHTree_NearestPointCallback callback, void *userdata)
1563 {
1564         axis_t axis_iter;
1565
1566         BVHNearestData data;
1567         BVHNode *root = tree->nodes[tree->totleaf];
1568
1569         /* init data to search */
1570         data.tree = tree;
1571         data.co = co;
1572
1573         data.callback = callback;
1574         data.userdata = userdata;
1575
1576         for (axis_iter = data.tree->start_axis; axis_iter != data.tree->stop_axis; axis_iter++) {
1577                 data.proj[axis_iter] = dot_v3v3(data.co, bvhtree_kdop_axes[axis_iter]);
1578         }
1579
1580         if (nearest) {
1581                 memcpy(&data.nearest, nearest, sizeof(*nearest));
1582         }
1583         else {
1584                 data.nearest.index = -1;
1585                 data.nearest.dist_sq = FLT_MAX;
1586         }
1587
1588         /* dfs search */
1589         if (root)
1590                 dfs_find_nearest_begin(&data, root);
1591
1592         /* copy back results */
1593         if (nearest) {
1594                 memcpy(nearest, &data.nearest, sizeof(*nearest));
1595         }
1596
1597         return data.nearest.index;
1598 }
1599
1600 /** \} */
1601
1602
1603 /* -------------------------------------------------------------------- */
1604
1605 /** \name BLI_bvhtree_ray_cast
1606  *
1607  * raycast is done by performing a DFS on the BVHTree and saving the closest hit.
1608  *
1609  * \{ */
1610
1611
1612 /* Determines the distance that the ray must travel to hit the bounding volume of the given node */
1613 static float ray_nearest_hit(const BVHRayCastData *data, const float bv[6])
1614 {
1615         int i;
1616
1617         float low = 0, upper = data->hit.dist;
1618
1619         for (i = 0; i != 3; i++, bv += 2) {
1620                 if (data->ray_dot_axis[i] == 0.0f) {
1621                         /* axis aligned ray */
1622                         if (data->ray.origin[i] < bv[0] - data->ray.radius ||
1623                             data->ray.origin[i] > bv[1] + data->ray.radius)
1624                         {
1625                                 return FLT_MAX;
1626                         }
1627                 }
1628                 else {
1629                         float ll = (bv[0] - data->ray.radius - data->ray.origin[i]) / data->ray_dot_axis[i];
1630                         float lu = (bv[1] + data->ray.radius - data->ray.origin[i]) / data->ray_dot_axis[i];
1631
1632                         if (data->ray_dot_axis[i] > 0.0f) {
1633                                 if (ll > low) low = ll;
1634                                 if (lu < upper) upper = lu;
1635                         }
1636                         else {
1637                                 if (lu > low) low = lu;
1638                                 if (ll < upper) upper = ll;
1639                         }
1640         
1641                         if (low > upper) return FLT_MAX;
1642                 }
1643         }
1644         return low;
1645 }
1646
1647 /**
1648  * Determines the distance that the ray must travel to hit the bounding volume of the given node
1649  * Based on Tactical Optimization of Ray/Box Intersection, by Graham Fyffe
1650  * [http://tog.acm.org/resources/RTNews/html/rtnv21n1.html#art9]
1651  *
1652  * TODO this doesn't take data->ray.radius into consideration */
1653 static float fast_ray_nearest_hit(const BVHRayCastData *data, const BVHNode *node)
1654 {
1655         const float *bv = node->bv;
1656         
1657         float t1x = (bv[data->index[0]] - data->ray.origin[0]) * data->idot_axis[0];
1658         float t2x = (bv[data->index[1]] - data->ray.origin[0]) * data->idot_axis[0];
1659         float t1y = (bv[data->index[2]] - data->ray.origin[1]) * data->idot_axis[1];
1660         float t2y = (bv[data->index[3]] - data->ray.origin[1]) * data->idot_axis[1];
1661         float t1z = (bv[data->index[4]] - data->ray.origin[2]) * data->idot_axis[2];
1662         float t2z = (bv[data->index[5]] - data->ray.origin[2]) * data->idot_axis[2];
1663
1664         if ((t1x > t2y || t2x < t1y || t1x > t2z || t2x < t1z || t1y > t2z || t2y < t1z) ||
1665             (t2x < 0.0f || t2y < 0.0f || t2z < 0.0f) ||
1666             (t1x > data->hit.dist || t1y > data->hit.dist || t1z > data->hit.dist))
1667         {
1668                 return FLT_MAX;
1669         }
1670         else {
1671                 return max_fff(t1x, t1y, t1z);
1672         }
1673 }
1674
1675 static void dfs_raycast(BVHRayCastData *data, BVHNode *node)
1676 {
1677         int i;
1678
1679         /* ray-bv is really fast.. and simple tests revealed its worth to test it
1680          * before calling the ray-primitive functions */
1681         /* XXX: temporary solution for particles until fast_ray_nearest_hit supports ray.radius */
1682         float dist = (data->ray.radius == 0.0f) ? fast_ray_nearest_hit(data, node) : ray_nearest_hit(data, node->bv);
1683         if (dist >= data->hit.dist) {
1684                 return;
1685         }
1686
1687         if (node->totnode == 0) {
1688                 if (data->callback) {
1689                         data->callback(data->userdata, node->index, &data->ray, &data->hit);
1690                 }
1691                 else {
1692                         data->hit.index = node->index;
1693                         data->hit.dist  = dist;
1694                         madd_v3_v3v3fl(data->hit.co, data->ray.origin, data->ray.direction, dist);
1695                 }
1696         }
1697         else {
1698                 /* pick loop direction to dive into the tree (based on ray direction and split axis) */
1699                 if (data->ray_dot_axis[node->main_axis] > 0.0f) {
1700                         for (i = 0; i != node->totnode; i++) {
1701                                 dfs_raycast(data, node->children[i]);
1702                         }
1703                 }
1704                 else {
1705                         for (i = node->totnode - 1; i >= 0; i--) {
1706                                 dfs_raycast(data, node->children[i]);
1707                         }
1708                 }
1709         }
1710 }
1711
1712 /**
1713  * A version of #dfs_raycast with minor changes to reset the index & dist each ray cast.
1714  */
1715 static void dfs_raycast_all(BVHRayCastData *data, BVHNode *node)
1716 {
1717         int i;
1718
1719         /* ray-bv is really fast.. and simple tests revealed its worth to test it
1720          * before calling the ray-primitive functions */
1721         /* XXX: temporary solution for particles until fast_ray_nearest_hit supports ray.radius */
1722         float dist = (data->ray.radius == 0.0f) ? fast_ray_nearest_hit(data, node) : ray_nearest_hit(data, node->bv);
1723         if (dist >= data->hit.dist) {
1724                 return;
1725         }
1726
1727         if (node->totnode == 0) {
1728                 /* no need to check for 'data->callback' (using 'all' only makes sense with a callback). */
1729                 dist = data->hit.dist;
1730                 data->callback(data->userdata, node->index, &data->ray, &data->hit);
1731                 data->hit.index = -1;
1732                 data->hit.dist = dist;
1733         }
1734         else {
1735                 /* pick loop direction to dive into the tree (based on ray direction and split axis) */
1736                 if (data->ray_dot_axis[node->main_axis] > 0.0f) {
1737                         for (i = 0; i != node->totnode; i++) {
1738                                 dfs_raycast_all(data, node->children[i]);
1739                         }
1740                 }
1741                 else {
1742                         for (i = node->totnode - 1; i >= 0; i--) {
1743                                 dfs_raycast_all(data, node->children[i]);
1744                         }
1745                 }
1746         }
1747 }
1748
1749 #if 0
1750 static void iterative_raycast(BVHRayCastData *data, BVHNode *node)
1751 {
1752         while (node) {
1753                 float dist = fast_ray_nearest_hit(data, node);
1754                 if (dist >= data->hit.dist) {
1755                         node = node->skip[1];
1756                         continue;
1757                 }
1758
1759                 if (node->totnode == 0) {
1760                         if (data->callback) {
1761                                 data->callback(data->userdata, node->index, &data->ray, &data->hit);
1762                         }
1763                         else {
1764                                 data->hit.index = node->index;
1765                                 data->hit.dist  = dist;
1766                                 madd_v3_v3v3fl(data->hit.co, data->ray.origin, data->ray.direction, dist);
1767                         }
1768                         
1769                         node = node->skip[1];
1770                 }
1771                 else {
1772                         node = node->children[0];
1773                 }
1774         }
1775 }
1776 #endif
1777
1778 static void bvhtree_ray_cast_data_precalc(BVHRayCastData *data, int flag)
1779 {
1780         int i;
1781
1782         for (i = 0; i < 3; i++) {
1783                 data->ray_dot_axis[i] = dot_v3v3(data->ray.direction, bvhtree_kdop_axes[i]);
1784                 data->idot_axis[i] = 1.0f / data->ray_dot_axis[i];
1785
1786                 if (fabsf(data->ray_dot_axis[i]) < FLT_EPSILON) {
1787                         data->ray_dot_axis[i] = 0.0;
1788                 }
1789                 data->index[2 * i] = data->idot_axis[i] < 0.0f ? 1 : 0;
1790                 data->index[2 * i + 1] = 1 - data->index[2 * i];
1791                 data->index[2 * i]   += 2 * i;
1792                 data->index[2 * i + 1] += 2 * i;
1793         }
1794
1795 #ifdef USE_KDOPBVH_WATERTIGHT
1796         if (flag & BVH_RAYCAST_WATERTIGHT) {
1797                 isect_ray_tri_watertight_v3_precalc(&data->isect_precalc, data->ray.direction);
1798                 data->ray.isect_precalc = &data->isect_precalc;
1799         }
1800         else {
1801                 data->ray.isect_precalc = NULL;
1802         }
1803 #else
1804         UNUSED_VARS(flag);
1805 #endif
1806 }
1807
1808 int BLI_bvhtree_ray_cast_ex(
1809         BVHTree *tree, const float co[3], const float dir[3], float radius, BVHTreeRayHit *hit,
1810         BVHTree_RayCastCallback callback, void *userdata,
1811         int flag)
1812 {
1813         BVHRayCastData data;
1814         BVHNode *root = tree->nodes[tree->totleaf];
1815
1816         BLI_ASSERT_UNIT_V3(dir);
1817
1818         data.tree = tree;
1819
1820         data.callback = callback;
1821         data.userdata = userdata;
1822
1823         copy_v3_v3(data.ray.origin,    co);
1824         copy_v3_v3(data.ray.direction, dir);
1825         data.ray.radius = radius;
1826
1827         bvhtree_ray_cast_data_precalc(&data, flag);
1828
1829         if (hit) {
1830                 memcpy(&data.hit, hit, sizeof(*hit));
1831         }
1832         else {
1833                 data.hit.index = -1;
1834                 data.hit.dist = BVH_RAYCAST_DIST_MAX;
1835         }
1836
1837         if (root) {
1838                 dfs_raycast(&data, root);
1839 //              iterative_raycast(&data, root);
1840         }
1841
1842
1843         if (hit)
1844                 memcpy(hit, &data.hit, sizeof(*hit));
1845
1846         return data.hit.index;
1847 }
1848
1849 int BLI_bvhtree_ray_cast(
1850         BVHTree *tree, const float co[3], const float dir[3], float radius, BVHTreeRayHit *hit,
1851         BVHTree_RayCastCallback callback, void *userdata)
1852 {
1853         return BLI_bvhtree_ray_cast_ex(tree, co, dir, radius, hit, callback, userdata, BVH_RAYCAST_DEFAULT);
1854 }
1855
1856 float BLI_bvhtree_bb_raycast(const float bv[6], const float light_start[3], const float light_end[3], float pos[3])
1857 {
1858         BVHRayCastData data;
1859         float dist;
1860
1861         data.hit.dist = BVH_RAYCAST_DIST_MAX;
1862         
1863         /* get light direction */
1864         sub_v3_v3v3(data.ray.direction, light_end, light_start);
1865         
1866         data.ray.radius = 0.0;
1867         
1868         copy_v3_v3(data.ray.origin, light_start);
1869
1870         normalize_v3(data.ray.direction);
1871         copy_v3_v3(data.ray_dot_axis, data.ray.direction);
1872         
1873         dist = ray_nearest_hit(&data, bv);
1874
1875         madd_v3_v3v3fl(pos, light_start, data.ray.direction, dist);
1876
1877         return dist;
1878         
1879 }
1880
1881 /**
1882  * Calls the callback for every ray intersection
1883  *
1884  * \note Using a \a callback which resets or never sets the #BVHTreeRayHit index & dist works too,
1885  * however using this function means existing generic callbacks can be used from custom callbacks without
1886  * having to handle resetting the hit beforehand.
1887  * It also avoid redundant argument and return value which aren't meaningful when collecting multiple hits.
1888  */
1889 void BLI_bvhtree_ray_cast_all_ex(
1890         BVHTree *tree, const float co[3], const float dir[3], float radius, float hit_dist,
1891         BVHTree_RayCastCallback callback, void *userdata,
1892         int flag)
1893 {
1894         BVHRayCastData data;
1895         BVHNode *root = tree->nodes[tree->totleaf];
1896
1897         BLI_ASSERT_UNIT_V3(dir);
1898         BLI_assert(callback != NULL);
1899
1900         data.tree = tree;
1901
1902         data.callback = callback;
1903         data.userdata = userdata;
1904
1905         copy_v3_v3(data.ray.origin,    co);
1906         copy_v3_v3(data.ray.direction, dir);
1907         data.ray.radius = radius;
1908
1909         bvhtree_ray_cast_data_precalc(&data, flag);
1910
1911         data.hit.index = -1;
1912         data.hit.dist = hit_dist;
1913
1914         if (root) {
1915                 dfs_raycast_all(&data, root);
1916         }
1917 }
1918
1919 void BLI_bvhtree_ray_cast_all(
1920         BVHTree *tree, const float co[3], const float dir[3], float radius, float hit_dist,
1921         BVHTree_RayCastCallback callback, void *userdata)
1922 {
1923         BLI_bvhtree_ray_cast_all_ex(tree, co, dir, radius, hit_dist, callback, userdata, BVH_RAYCAST_DEFAULT);
1924 }
1925
1926 /** \} */
1927
1928 /* -------------------------------------------------------------------- */
1929
1930 /** \name BLI_bvhtree_range_query
1931  *
1932  * Allocs and fills an array with the indexs of node that are on the given spherical range (center, radius).
1933  * Returns the size of the array.
1934  *
1935  * \{ */
1936
1937 typedef struct RangeQueryData {
1938         BVHTree *tree;
1939         const float *center;
1940         float radius_sq;  /* squared radius */
1941
1942         int hits;
1943
1944         BVHTree_RangeQuery callback;
1945         void *userdata;
1946 } RangeQueryData;
1947
1948
1949 static void dfs_range_query(RangeQueryData *data, BVHNode *node)
1950 {
1951         if (node->totnode == 0) {
1952 #if 0   /*UNUSED*/
1953                 /* Calculate the node min-coords (if the node was a point then this is the point coordinates) */
1954                 float co[3];
1955                 co[0] = node->bv[0];
1956                 co[1] = node->bv[2];
1957                 co[2] = node->bv[4];
1958 #endif
1959         }
1960         else {
1961                 int i;
1962                 for (i = 0; i != node->totnode; i++) {
1963                         float nearest[3];
1964                         float dist_sq = calc_nearest_point_squared(data->center, node->children[i], nearest);
1965                         if (dist_sq < data->radius_sq) {
1966                                 /* Its a leaf.. call the callback */
1967                                 if (node->children[i]->totnode == 0) {
1968                                         data->hits++;
1969                                         data->callback(data->userdata, node->children[i]->index, data->center, dist_sq);
1970                                 }
1971                                 else
1972                                         dfs_range_query(data, node->children[i]);
1973                         }
1974                 }
1975         }
1976 }
1977
1978 int BLI_bvhtree_range_query(
1979         BVHTree *tree, const float co[3], float radius,
1980         BVHTree_RangeQuery callback, void *userdata)
1981 {
1982         BVHNode *root = tree->nodes[tree->totleaf];
1983
1984         RangeQueryData data;
1985         data.tree = tree;
1986         data.center = co;
1987         data.radius_sq = radius * radius;
1988         data.hits = 0;
1989
1990         data.callback = callback;
1991         data.userdata = userdata;
1992
1993         if (root != NULL) {
1994                 float nearest[3];
1995                 float dist_sq = calc_nearest_point_squared(data.center, root, nearest);
1996                 if (dist_sq < data.radius_sq) {
1997                         /* Its a leaf.. call the callback */
1998                         if (root->totnode == 0) {
1999                                 data.hits++;
2000                                 data.callback(data.userdata, root->index, co, dist_sq);
2001                         }
2002                         else
2003                                 dfs_range_query(&data, root);
2004                 }
2005         }
2006
2007         return data.hits;
2008 }
2009
2010 /** \} */
2011
2012
2013 /* -------------------------------------------------------------------- */
2014
2015 /** \name BLI_bvhtree_walk_dfs
2016  * \{ */
2017
2018 /**
2019  * Runs first among nodes children of the first node before going to the next node in the same layer.
2020  *
2021  * \return false to break out of the search early.
2022  */
2023 static bool bvhtree_walk_dfs_recursive(
2024         BVHTree_WalkParentCallback walk_parent_cb,
2025         BVHTree_WalkLeafCallback walk_leaf_cb,
2026         BVHTree_WalkOrderCallback walk_order_cb,
2027         const BVHNode *node, void *userdata)
2028 {
2029         if (node->totnode == 0) {
2030                 return walk_leaf_cb((const BVHTreeAxisRange *)node->bv, node->index, userdata);
2031         }
2032         else {
2033                 /* First pick the closest node to recurse into */
2034                 if (walk_order_cb((const BVHTreeAxisRange *)node->bv, node->main_axis, userdata)) {
2035                         for (int i = 0; i != node->totnode; i++) {
2036                                 if (walk_parent_cb((const BVHTreeAxisRange *)node->children[i]->bv, userdata)) {
2037                                         if (!bvhtree_walk_dfs_recursive(
2038                                                 walk_parent_cb, walk_leaf_cb, walk_order_cb,
2039                                                 node->children[i], userdata))
2040                                         {
2041                                                 return false;
2042                                         }
2043                                 }
2044                         }
2045                 }
2046                 else {
2047                         for (int i = node->totnode - 1; i >= 0; i--) {
2048                                 if (walk_parent_cb((const BVHTreeAxisRange *)node->children[i]->bv, userdata)) {
2049                                         if (!bvhtree_walk_dfs_recursive(
2050                                                 walk_parent_cb, walk_leaf_cb, walk_order_cb,
2051                                                 node->children[i], userdata))
2052                                         {
2053                                                 return false;
2054                                         }
2055                                 }
2056                         }
2057                 }
2058         }
2059         return true;
2060 }
2061
2062 /**
2063  * This is a generic function to perform a depth first search on the BVHTree
2064  * where the search order and nodes traversed depend on callbacks passed in.
2065  *
2066  * \param tree: Tree to walk.
2067  * \param walk_parent_cb: Callback on a parents bound-box to test if it should be traversed.
2068  * \param walk_leaf_cb: Callback to test leaf nodes, callback must store its own result,
2069  * returning false exits early.
2070  * \param walk_order_cb: Callback that indicates which direction to search,
2071  * either from the node with the lower or higher k-dop axis value.
2072  * \param userdata: Argument passed to all callbacks.
2073  */
2074 void BLI_bvhtree_walk_dfs(
2075         BVHTree *tree,
2076         BVHTree_WalkParentCallback walk_parent_cb,
2077         BVHTree_WalkLeafCallback walk_leaf_cb,
2078         BVHTree_WalkOrderCallback walk_order_cb, void *userdata)
2079 {
2080         const BVHNode *root = tree->nodes[tree->totleaf];
2081         if (root != NULL) {
2082                 /* first make sure the bv of root passes in the test too */
2083                 if (walk_parent_cb((const BVHTreeAxisRange *)root->bv, userdata)) {
2084                         bvhtree_walk_dfs_recursive(walk_parent_cb, walk_leaf_cb, walk_order_cb, root, userdata);
2085                 }
2086         }
2087 }
2088
2089 /** \} */