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