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