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