2 * ***** BEGIN GPL LICENSE BLOCK *****
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.
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.
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.
18 * The Original Code is Copyright (C) 2006 by NaN Holding BV.
19 * All rights reserved.
21 * The Original Code is: all of this file.
23 * Contributor(s): Daniel Genrich, Andre Pinto
25 * ***** END GPL LICENSE BLOCK *****
28 /** \file blender/blenlib/intern/BLI_kdopbvh.c
35 #include "MEM_guardedalloc.h"
37 #include "BLI_utildefines.h"
41 #include "BLI_kdopbvh.h"
50 #define MAX_TREETYPE 32
51 #define DEFAULT_FIND_NEAREST_HEAP_SIZE 1024
53 typedef struct BVHNode
55 struct BVHNode **children;
56 struct BVHNode *parent; // some user defined traversed need that
57 struct BVHNode *skip[2];
58 float *bv; // Bounding volume of all nodes, max 13 axis
59 int index; // face, edge, vertex index
60 char totnode; // how many nodes are used, used for speedup
61 char main_axis; // Axis used to split this node
67 BVHNode *nodearray; /* pre-alloc branch nodes */
68 BVHNode **nodechild; // pre-alloc childs for nodes
69 float *nodebv; // pre-alloc bounding-volumes for nodes
70 float epsilon; /* epslion is used for inflation of the k-dop */
73 char tree_type; // type of tree (4 => quadtree)
74 char axis; // kdop type (6 => OBB, 7 => AABB, ...)
75 char start_axis, stop_axis; // KDOP_AXES array indices according to axis
78 typedef struct BVHOverlapData
80 BVHTree *tree1, *tree2;
81 BVHTreeOverlap *overlap;
82 int i, max_overlap; /* i is number of overlaps */
83 int start_axis, stop_axis;
86 typedef struct BVHNearestData
90 BVHTree_NearestPointCallback callback;
92 float proj[13]; //coordinates projection over axis
93 BVHTreeNearest nearest;
97 typedef struct BVHRayCastData
101 BVHTree_RayCastCallback callback;
106 float ray_dot_axis[13];
112 ////////////////////////////////////////m
115 ////////////////////////////////////////////////////////////////////////
116 // Bounding Volume Hierarchy Definition
118 // Notes: From OBB until 26-DOP --> all bounding volumes possible, just choose type below
119 // Notes: You have to choose the type at compile time ITM
120 // Notes: You can choose the tree type --> binary, quad, octree, choose below
121 ////////////////////////////////////////////////////////////////////////
123 static float KDOP_AXES[13][3] =
124 { {1.0, 0, 0}, {0, 1.0, 0}, {0, 0, 1.0}, {1.0, 1.0, 1.0}, {1.0, -1.0, 1.0}, {1.0, 1.0, -1.0},
125 {1.0, -1.0, -1.0}, {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},
130 * Generic push and pop heap
132 #define PUSH_HEAP_BODY(HEAP_TYPE, PRIORITY, heap, heap_size) \
134 HEAP_TYPE element = heap[heap_size-1]; \
135 int child = heap_size-1; \
138 int parent = (child-1) / 2; \
139 if (PRIORITY(element, heap[parent])) \
141 heap[child] = heap[parent]; \
146 heap[child] = element; \
149 #define POP_HEAP_BODY(HEAP_TYPE, PRIORITY, heap, heap_size) \
151 HEAP_TYPE element = heap[heap_size-1]; \
153 while (parent < (heap_size-1)/2 ) \
155 int child2 = (parent+1)*2; \
156 if (PRIORITY(heap[child2-1], heap[child2])) \
159 if (PRIORITY(element, heap[child2])) \
162 heap[parent] = heap[child2]; \
165 heap[parent] = element; \
169 static int ADJUST_MEMORY(void *local_memblock, void **memblock, int new_size, int *max_size, int size_per_item)
171 int new_max_size = *max_size * 2;
172 void *new_memblock = NULL;
174 if (new_size <= *max_size)
177 if (*memblock == local_memblock)
179 new_memblock = malloc( size_per_item * new_max_size );
180 memcpy(new_memblock, *memblock, size_per_item * *max_size);
183 new_memblock = realloc(*memblock, size_per_item * new_max_size );
187 *memblock = new_memblock;
188 *max_size = new_max_size;
196 //////////////////////////////////////////////////////////////////////////////////////////////////////
198 // with permission deriven from the following Java code:
199 // http://ralphunden.net/content/tutorials/a-guide-to-introsort/
200 // and he derived it from the SUN STL
201 //////////////////////////////////////////////////////////////////////////////////////////////////////
202 //static int size_threshold = 16;
204 * Common methods for all algorithms
207 static int floor_lg(int a)
209 return (int)(floor(log(a)/log(2)));
214 * Insertion sort algorithm
216 static void bvh_insertionsort(BVHNode **a, int lo, int hi, int axis)
220 for (i=lo; i < hi; i++) {
223 while ((j!=lo) && (t->bv[axis] < (a[j-1])->bv[axis])) {
231 static int bvh_partition(BVHNode **a, int lo, int hi, BVHNode * x, int axis)
235 while ((a[i])->bv[axis] < x->bv[axis]) i++;
237 while (x->bv[axis] < (a[j])->bv[axis]) j--;
240 SWAP(BVHNode *, a[i], a[j]);
249 static void bvh_downheap(BVHNode **a, int i, int n, int lo, int axis)
251 BVHNode * d = a[lo+i-1];
256 if ((child < n) && ((a[lo+child-1])->bv[axis] < (a[lo+child])->bv[axis]))
260 if (!(d->bv[axis] < (a[lo+child-1])->bv[axis])) break;
261 a[lo+i-1] = a[lo+child-1];
267 static void bvh_heapsort(BVHNode **a, int lo, int hi, int axis)
270 for (i=n/2; i>=1; i=i-1)
272 bvh_downheap(a, i, n, lo, axis);
274 for (i=n; i>1; i=i-1)
276 SWAP(BVHNode*, a[lo], a[lo+i-1]);
277 bvh_downheap(a, 1, i-1, lo, axis);
282 static BVHNode *bvh_medianof3(BVHNode **a, int lo, int mid, int hi, int axis) // returns Sortable
284 if ((a[mid])->bv[axis] < (a[lo])->bv[axis]) {
285 if ((a[hi])->bv[axis] < (a[mid])->bv[axis])
288 if ((a[hi])->bv[axis] < (a[lo])->bv[axis])
295 if ((a[hi])->bv[axis] < (a[mid])->bv[axis]) {
296 if ((a[hi])->bv[axis] < (a[lo])->bv[axis])
308 * Quicksort algorithm modified for Introsort
310 static void bvh_introsort_loop (BVHNode **a, int lo, int hi, int depth_limit, int axis)
314 while (hi-lo > size_threshold)
316 if (depth_limit == 0)
318 bvh_heapsort(a, lo, hi, axis);
321 depth_limit=depth_limit-1;
322 p=bvh_partition(a, lo, hi, bvh_medianof3(a, lo, lo+((hi-lo)/2)+1, hi-1, axis), axis);
323 bvh_introsort_loop(a, p, hi, depth_limit, axis);
328 static void sort(BVHNode **a0, int begin, int end, int axis)
333 bvh_introsort_loop(a, begin, end, 2*floor_lg(end-begin), axis);
334 bvh_insertionsort(a, begin, end, axis);
338 static void sort_along_axis(BVHTree *tree, int start, int end, int axis)
340 sort(tree->nodes, start, end, axis);
344 //after a call to this function you can expect one of:
345 // every node to left of a[n] are smaller or equal to it
346 // every node to the right of a[n] are greater or equal to it
347 static int partition_nth_element(BVHNode **a, int _begin, int _end, int n, int axis)
349 int begin = _begin, end = _end, cut;
350 while (end-begin > 3) {
351 cut = bvh_partition(a, begin, end, bvh_medianof3(a, begin, (begin+end)/2, end-1, axis), axis );
357 bvh_insertionsort(a, begin, end, axis);
362 //////////////////////////////////////////////////////////////////////////////////////////////////////
363 static void build_skip_links(BVHTree *tree, BVHNode *node, BVHNode *left, BVHNode *right)
367 node->skip[0] = left;
368 node->skip[1] = right;
370 for (i = 0; i < node->totnode; i++) {
371 if (i+1 < node->totnode)
372 build_skip_links(tree, node->children[i], left, node->children[i+1] );
374 build_skip_links(tree, node->children[i], left, right );
376 left = node->children[i];
381 * BVHTree bounding volumes functions
383 static void create_kdop_hull(BVHTree *tree, BVHNode *node, const float *co, int numpoints, int moving)
386 float *bv = node->bv;
389 // don't init boudings for the moving case
391 for (i = tree->start_axis; i < tree->stop_axis; i++) {
393 bv[2 * i + 1] = -FLT_MAX;
397 for (k = 0; k < numpoints; k++) {
399 for (i = tree->start_axis; i < tree->stop_axis; i++) {
400 newminmax = dot_v3v3(&co[k * 3], KDOP_AXES[i]);
401 if (newminmax < bv[2 * i])
402 bv[2 * i] = newminmax;
403 if (newminmax > bv[(2 * i) + 1])
404 bv[(2 * i) + 1] = newminmax;
409 // depends on the fact that the BVH's for each face is already build
410 static void refit_kdop_hull(BVHTree *tree, BVHNode *node, int start, int end)
412 float newmin, newmax;
414 float *bv = node->bv;
417 for (i = tree->start_axis; i < tree->stop_axis; i++) {
419 bv[2*i + 1] = -FLT_MAX;
422 for (j = start; j < end; j++) {
424 for (i = tree->start_axis; i < tree->stop_axis; i++) {
425 newmin = tree->nodes[j]->bv[(2 * i)];
426 if ((newmin < bv[(2 * i)]))
427 bv[(2 * i)] = newmin;
429 newmax = tree->nodes[j]->bv[(2 * i) + 1];
430 if ((newmax > bv[(2 * i) + 1]))
431 bv[(2 * i) + 1] = newmax;
437 // only supports x,y,z axis in the moment
438 // but we should use a plain and simple function here for speed sake
439 static char get_largest_axis(float *bv)
441 float middle_point[3];
443 middle_point[0] = (bv[1]) - (bv[0]); // x axis
444 middle_point[1] = (bv[3]) - (bv[2]); // y axis
445 middle_point[2] = (bv[5]) - (bv[4]); // z axis
446 if (middle_point[0] > middle_point[1]) {
447 if (middle_point[0] > middle_point[2])
448 return 1; // max x axis
450 return 5; // max z axis
453 if (middle_point[1] > middle_point[2])
454 return 3; // max y axis
456 return 5; // max z axis
460 // bottom-up update of bvh node BV
461 // join the children on the parent BV
462 static void node_join(BVHTree *tree, BVHNode *node)
466 for (i = tree->start_axis; i < tree->stop_axis; i++) {
467 node->bv[2*i] = FLT_MAX;
468 node->bv[2*i + 1] = -FLT_MAX;
471 for (i = 0; i < tree->tree_type; i++) {
472 if (node->children[i]) {
473 for (j = tree->start_axis; j < tree->stop_axis; j++) {
475 if (node->children[i]->bv[(2 * j)] < node->bv[(2 * j)])
476 node->bv[(2 * j)] = node->children[i]->bv[(2 * j)];
479 if (node->children[i]->bv[(2 * j) + 1] > node->bv[(2 * j) + 1])
480 node->bv[(2 * j) + 1] = node->children[i]->bv[(2 * j) + 1];
489 * Debug and information functions
492 static void bvhtree_print_tree(BVHTree *tree, BVHNode *node, int depth)
495 for (i=0; i<depth; i++) printf(" ");
496 printf(" - %d (%ld): ", node->index, node - tree->nodearray);
497 for (i=2*tree->start_axis; i<2*tree->stop_axis; i++)
498 printf("%.3f ", node->bv[i]);
501 for (i=0; i<tree->tree_type; i++)
502 if (node->children[i])
503 bvhtree_print_tree(tree, node->children[i], depth+1);
506 static void bvhtree_info(BVHTree *tree)
508 printf("BVHTree info\n");
509 printf("tree_type = %d, axis = %d, epsilon = %f\n", tree->tree_type, tree->axis, tree->epsilon);
510 printf("nodes = %d, branches = %d, leafs = %d\n", tree->totbranch + tree->totleaf, tree->totbranch, tree->totleaf);
511 printf("Memory per node = %ldbytes\n", sizeof(BVHNode) + sizeof(BVHNode*)*tree->tree_type + sizeof(float)*tree->axis);
512 printf("BV memory = %dbytes\n", MEM_allocN_len(tree->nodebv));
514 printf("Total memory = %ldbytes\n", sizeof(BVHTree)
515 + MEM_allocN_len(tree->nodes)
516 + MEM_allocN_len(tree->nodearray)
517 + MEM_allocN_len(tree->nodechild)
518 + MEM_allocN_len(tree->nodebv)
521 // bvhtree_print_tree(tree, tree->nodes[tree->totleaf], 0);
528 static void verify_tree(BVHTree *tree)
532 // check the pointer list
533 for (i = 0; i < tree->totleaf; i++)
535 if (tree->nodes[i]->parent == NULL)
536 printf("Leaf has no parent: %d\n", i);
539 for (j = 0; j < tree->tree_type; j++)
541 if (tree->nodes[i]->parent->children[j] == tree->nodes[i])
546 printf("Parent child relationship doesn't match: %d\n", i);
552 // check the leaf list
553 for (i = 0; i < tree->totleaf; i++)
555 if (tree->nodearray[i].parent == NULL)
556 printf("Leaf has no parent: %d\n", i);
559 for (j = 0; j < tree->tree_type; j++)
561 if (tree->nodearray[i].parent->children[j] == &tree->nodearray[i])
566 printf("Parent child relationship doesn't match: %d\n", i);
572 printf("branches: %d, leafs: %d, total: %d\n", tree->totbranch, tree->totleaf, tree->totbranch + tree->totleaf);
576 //Helper data and structures to build a min-leaf generalized implicit tree
577 //This code can be easily reduced (basicly this is only method to calculate pow(k, n) in O(1).. and stuff like that)
578 typedef struct BVHBuildHelper
583 int leafs_per_child[32]; /* Min number of leafs that are archievable from a node at depth N */
584 int branches_on_level[32]; /* Number of nodes at depth N (tree_type^N) */
586 int remain_leafs; /* Number of leafs that are placed on the level that is not 100% filled */
590 static void build_implicit_tree_helper(BVHTree *tree, BVHBuildHelper *data)
596 data->totleafs = tree->totleaf;
597 data->tree_type= tree->tree_type;
599 //Calculate the smallest tree_type^n such that tree_type^n >= num_leafs
600 for (data->leafs_per_child[0] = 1;
601 data->leafs_per_child[0] < data->totleafs;
602 data->leafs_per_child[0] *= data->tree_type)
607 data->branches_on_level[0] = 1;
609 //We could stop the loop first (but I am lazy to find out when)
610 for (depth = 1; depth < 32; depth++) {
611 data->branches_on_level[depth] = data->branches_on_level[depth-1] * data->tree_type;
612 data->leafs_per_child [depth] = data->leafs_per_child [depth-1] / data->tree_type;
615 remain = data->totleafs - data->leafs_per_child[1];
616 nnodes = (remain + data->tree_type - 2) / (data->tree_type - 1);
617 data->remain_leafs = remain + nnodes;
620 // return the min index of all the leafs archivable with the given branch
621 static int implicit_leafs_index(BVHBuildHelper *data, int depth, int child_index)
623 int min_leaf_index = child_index * data->leafs_per_child[depth-1];
624 if (min_leaf_index <= data->remain_leafs)
625 return min_leaf_index;
626 else if (data->leafs_per_child[depth])
627 return data->totleafs - (data->branches_on_level[depth-1] - child_index) * data->leafs_per_child[depth];
629 return data->remain_leafs;
633 * Generalized implicit tree build
635 * An implicit tree is a tree where its structure is implied, thus there is no need to store child pointers or indexs.
636 * Its possible to find the position of the child or the parent with simple maths (multiplication and adittion). This type
637 * of tree is for example used on heaps.. where node N has its childs at indexs N*2 and N*2+1.
639 * Although in this case the tree type is general.. and not know until runtime.
640 * tree_type stands for the maximum number of childs that a tree node can have.
641 * All tree types >= 2 are supported.
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;
649 * Some math relations derived for general implicit trees:
651 * K = tree_type, ( 2 <= K )
653 * N child of node A = A * K + (2 - K) + N, (0 <= N < K)
657 * (looping elements, knowing if its a leaf or not.. etc...)
660 // This functions returns the number of branches needed to have the requested number of leafs.
661 static int implicit_needed_branches(int tree_type, int leafs)
663 return MAX2(1, (leafs + tree_type - 3) / (tree_type-1) );
667 * This function handles the problem of "sorting" the leafs (along the split_axis).
669 * It arranges the elements in the given partitions such that:
670 * - any element in partition N is less or equal to any element in partition N+1.
671 * - if all elements are different all partition will get the same subset of elements
672 * as if the array was sorted.
674 * partition P is described as the elements in the range ( nth[P], nth[P+1] ]
676 * TODO: This can be optimized a bit by doing a specialized nth_element instead of K nth_elements
678 static void split_leafs(BVHNode **leafs_array, int *nth, int partitions, int split_axis)
681 for (i=0; i < partitions-1; i++) {
682 if (nth[i] >= nth[partitions])
685 partition_nth_element(leafs_array, nth[i], nth[partitions], nth[i+1], split_axis);
690 * This functions builds an optimal implicit tree from the given leafs.
691 * Where optimal stands for:
692 * - The resulting tree will have the smallest number of branches;
693 * - At most only one branch will have NULL childs;
694 * - All leafs will be stored at level N or N+1.
696 * This function creates an implicit tree on branches_array, the leafs are given on the leafs_array.
698 * The tree is built per depth levels. First branches at depth 1.. then branches at depth 2.. etc..
699 * The reason is that we can build level N+1 from level N without any data dependencies.. thus it allows
700 * to use multithread building.
702 * To archieve this is necessary to find how much leafs are accessible from a certain branch, BVHBuildHelper
703 * implicit_needed_branches and implicit_leafs_index are auxiliary functions to solve that "optimal-split".
705 static void non_recursive_bvh_div_nodes(BVHTree *tree, BVHNode *branches_array, BVHNode **leafs_array, int num_leafs)
709 const int tree_type = tree->tree_type;
710 const int tree_offset = 2 - tree->tree_type; //this value is 0 (on binary trees) and negative on the others
711 const int num_branches= implicit_needed_branches(tree_type, num_leafs);
716 // set parent from root node to NULL
717 BVHNode *tmp = branches_array+0;
720 //Most of bvhtree code relies on 1-leaf trees having at least one branch
721 //We handle that special case here
722 if (num_leafs == 1) {
723 BVHNode *root = branches_array+0;
724 refit_kdop_hull(tree, root, 0, num_leafs);
725 root->main_axis = get_largest_axis(root->bv) / 2;
727 root->children[0] = leafs_array[0];
728 root->children[0]->parent = root;
732 branches_array--; //Implicit trees use 1-based indexs
734 build_implicit_tree_helper(tree, &data);
736 //Loop tree levels (log N) loops
737 for (i=1, depth = 1; i <= num_branches; i = i*tree_type + tree_offset, depth++) {
738 const int first_of_next_level = i*tree_type + tree_offset;
739 const int end_j = MIN2(first_of_next_level, num_branches + 1); //index of last branch on this level
742 //Loop all branches on this level
743 #pragma omp parallel for private(j) schedule(static)
744 for (j = i; j < end_j; j++) {
746 const int parent_level_index= j-i;
747 BVHNode* parent = branches_array + j;
748 int nth_positions[ MAX_TREETYPE + 1];
751 int parent_leafs_begin = implicit_leafs_index(&data, depth, parent_level_index);
752 int parent_leafs_end = implicit_leafs_index(&data, depth, parent_level_index+1);
754 //This calculates the bounding box of this branch
755 //and chooses the largest axis as the axis to divide leafs
756 refit_kdop_hull(tree, parent, parent_leafs_begin, parent_leafs_end);
757 split_axis = get_largest_axis(parent->bv);
759 //Save split axis (this can be used on raytracing to speedup the query time)
760 parent->main_axis = split_axis / 2;
762 //Split the childs along the split_axis, note: its not needed to sort the whole leafs array
763 //Only to assure that the elements are partioned on a way that each child takes the elements
764 //it would take in case the whole array was sorted.
765 //Split_leafs takes care of that "sort" problem.
766 nth_positions[ 0] = parent_leafs_begin;
767 nth_positions[tree_type] = parent_leafs_end;
768 for (k = 1; k < tree_type; k++) {
769 int child_index = j * tree_type + tree_offset + k;
770 int child_level_index = child_index - first_of_next_level; //child level index
771 nth_positions[k] = implicit_leafs_index(&data, depth+1, child_level_index);
774 split_leafs(leafs_array, nth_positions, tree_type, split_axis);
777 //Setup children and totnode counters
778 //Not really needed but currently most of BVH code relies on having an explicit children structure
779 for (k = 0; k < tree_type; k++) {
780 int child_index = j * tree_type + tree_offset + k;
781 int child_level_index = child_index - first_of_next_level; //child level index
783 int child_leafs_begin = implicit_leafs_index(&data, depth+1, child_level_index);
784 int child_leafs_end = implicit_leafs_index(&data, depth+1, child_level_index+1);
786 if (child_leafs_end - child_leafs_begin > 1) {
787 parent->children[k] = branches_array + child_index;
788 parent->children[k]->parent = parent;
790 else if (child_leafs_end - child_leafs_begin == 1) {
791 parent->children[k] = leafs_array[ child_leafs_begin ];
792 parent->children[k]->parent = parent;
798 parent->totnode = k+1;
808 BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
813 // theres not support for trees below binary-trees :P
817 if (tree_type > MAX_TREETYPE)
820 tree = (BVHTree *)MEM_callocN(sizeof(BVHTree), "BVHTree");
822 //tree epsilon must be >= FLT_EPSILON
823 //so that tangent rays can still hit a bounding volume..
824 //this bug would show up when casting a ray aligned with a kdop-axis and with an edge of 2 faces
825 epsilon = MAX2(FLT_EPSILON, epsilon);
828 tree->epsilon = epsilon;
829 tree->tree_type = tree_type;
833 tree->start_axis = 0;
834 tree->stop_axis = 13;
836 else if (axis == 18) {
837 tree->start_axis = 7;
838 tree->stop_axis = 13;
840 else if (axis == 14) {
841 tree->start_axis = 0;
844 else if (axis == 8) { /* AABB */
845 tree->start_axis = 0;
848 else if (axis == 6) { /* OBB */
849 tree->start_axis = 0;
859 numnodes = maxsize + implicit_needed_branches(tree_type, maxsize) + tree_type;
861 tree->nodes = (BVHNode **)MEM_callocN(sizeof(BVHNode *)*numnodes, "BVHNodes");
868 tree->nodebv = (float*)MEM_callocN(sizeof(float)* axis * numnodes, "BVHNodeBV");
870 MEM_freeN(tree->nodes);
874 tree->nodechild = (BVHNode**)MEM_callocN(sizeof(BVHNode*) * tree_type * numnodes, "BVHNodeBV");
875 if (!tree->nodechild) {
876 MEM_freeN(tree->nodebv);
877 MEM_freeN(tree->nodes);
881 tree->nodearray = (BVHNode *)MEM_callocN(sizeof(BVHNode)* numnodes, "BVHNodeArray");
883 if (!tree->nodearray) {
884 MEM_freeN(tree->nodechild);
885 MEM_freeN(tree->nodebv);
886 MEM_freeN(tree->nodes);
891 //link the dynamic bv and child links
892 for (i=0; i< numnodes; i++) {
893 tree->nodearray[i].bv = tree->nodebv + i * axis;
894 tree->nodearray[i].children = tree->nodechild + i * tree_type;
902 void BLI_bvhtree_free(BVHTree *tree)
905 MEM_freeN(tree->nodes);
906 MEM_freeN(tree->nodearray);
907 MEM_freeN(tree->nodebv);
908 MEM_freeN(tree->nodechild);
913 void BLI_bvhtree_balance(BVHTree *tree)
917 BVHNode* branches_array = tree->nodearray + tree->totleaf;
918 BVHNode** leafs_array = tree->nodes;
920 //This function should only be called once (some big bug goes here if its being called more than once per tree)
921 assert(tree->totbranch == 0);
923 //Build the implicit tree
924 non_recursive_bvh_div_nodes(tree, branches_array, leafs_array, tree->totleaf);
926 //current code expects the branches to be linked to the nodes array
927 //we perform that linkage here
928 tree->totbranch = implicit_needed_branches(tree->tree_type, tree->totleaf);
929 for (i = 0; i < tree->totbranch; i++)
930 tree->nodes[tree->totleaf + i] = branches_array + i;
932 build_skip_links(tree, tree->nodes[tree->totleaf], NULL, NULL);
933 //bvhtree_info(tree);
936 int BLI_bvhtree_insert(BVHTree *tree, int index, const float *co, int numpoints)
939 BVHNode *node = NULL;
941 // insert should only possible as long as tree->totbranch is 0
942 if (tree->totbranch > 0)
945 if (tree->totleaf+1 >= MEM_allocN_len(tree->nodes)/sizeof(*(tree->nodes)))
948 // TODO check if have enough nodes in array
950 node = tree->nodes[tree->totleaf] = &(tree->nodearray[tree->totleaf]);
953 create_kdop_hull(tree, node, co, numpoints, 0);
956 // inflate the bv with some epsilon
957 for (i = tree->start_axis; i < tree->stop_axis; i++) {
958 node->bv[(2 * i)] -= tree->epsilon; // minimum
959 node->bv[(2 * i) + 1] += tree->epsilon; // maximum
966 // call before BLI_bvhtree_update_tree()
967 int BLI_bvhtree_update_node(BVHTree *tree, int index, const float *co, const float *co_moving, int numpoints)
972 // check if index exists
973 if (index > tree->totleaf)
976 node = tree->nodearray + index;
978 create_kdop_hull(tree, node, co, numpoints, 0);
981 create_kdop_hull(tree, node, co_moving, numpoints, 1);
983 // inflate the bv with some epsilon
984 for (i = tree->start_axis; i < tree->stop_axis; i++) {
985 node->bv[(2 * i)] -= tree->epsilon; // minimum
986 node->bv[(2 * i) + 1] += tree->epsilon; // maximum
992 // call BLI_bvhtree_update_node() first for every node/point/triangle
993 void BLI_bvhtree_update_tree(BVHTree *tree)
996 //TRICKY: the way we build the tree all the childs have an index greater than the parent
997 //This allows us todo a bottom up update by starting on the biger numbered branch
999 BVHNode** root = tree->nodes + tree->totleaf;
1000 BVHNode** index = tree->nodes + tree->totleaf + tree->totbranch-1;
1002 for (; index >= root; index--)
1003 node_join(tree, *index);
1006 float BLI_bvhtree_getepsilon(BVHTree *tree)
1008 return tree->epsilon;
1013 * BLI_bvhtree_overlap
1015 // overlap - is it possbile for 2 bv's to collide ?
1016 static int tree_overlap(BVHNode *node1, BVHNode *node2, int start_axis, int stop_axis)
1018 float *bv1 = node1->bv;
1019 float *bv2 = node2->bv;
1021 float *bv1_end = bv1 + (stop_axis<<1);
1023 bv1 += start_axis<<1;
1024 bv2 += start_axis<<1;
1026 // test all axis if min + max overlap
1027 for (; bv1 != bv1_end; bv1+=2, bv2+=2) {
1028 if ((*(bv1) > *(bv2 + 1)) || (*(bv2) > *(bv1 + 1)))
1035 static void traverse(BVHOverlapData *data, BVHNode *node1, BVHNode *node2)
1039 if (tree_overlap(node1, node2, data->start_axis, data->stop_axis)) {
1040 // check if node1 is a leaf
1041 if (!node1->totnode) {
1042 // check if node2 is a leaf
1043 if (!node2->totnode) {
1045 if (node1 == node2) {
1049 if (data->i >= data->max_overlap) {
1050 // try to make alloc'ed memory bigger
1051 data->overlap = realloc(data->overlap, sizeof(BVHTreeOverlap)*data->max_overlap*2);
1053 if (!data->overlap) {
1054 printf("Out of Memory in traverse\n");
1057 data->max_overlap *= 2;
1060 // both leafs, insert overlap!
1061 data->overlap[data->i].indexA = node1->index;
1062 data->overlap[data->i].indexB = node2->index;
1067 for (j = 0; j < data->tree2->tree_type; j++) {
1068 if (node2->children[j])
1069 traverse(data, node1, node2->children[j]);
1074 for (j = 0; j < data->tree2->tree_type; j++) {
1075 if (node1->children[j])
1076 traverse(data, node1->children[j], node2);
1083 BVHTreeOverlap *BLI_bvhtree_overlap(BVHTree *tree1, BVHTree *tree2, unsigned int *result)
1086 unsigned int total = 0;
1087 BVHTreeOverlap *overlap = NULL, *to = NULL;
1088 BVHOverlapData **data;
1090 // check for compatibility of both trees (can't compare 14-DOP with 18-DOP)
1091 if ((tree1->axis != tree2->axis) && (tree1->axis == 14 || tree2->axis == 14) && (tree1->axis == 18 || tree2->axis == 18))
1094 // fast check root nodes for collision before doing big splitting + traversal
1095 if (!tree_overlap(tree1->nodes[tree1->totleaf], tree2->nodes[tree2->totleaf], MIN2(tree1->start_axis, tree2->start_axis), MIN2(tree1->stop_axis, tree2->stop_axis)))
1098 data = MEM_callocN(sizeof(BVHOverlapData *)* tree1->tree_type, "BVHOverlapData_star");
1100 for (j = 0; j < tree1->tree_type; j++) {
1101 data[j] = (BVHOverlapData *)MEM_callocN(sizeof(BVHOverlapData), "BVHOverlapData");
1103 // init BVHOverlapData
1104 data[j]->overlap = (BVHTreeOverlap *)malloc(sizeof(BVHTreeOverlap)*MAX2(tree1->totleaf, tree2->totleaf));
1105 data[j]->tree1 = tree1;
1106 data[j]->tree2 = tree2;
1107 data[j]->max_overlap = MAX2(tree1->totleaf, tree2->totleaf);
1109 data[j]->start_axis = MIN2(tree1->start_axis, tree2->start_axis);
1110 data[j]->stop_axis = MIN2(tree1->stop_axis, tree2->stop_axis );
1113 #pragma omp parallel for private(j) schedule(static)
1114 for (j = 0; j < MIN2(tree1->tree_type, tree1->nodes[tree1->totleaf]->totnode); j++) {
1115 traverse(data[j], tree1->nodes[tree1->totleaf]->children[j], tree2->nodes[tree2->totleaf]);
1118 for (j = 0; j < tree1->tree_type; j++)
1119 total += data[j]->i;
1121 to = overlap = (BVHTreeOverlap *)MEM_callocN(sizeof(BVHTreeOverlap)*total, "BVHTreeOverlap");
1123 for (j = 0; j < tree1->tree_type; j++) {
1124 memcpy(to, data[j]->overlap, data[j]->i*sizeof(BVHTreeOverlap));
1128 for (j = 0; j < tree1->tree_type; j++) {
1129 free(data[j]->overlap);
1138 //Determines the nearest point of the given node BV. Returns the squared distance to that point.
1139 static float calc_nearest_point(const float proj[3], BVHNode *node, float *nearest)
1142 const float *bv = node->bv;
1144 //nearest on AABB hull
1145 for (i=0; i != 3; i++, bv += 2) {
1146 if (bv[0] > proj[i])
1148 else if (bv[1] < proj[i])
1151 nearest[i] = proj[i];
1155 //nearest on a general hull
1156 copy_v3_v3(nearest, data->co);
1157 for (i = data->tree->start_axis; i != data->tree->stop_axis; i++, bv+=2)
1159 float proj = dot_v3v3( nearest, KDOP_AXES[i]);
1160 float dl = bv[0] - proj;
1161 float du = bv[1] - proj;
1164 madd_v3_v3fl(nearest, KDOP_AXES[i], dl);
1167 madd_v3_v3fl(nearest, KDOP_AXES[i], du);
1172 return len_squared_v3v3(proj, nearest);
1176 typedef struct NodeDistance
1183 #define NodeDistance_priority(a, b) ( (a).dist < (b).dist )
1185 // TODO: use a priority queue to reduce the number of nodes looked on
1186 static void dfs_find_nearest_dfs(BVHNearestData *data, BVHNode *node)
1188 if (node->totnode == 0) {
1190 data->callback(data->userdata, node->index, data->co, &data->nearest);
1192 data->nearest.index = node->index;
1193 data->nearest.dist = calc_nearest_point(data->proj, node, data->nearest.co);
1197 //Better heuristic to pick the closest node to dive on
1201 if (data->proj[ node->main_axis ] <= node->children[0]->bv[node->main_axis*2+1]) {
1203 for (i=0; i != node->totnode; i++) {
1204 if ( calc_nearest_point(data->proj, node->children[i], nearest) >= data->nearest.dist) continue;
1205 dfs_find_nearest_dfs(data, node->children[i]);
1209 for (i=node->totnode-1; i >= 0 ; i--) {
1210 if ( calc_nearest_point(data->proj, node->children[i], nearest) >= data->nearest.dist) continue;
1211 dfs_find_nearest_dfs(data, node->children[i]);
1217 static void dfs_find_nearest_begin(BVHNearestData *data, BVHNode *node)
1219 float nearest[3], sdist;
1220 sdist = calc_nearest_point(data->proj, node, nearest);
1221 if (sdist >= data->nearest.dist) return;
1222 dfs_find_nearest_dfs(data, node);
1227 static void NodeDistance_push_heap(NodeDistance *heap, int heap_size)
1228 PUSH_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size)
1230 static void NodeDistance_pop_heap(NodeDistance *heap, int heap_size)
1231 POP_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size)
1233 //NN function that uses an heap.. this functions leads to an optimal number of min-distance
1234 //but for normal tri-faces and BV 6-dop.. a simple dfs with local heuristics (as implemented
1235 //in source/blender/blenkernel/intern/shrinkwrap.c) works faster.
1237 //It may make sense to use this function if the callback queries are very slow.. or if its impossible
1238 //to get a nice heuristic
1240 //this function uses "malloc/free" instead of the MEM_* because it intends to be openmp safe
1241 static void bfs_find_nearest(BVHNearestData *data, BVHNode *node)
1244 NodeDistance default_heap[DEFAULT_FIND_NEAREST_HEAP_SIZE];
1245 NodeDistance *heap=default_heap, current;
1246 int heap_size = 0, max_heap_size = sizeof(default_heap)/sizeof(default_heap[0]);
1249 int callbacks = 0, push_heaps = 0;
1251 if (node->totnode == 0)
1253 dfs_find_nearest_dfs(data, node);
1257 current.node = node;
1258 current.dist = calc_nearest_point(data->proj, node, nearest);
1260 while (current.dist < data->nearest.dist)
1262 // printf("%f : %f\n", current.dist, data->nearest.dist);
1263 for (i=0; i< current.node->totnode; i++)
1265 BVHNode *child = current.node->children[i];
1266 if (child->totnode == 0)
1269 dfs_find_nearest_dfs(data, child);
1274 if ((heap_size >= max_heap_size) &&
1275 ADJUST_MEMORY(default_heap, (void**)&heap, heap_size+1, &max_heap_size, sizeof(heap[0])) == FALSE)
1277 printf("WARNING: bvh_find_nearest got out of memory\n");
1279 if (heap != default_heap)
1285 heap[heap_size].node = current.node->children[i];
1286 heap[heap_size].dist = calc_nearest_point(data->proj, current.node->children[i], nearest);
1288 if (heap[heap_size].dist >= data->nearest.dist) continue;
1291 NodeDistance_push_heap(heap, heap_size);
1292 // PUSH_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size);
1297 if (heap_size == 0) break;
1300 NodeDistance_pop_heap(heap, heap_size);
1301 // POP_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size);
1305 // printf("hsize=%d, callbacks=%d, pushs=%d\n", heap_size, callbacks, push_heaps);
1307 if (heap != default_heap)
1313 int BLI_bvhtree_find_nearest(BVHTree *tree, const float co[3], BVHTreeNearest *nearest, BVHTree_NearestPointCallback callback, void *userdata)
1317 BVHNearestData data;
1318 BVHNode* root = tree->nodes[tree->totleaf];
1320 //init data to search
1324 data.callback = callback;
1325 data.userdata = userdata;
1327 for (i = data.tree->start_axis; i != data.tree->stop_axis; i++) {
1328 data.proj[i] = dot_v3v3(data.co, KDOP_AXES[i]);
1332 memcpy(&data.nearest, nearest, sizeof(*nearest));
1335 data.nearest.index = -1;
1336 data.nearest.dist = FLT_MAX;
1341 dfs_find_nearest_begin(&data, root);
1345 memcpy(nearest, &data.nearest, sizeof(*nearest));
1348 return data.nearest.index;
1353 * Raycast - BLI_bvhtree_ray_cast
1355 * raycast is done by performing a DFS on the BVHTree and saving the closest hit
1359 //Determines the distance that the ray must travel to hit the bounding volume of the given node
1360 static float ray_nearest_hit(BVHRayCastData *data, float *bv)
1364 float low = 0, upper = data->hit.dist;
1366 for (i=0; i != 3; i++, bv += 2) {
1367 if (data->ray_dot_axis[i] == 0.0f) {
1369 if (data->ray.origin[i] < bv[0] - data->ray.radius ||
1370 data->ray.origin[i] > bv[1] + data->ray.radius)
1376 float ll = (bv[0] - data->ray.radius - data->ray.origin[i]) / data->ray_dot_axis[i];
1377 float lu = (bv[1] + data->ray.radius - data->ray.origin[i]) / data->ray_dot_axis[i];
1379 if (data->ray_dot_axis[i] > 0.0f) {
1380 if (ll > low) low = ll;
1381 if (lu < upper) upper = lu;
1384 if (lu > low) low = lu;
1385 if (ll < upper) upper = ll;
1388 if (low > upper) return FLT_MAX;
1394 //Determines the distance that the ray must travel to hit the bounding volume of the given node
1395 //Based on Tactical Optimization of Ray/Box Intersection, by Graham Fyffe
1396 //[http://tog.acm.org/resources/RTNews/html/rtnv21n1.html#art9]
1398 //TODO this doens't has data->ray.radius in consideration
1399 static float fast_ray_nearest_hit(const BVHRayCastData *data, const BVHNode *node)
1401 const float *bv = node->bv;
1404 float t1x = (bv[data->index[0]] - data->ray.origin[0]) * data->idot_axis[0];
1405 float t2x = (bv[data->index[1]] - data->ray.origin[0]) * data->idot_axis[0];
1406 float t1y = (bv[data->index[2]] - data->ray.origin[1]) * data->idot_axis[1];
1407 float t2y = (bv[data->index[3]] - data->ray.origin[1]) * data->idot_axis[1];
1408 float t1z = (bv[data->index[4]] - data->ray.origin[2]) * data->idot_axis[2];
1409 float t2z = (bv[data->index[5]] - data->ray.origin[2]) * data->idot_axis[2];
1411 if (t1x > t2y || t2x < t1y || t1x > t2z || t2x < t1z || t1y > t2z || t2y < t1z) return FLT_MAX;
1412 if (t2x < 0.0f || t2y < 0.0f || t2z < 0.0f) return FLT_MAX;
1413 if (t1x > data->hit.dist || t1y > data->hit.dist || t1z > data->hit.dist) return FLT_MAX;
1416 if (t1y > dist) dist = t1y;
1417 if (t1z > dist) dist = t1z;
1421 static void dfs_raycast(BVHRayCastData *data, BVHNode *node)
1425 //ray-bv is really fast.. and simple tests revealed its worth to test it
1426 //before calling the ray-primitive functions
1427 /* XXX: temporary solution for particles until fast_ray_nearest_hit supports ray.radius */
1428 float dist = (data->ray.radius > 0.0f) ? ray_nearest_hit(data, node->bv) : fast_ray_nearest_hit(data, node);
1429 if (dist >= data->hit.dist) return;
1431 if (node->totnode == 0) {
1432 if (data->callback) {
1433 data->callback(data->userdata, node->index, &data->ray, &data->hit);
1436 data->hit.index = node->index;
1437 data->hit.dist = dist;
1438 madd_v3_v3v3fl(data->hit.co, data->ray.origin, data->ray.direction, dist);
1442 //pick loop direction to dive into the tree (based on ray direction and split axis)
1443 if (data->ray_dot_axis[ (int)node->main_axis ] > 0.0f) {
1444 for (i=0; i != node->totnode; i++) {
1445 dfs_raycast(data, node->children[i]);
1449 for (i=node->totnode-1; i >= 0; i--) {
1450 dfs_raycast(data, node->children[i]);
1457 static void iterative_raycast(BVHRayCastData *data, BVHNode *node)
1461 float dist = fast_ray_nearest_hit(data, node);
1462 if (dist >= data->hit.dist)
1464 node = node->skip[1];
1468 if (node->totnode == 0)
1471 data->callback(data->userdata, node->index, &data->ray, &data->hit);
1474 data->hit.index = node->index;
1475 data->hit.dist = dist;
1476 madd_v3_v3v3fl(data->hit.co, data->ray.origin, data->ray.direction, dist);
1479 node = node->skip[1];
1483 node = node->children[0];
1489 int BLI_bvhtree_ray_cast(BVHTree *tree, const float co[3], const float dir[3], float radius, BVHTreeRayHit *hit, BVHTree_RayCastCallback callback, void *userdata)
1492 BVHRayCastData data;
1493 BVHNode * root = tree->nodes[tree->totleaf];
1497 data.callback = callback;
1498 data.userdata = userdata;
1500 copy_v3_v3(data.ray.origin, co);
1501 copy_v3_v3(data.ray.direction, dir);
1502 data.ray.radius = radius;
1504 normalize_v3(data.ray.direction);
1506 for (i=0; i<3; i++) {
1507 data.ray_dot_axis[i] = dot_v3v3(data.ray.direction, KDOP_AXES[i]);
1508 data.idot_axis[i] = 1.0f / data.ray_dot_axis[i];
1510 if (fabsf(data.ray_dot_axis[i]) < FLT_EPSILON) {
1511 data.ray_dot_axis[i] = 0.0;
1513 data.index[2*i] = data.idot_axis[i] < 0.0f ? 1 : 0;
1514 data.index[2*i+1] = 1 - data.index[2*i];
1515 data.index[2*i] += 2*i;
1516 data.index[2*i+1] += 2*i;
1521 memcpy(&data.hit, hit, sizeof(*hit));
1523 data.hit.index = -1;
1524 data.hit.dist = FLT_MAX;
1528 dfs_raycast(&data, root);
1529 // iterative_raycast(&data, root);
1534 memcpy(hit, &data.hit, sizeof(*hit));
1536 return data.hit.index;
1539 float BLI_bvhtree_bb_raycast(float *bv, const float light_start[3], const float light_end[3], float pos[3])
1541 BVHRayCastData data;
1544 data.hit.dist = FLT_MAX;
1546 // get light direction
1547 data.ray.direction[0] = light_end[0] - light_start[0];
1548 data.ray.direction[1] = light_end[1] - light_start[1];
1549 data.ray.direction[2] = light_end[2] - light_start[2];
1551 data.ray.radius = 0.0;
1553 data.ray.origin[0] = light_start[0];
1554 data.ray.origin[1] = light_start[1];
1555 data.ray.origin[2] = light_start[2];
1557 normalize_v3(data.ray.direction);
1558 copy_v3_v3(data.ray_dot_axis, data.ray.direction);
1560 dist = ray_nearest_hit(&data, bv);
1563 madd_v3_v3v3fl(pos, light_start, data.ray.direction, dist);
1570 * Range Query - as request by broken :P
1572 * Allocs and fills an array with the indexs of node that are on the given spherical range (center, radius)
1573 * Returns the size of the array.
1575 typedef struct RangeQueryData
1578 const float *center;
1579 float radius; //squared radius
1583 BVHTree_RangeQuery callback;
1590 static void dfs_range_query(RangeQueryData *data, BVHNode *node)
1592 if (node->totnode == 0) {
1594 //Calculate the node min-coords (if the node was a point then this is the point coordinates)
1596 co[0] = node->bv[0];
1597 co[1] = node->bv[2];
1598 co[2] = node->bv[4];
1603 for (i=0; i != node->totnode; i++) {
1605 float dist = calc_nearest_point(data->center, node->children[i], nearest);
1606 if (dist < data->radius) {
1607 //Its a leaf.. call the callback
1608 if (node->children[i]->totnode == 0) {
1610 data->callback(data->userdata, node->children[i]->index, dist);
1613 dfs_range_query(data, node->children[i]);
1619 int BLI_bvhtree_range_query(BVHTree *tree, const float co[3], float radius, BVHTree_RangeQuery callback, void *userdata)
1621 BVHNode * root = tree->nodes[tree->totleaf];
1623 RangeQueryData data;
1626 data.radius = radius*radius;
1629 data.callback = callback;
1630 data.userdata = userdata;
1634 float dist = calc_nearest_point(data.center, root, nearest);
1635 if (dist < data.radius) {
1636 /* Its a leaf.. call the callback */
1637 if (root->totnode == 0) {
1639 data.callback(data.userdata, root->index, dist);
1642 dfs_range_query(&data, root);