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