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