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