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