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