diff options
Diffstat (limited to 'source/blender/blenlib')
-rw-r--r-- | source/blender/blenlib/BLI_kdopbvh.h | 36 | ||||
-rw-r--r-- | source/blender/blenlib/intern/BLI_kdopbvh.c | 1025 |
2 files changed, 800 insertions, 261 deletions
diff --git a/source/blender/blenlib/BLI_kdopbvh.h b/source/blender/blenlib/BLI_kdopbvh.h index b81ff0ee66f..6d9a17efebf 100644 --- a/source/blender/blenlib/BLI_kdopbvh.h +++ b/source/blender/blenlib/BLI_kdopbvh.h @@ -1,5 +1,7 @@ /** * + * $Id$ + * * ***** BEGIN GPL LICENSE BLOCK ***** * * This program is free software; you can redistribute it and/or @@ -40,6 +42,35 @@ typedef struct BVHTreeOverlap { int indexB; } BVHTreeOverlap; +typedef struct BVHTreeNearest +{ + int index; /* the index of the nearest found (untouched if none is found within a dist radius from the given coordinates) */ + float co[3]; /* nearest coordinates (untouched it none is found within a dist radius from the given coordinates) */ + float no[3]; /* normal at nearest coordinates (untouched it none is found within a dist radius from the given coordinates) */ + float dist; /* squared distance to search arround */ +} BVHTreeNearest; + +typedef struct BVHTreeRay +{ + float origin[3]; /* ray origin */ + float direction[3]; /* ray direction */ +} BVHTreeRay; + +typedef struct BVHTreeRayHit +{ + int index; /* index of the tree node (untouched if no hit is found) */ + float co[3]; /* coordinates of the hit point */ + float no[3]; /* normal on hit point */ + float dist; /* distance to the hit point */ +} BVHTreeRayHit; + +/* callback must update nearest in case it finds a nearest result */ +typedef void (*BVHTree_NearestPointCallback) (void *userdata, int index, const float *co, BVHTreeNearest *nearest); + +/* callback must update hit in case it finds a nearest successful hit */ +typedef void (*BVHTree_RayCastCallback) (void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit); + + BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis); void BLI_bvhtree_free(BVHTree *tree); @@ -56,5 +87,10 @@ BVHTreeOverlap *BLI_bvhtree_overlap(BVHTree *tree1, BVHTree *tree2, int *result) float BLI_bvhtree_getepsilon(BVHTree *tree); +/* find nearest node to the given coordinates (if nearest is given it will only search nodes where square distance is smaller than nearest->dist) */ +int BLI_bvhtree_find_nearest(BVHTree *tree, const float *co, BVHTreeNearest *nearest, BVHTree_NearestPointCallback callback, void *userdata); + +int BLI_bvhtree_ray_cast(BVHTree *tree, const float *co, const float *dir, BVHTreeRayHit *hit, BVHTree_RayCastCallback callback, void *userdata); + #endif // BLI_KDOPBVH_H diff --git a/source/blender/blenlib/intern/BLI_kdopbvh.c b/source/blender/blenlib/intern/BLI_kdopbvh.c index 4ceb9762a7b..9671551a7f1 100644 --- a/source/blender/blenlib/intern/BLI_kdopbvh.c +++ b/source/blender/blenlib/intern/BLI_kdopbvh.c @@ -28,8 +28,9 @@ #include "math.h" #include <stdio.h> -#include <stdlib.h> +#include <stdlib.h> #include <string.h> +#include <assert.h> #include "MEM_guardedalloc.h" @@ -42,15 +43,17 @@ #include <omp.h> #endif + + +#define MAX_TREETYPE 32 + typedef struct BVHNode { - struct BVHNode **children; // max 8 children - struct BVHNode *parent; // needed for bottom - top update - float *bv; // Bounding volume of all nodes, max 13 axis - int index; /* face, edge, vertex index */ - char totnode; // how many nodes are used, used for speedup - char traversed; // how many nodes already traversed until this level? - char main_axis; + struct BVHNode **children; + float *bv; // Bounding volume of all nodes, max 13 axis + int index; // face, edge, vertex index + char totnode; // how many nodes are used, used for speedup + char main_axis; // Axis used to split this node } BVHNode; struct BVHTree @@ -72,8 +75,34 @@ typedef struct BVHOverlapData BVHTree *tree1, *tree2; BVHTreeOverlap *overlap; int i, max_overlap; /* i is number of overlaps */ + int start_axis, stop_axis; } BVHOverlapData; -//////////////////////////////////////// + +typedef struct BVHNearestData +{ + BVHTree *tree; + float *co; + BVHTree_NearestPointCallback callback; + void *userdata; + float proj[13]; //coordinates projection over axis + BVHTreeNearest nearest; + +} BVHNearestData; + +typedef struct BVHRayCastData +{ + BVHTree *tree; + + BVHTree_RayCastCallback callback; + void *userdata; + + + BVHTreeRay ray; + float ray_dot_axis[13]; + + BVHTreeRayHit hit; +} BVHRayCastData; +////////////////////////////////////////m //////////////////////////////////////////////////////////////////////// @@ -244,7 +273,7 @@ int partition_nth_element(BVHNode **a, int _begin, int _end, int n, int axis){ int begin = _begin, end = _end, cut; while(end-begin > 3) { - cut = bvh_partition(a, begin, end, bvh_medianof3(a, begin, (begin+end)/2, end-1, axis), axis ); + cut = bvh_partition(a, begin, end, bvh_medianof3(a, begin, (begin+end)/2, end-1, axis), axis ); if(cut <= n) begin = cut; else @@ -255,124 +284,15 @@ int partition_nth_element(BVHNode **a, int _begin, int _end, int n, int axis){ return n; } - ////////////////////////////////////////////////////////////////////////////////////////////////////// -void BLI_bvhtree_free(BVHTree *tree) -{ - if(tree) - { - MEM_freeN(tree->nodes); - MEM_freeN(tree->nodearray); - MEM_freeN(tree->nodebv); - MEM_freeN(tree->nodechild); - MEM_freeN(tree); - } -} - -BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis) -{ - BVHTree *tree; - int numbranches=0, i; - - // only support up to octree - if(tree_type > 8) - return NULL; - - tree = (BVHTree *)MEM_callocN(sizeof(BVHTree), "BVHTree"); - - if(tree) - { - tree->epsilon = epsilon; - tree->tree_type = tree_type; - tree->axis = axis; - - if(axis == 26) - { - tree->start_axis = 0; - tree->stop_axis = 13; - } - else if(axis == 18) - { - tree->start_axis = 7; - tree->stop_axis = 13; - } - else if(axis == 14) - { - tree->start_axis = 0; - tree->stop_axis = 7; - } - else if(axis == 8) // AABB - { - tree->start_axis = 0; - tree->stop_axis = 4; - } - else if(axis == 6) // OBB - { - tree->start_axis = 0; - tree->stop_axis = 3; - } - else - { - MEM_freeN(tree); - return NULL; - } - - - // calculate max number of branches, our bvh kdop is "almost perfect" - for(i = 1; i <= (int)ceil((float)((float)log(maxsize)/(float)log(tree_type))); i++) - numbranches += (pow(tree_type, i) / tree_type); - - tree->nodes = (BVHNode **)MEM_callocN(sizeof(BVHNode *)*(numbranches+maxsize + tree_type), "BVHNodes"); - - if(!tree->nodes) - { - MEM_freeN(tree); - return NULL; - } - - tree->nodebv = (float*)MEM_callocN(sizeof(float)* axis * (numbranches+maxsize + tree_type), "BVHNodeBV"); - if(!tree->nodebv) - { - MEM_freeN(tree->nodes); - MEM_freeN(tree); - } - - tree->nodechild = (BVHNode**)MEM_callocN(sizeof(BVHNode*) * tree_type * (numbranches+maxsize + tree_type), "BVHNodeBV"); - if(!tree->nodechild) - { - MEM_freeN(tree->nodebv); - MEM_freeN(tree->nodes); - MEM_freeN(tree); - } - - tree->nodearray = (BVHNode *)MEM_callocN(sizeof(BVHNode)*(numbranches+maxsize + tree_type), "BVHNodeArray"); - - if(!tree->nodearray) - { - MEM_freeN(tree->nodechild); - MEM_freeN(tree->nodebv); - MEM_freeN(tree->nodes); - MEM_freeN(tree); - return NULL; - } - - //link the dynamic bv and child links - for(i=0; i< numbranches+maxsize + tree_type; i++) - { - tree->nodearray[i].bv = tree->nodebv + i * axis; - tree->nodearray[i].children = tree->nodechild + i * tree_type; - } - - } - - return tree; -} - - +/* + * BVHTree bounding volumes functions + */ static void create_kdop_hull(BVHTree *tree, BVHNode *node, float *co, int numpoints, int moving) { float newminmax; + float *bv = node->bv; int i, k; // don't init boudings for the moving case @@ -380,8 +300,8 @@ static void create_kdop_hull(BVHTree *tree, BVHNode *node, float *co, int numpoi { for (i = tree->start_axis; i < tree->stop_axis; i++) { - node->bv[2*i] = FLT_MAX; - node->bv[2*i + 1] = -FLT_MAX; + bv[2*i] = FLT_MAX; + bv[2*i + 1] = -FLT_MAX; } } @@ -391,10 +311,10 @@ static void create_kdop_hull(BVHTree *tree, BVHNode *node, float *co, int numpoi for (i = tree->start_axis; i < tree->stop_axis; i++) { newminmax = INPR(&co[k * 3], KDOP_AXES[i]); - if (newminmax < node->bv[2 * i]) - node->bv[2 * i] = newminmax; - if (newminmax > node->bv[(2 * i) + 1]) - node->bv[(2 * i) + 1] = newminmax; + if (newminmax < bv[2 * i]) + bv[2 * i] = newminmax; + if (newminmax > bv[(2 * i) + 1]) + bv[(2 * i) + 1] = newminmax; } } } @@ -405,6 +325,7 @@ static void refit_kdop_hull(BVHTree *tree, BVHNode *node, int start, int end) float newmin,newmax; int i, j; float *bv = node->bv; + for (i = tree->start_axis; i < tree->stop_axis; i++) { @@ -426,37 +347,7 @@ static void refit_kdop_hull(BVHTree *tree, BVHNode *node, int start, int end) bv[(2 * i) + 1] = newmax; } } -} -int BLI_bvhtree_insert(BVHTree *tree, int index, float *co, int numpoints) -{ - BVHNode *node= NULL; - int i; - - // insert should only possible as long as tree->totbranch is 0 - if(tree->totbranch > 0) - return 0; - - if(tree->totleaf+1 >= MEM_allocN_len(tree->nodes)) - return 0; - - // TODO check if have enough nodes in array - - node = tree->nodes[tree->totleaf] = &(tree->nodearray[tree->totleaf]); - tree->totleaf++; - - create_kdop_hull(tree, node, co, numpoints, 0); - - // inflate the bv with some epsilon - for (i = tree->start_axis; i < tree->stop_axis; i++) - { - node->bv[(2 * i)] -= tree->epsilon; // minimum - node->bv[(2 * i) + 1] += tree->epsilon; // maximum - } - - node->index= index; - - return 1; } // only supports x,y,z axis in the moment @@ -484,45 +375,76 @@ static char get_largest_axis(float *bv) } } -static void bvh_div_nodes(BVHTree *tree, BVHNode *node, int start, int end) +// bottom-up update of bvh node BV +// join the children on the parent BV +static void node_join(BVHTree *tree, BVHNode *node) { - int i, tend; - BVHNode *tnode; - int slice = (end-start+tree->tree_type-1)/tree->tree_type; //division rounded up + int i, j; - // Determine which axis to split along - char laxis = get_largest_axis(node->bv); + for (i = tree->start_axis; i < tree->stop_axis; i++) + { + node->bv[2*i] = FLT_MAX; + node->bv[2*i + 1] = -FLT_MAX; + } - // split nodes along longest axis - for (i=0; start < end; start += slice, i++) //i counts the current child - { - tend = start + slice; - - if(tend > end) tend = end; - - if(tend-start == 1) // ok, we have 1 left for this node + for (i = 0; i < tree->tree_type; i++) + { + if (node->children[i]) { - node->children[i] = tree->nodes[start]; - node->children[i]->parent = node; + for (j = tree->start_axis; j < tree->stop_axis; j++) + { + // update minimum + if (node->children[i]->bv[(2 * j)] < node->bv[(2 * j)]) + node->bv[(2 * j)] = node->children[i]->bv[(2 * j)]; + + // update maximum + if (node->children[i]->bv[(2 * j) + 1] > node->bv[(2 * j) + 1]) + node->bv[(2 * j) + 1] = node->children[i]->bv[(2 * j) + 1]; + } } else - { - tnode = node->children[i] = tree->nodes[tree->totleaf + tree->totbranch] = &(tree->nodearray[tree->totbranch + tree->totleaf]); - tree->totbranch++; - tnode->parent = node; - - if(tend != end) - partition_nth_element(tree->nodes, start, end, tend, laxis); - refit_kdop_hull(tree, tnode, start, tend); - bvh_div_nodes(tree, tnode, start, tend); - } - node->totnode++; + break; } - - return; +} + +/* + * Debug and information functions + */ +static void bvhtree_print_tree(BVHTree *tree, BVHNode *node, int depth) +{ + int i; + for(i=0; i<depth; i++) printf(" "); + printf(" - %d (%d): ", node->index, node - tree->nodearray); + for(i=2*tree->start_axis; i<2*tree->stop_axis; i++) + printf("%.3f ", node->bv[i]); + printf("\n"); + + for(i=0; i<tree->tree_type; i++) + if(node->children[i]) + bvhtree_print_tree(tree, node->children[i], depth+1); +} + +static void bvhtree_info(BVHTree *tree) +{ + printf("BVHTree info\n"); + printf("tree_type = %d, axis = %d, epsilon = %f\n", tree->tree_type, tree->axis, tree->epsilon); + printf("nodes = %d, branches = %d, leafs = %d\n", tree->totbranch + tree->totleaf, tree->totbranch, tree->totleaf); + printf("Memory per node = %dbytes\n", sizeof(BVHNode) + sizeof(BVHNode*)*tree->tree_type + sizeof(float)*tree->axis); + printf("BV memory = %dbytes\n", MEM_allocN_len(tree->nodebv)); + + printf("Total memory = %dbytes\n", sizeof(BVHTree) + + MEM_allocN_len(tree->nodes) + + MEM_allocN_len(tree->nodearray) + + MEM_allocN_len(tree->nodechild) + + MEM_allocN_len(tree->nodebv) + ); + +// bvhtree_print_tree(tree, tree->nodes[tree->totleaf], 0); } #if 0 + + static void verify_tree(BVHTree *tree) { int i, j, check = 0; @@ -570,29 +492,445 @@ static void verify_tree(BVHTree *tree) printf("branches: %d, leafs: %d, total: %d\n", tree->totbranch, tree->totleaf, tree->totbranch + tree->totleaf); } #endif + +//Helper data and structures to build a min-leaf generalized implicit tree +//This code can be easily reduced (basicly this is only method to calculate pow(k, n) in O(1).. and stuff like that) +typedef struct BVHBuildHelper +{ + int tree_type; // + int totleafs; // + + int leafs_per_child [32]; //Min number of leafs that are archievable from a node at depth N + int branches_on_level[32]; //Number of nodes at depth N (tree_type^N) + + int remain_leafs; //Number of leafs that are placed on the level that is not 100% filled + +} BVHBuildHelper; + +static void build_implicit_tree_helper(BVHTree *tree, BVHBuildHelper *data) +{ + int depth = 0; + int remain; + int nnodes; + + data->totleafs = tree->totleaf; + data->tree_type= tree->tree_type; + + //Calculate the smallest tree_type^n such that tree_type^n >= num_leafs + for( + data->leafs_per_child[0] = 1; + data->leafs_per_child[0] < data->totleafs; + data->leafs_per_child[0] *= data->tree_type + ); + + data->branches_on_level[0] = 1; + + //We could stop the loop first (but I am lazy to find out when) + for(depth = 1; depth < 32; depth++) + { + data->branches_on_level[depth] = data->branches_on_level[depth-1] * data->tree_type; + data->leafs_per_child [depth] = data->leafs_per_child [depth-1] / data->tree_type; + } + + remain = data->totleafs - data->leafs_per_child[1]; + nnodes = (remain + data->tree_type - 2) / (data->tree_type - 1); + data->remain_leafs = remain + nnodes; +} + +// return the min index of all the leafs archivable with the given branch +static int implicit_leafs_index(BVHBuildHelper *data, int depth, int child_index) +{ + int min_leaf_index = child_index * data->leafs_per_child[depth-1]; + if(min_leaf_index <= data->remain_leafs) + return min_leaf_index; + else if(data->leafs_per_child[depth]) + return data->totleafs - (data->branches_on_level[depth-1] - child_index) * data->leafs_per_child[depth]; + else + return data->remain_leafs; +} + +/** + * Generalized implicit tree build + * + * An implicit tree is a tree where its structure is implied, thus there is no need to store child pointers or indexs. + * Its possible to find the position of the child or the parent with simple maths (multiplication and adittion). This type + * of tree is for example used on heaps.. where node N has its childs at indexs N*2 and N*2+1. + * + * Altought in this case the tree type is general.. and not know until runtime. + * tree_type stands for the maximum number of childs that a tree node can have. + * All tree types >= 2 are supported. + * + * Advantages of the used trees include: + * - No need to store child/parent relations (they are implicit); + * - Any node child always has an index greater than the parent; + * - Brother nodes are sequencial in memory; + * + * + * Some math relations derived for general implicit trees: + * + * K = tree_type, ( 2 <= K ) + * ROOT = 1 + * N child of node A = A * K + (2 - K) + N, (0 <= N < K) + * + * Util methods: + * TODO... + * (looping elements, knowing if its a leaf or not.. etc...) + */ + +// This functions returns the number of branches needed to have the requested number of leafs. +static int implicit_needed_branches(int tree_type, int leafs) +{ + return MAX2(1, (leafs + tree_type - 3) / (tree_type-1) ); +} + +/* + * This function handles the problem of "sorting" the leafs (along the split_axis). + * + * It arranges the elements in the given partitions such that: + * - any element in partition N is less or equal to any element in partition N+1. + * - if all elements are diferent all partition will get the same subset of elements + * as if the array was sorted. + * + * partition P is described as the elements in the range ( nth[P] , nth[P+1] ] + * + * TODO: This can be optimized a bit by doing a specialized nth_element instead of K nth_elements + */ +static void split_leafs(BVHNode **leafs_array, int *nth, int partitions, int split_axis) +{ + int i; + for(i=0; i < partitions-1; i++) + { + if(nth[i] >= nth[partitions]) + break; + + partition_nth_element(leafs_array, nth[i], nth[partitions], nth[i+1], split_axis); + } +} + +/* + * This functions builds an optimal implicit tree from the given leafs. + * Where optimal stands for: + * - The resulting tree will have the smallest number of branches; + * - At most only one branch will have NULL childs; + * - All leafs will be stored at level N or N+1. + * + * This function creates an implicit tree on branches_array, the leafs are given on the leafs_array. + * + * The tree is built per depth levels. First branchs at depth 1.. then branches at depth 2.. etc.. + * The reason is that we can build level N+1 from level N witouth any data dependencies.. thus it allows + * to use multithread building. + * + * To archieve this is necessary to find how much leafs are accessible from a certain branch, BVHBuildHelper + * implicit_needed_branches and implicit_leafs_index are auxiliar functions to solve that "optimal-split". + */ +static void non_recursive_bvh_div_nodes(BVHTree *tree, BVHNode *branches_array, BVHNode **leafs_array, int num_leafs) +{ + int i; + + const int tree_type = tree->tree_type; + const int tree_offset = 2 - tree->tree_type; //this value is 0 (on binary trees) and negative on the others + const int num_branches= implicit_needed_branches(tree_type, num_leafs); + + BVHBuildHelper data; + int depth; + + branches_array--; //Implicit trees use 1-based indexs + + build_implicit_tree_helper(tree, &data); + + //Loop tree levels (log N) loops + for(i=1, depth = 1; i <= num_branches; i = i*tree_type + tree_offset, depth++) + { + const int first_of_next_level = i*tree_type + tree_offset; + const int end_j = MIN2(first_of_next_level, num_branches + 1); //index of last branch on this level + int j; + + //Loop all branches on this level +#pragma omp parallel for private(j) schedule(static) + for(j = i; j < end_j; j++) + { + int k; + const int parent_level_index= j-i; + BVHNode* parent = branches_array + j; + int nth_positions[ MAX_TREETYPE + 1]; + char split_axis; + + int parent_leafs_begin = implicit_leafs_index(&data, depth, parent_level_index); + int parent_leafs_end = implicit_leafs_index(&data, depth, parent_level_index+1); + + //This calculates the bounding box of this branch + //and chooses the largest axis as the axis to divide leafs + refit_kdop_hull(tree, parent, parent_leafs_begin, parent_leafs_end); + split_axis = get_largest_axis(parent->bv); + + //Save split axis (this can be used on raytracing to speedup the query time) + parent->main_axis = split_axis / 2; + + //Split the childs along the split_axis, note: its not needed to sort the whole leafs array + //Only to assure that the elements are partioned on a way that each child takes the elements + //it would take in case the whole array was sorted. + //Split_leafs takes care of that "sort" problem. + nth_positions[ 0] = parent_leafs_begin; + nth_positions[tree_type] = parent_leafs_end; + for(k = 1; k < tree_type; k++) + { + int child_index = j * tree_type + tree_offset + k; + int child_level_index = child_index - first_of_next_level; //child level index + nth_positions[k] = implicit_leafs_index(&data, depth+1, child_level_index); + } + + split_leafs(leafs_array, nth_positions, tree_type, split_axis); + + + //Setup children and totnode counters + //Not really needed but currently most of BVH code relies on having an explicit children structure + for(k = 0; k < tree_type; k++) + { + int child_index = j * tree_type + tree_offset + k; + int child_level_index = child_index - first_of_next_level; //child level index + + int child_leafs_begin = implicit_leafs_index(&data, depth+1, child_level_index); + int child_leafs_end = implicit_leafs_index(&data, depth+1, child_level_index+1); + + if(child_leafs_end - child_leafs_begin > 1) + parent->children[k] = branches_array + child_index; + else if(child_leafs_end - child_leafs_begin == 1) + parent->children[k] = leafs_array[ child_leafs_begin ]; + else + break; + + parent->totnode = k+1; + } + } + } +} + + +/* + * BLI_bvhtree api + */ +BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis) +{ + BVHTree *tree; + int numnodes, i; + // theres not support for trees below binary-trees :P + if(tree_type < 2) + return NULL; + + if(tree_type > MAX_TREETYPE) + return NULL; + + tree = (BVHTree *)MEM_callocN(sizeof(BVHTree), "BVHTree"); + + if(tree) + { + tree->epsilon = epsilon; + tree->tree_type = tree_type; + tree->axis = axis; + + if(axis == 26) + { + tree->start_axis = 0; + tree->stop_axis = 13; + } + else if(axis == 18) + { + tree->start_axis = 7; + tree->stop_axis = 13; + } + else if(axis == 14) + { + tree->start_axis = 0; + tree->stop_axis = 7; + } + else if(axis == 8) // AABB + { + tree->start_axis = 0; + tree->stop_axis = 4; + } + else if(axis == 6) // OBB + { + tree->start_axis = 0; + tree->stop_axis = 3; + } + else + { + MEM_freeN(tree); + return NULL; + } + + + //Allocate arrays + numnodes = maxsize + implicit_needed_branches(tree_type, maxsize) + tree_type; + + tree->nodes = (BVHNode **)MEM_callocN(sizeof(BVHNode *)*numnodes, "BVHNodes"); + + if(!tree->nodes) + { + MEM_freeN(tree); + return NULL; + } + + tree->nodebv = (float*)MEM_callocN(sizeof(float)* axis * numnodes, "BVHNodeBV"); + if(!tree->nodebv) + { + MEM_freeN(tree->nodes); + MEM_freeN(tree); + } + + tree->nodechild = (BVHNode**)MEM_callocN(sizeof(BVHNode*) * tree_type * numnodes, "BVHNodeBV"); + if(!tree->nodechild) + { + MEM_freeN(tree->nodebv); + MEM_freeN(tree->nodes); + MEM_freeN(tree); + } + + tree->nodearray = (BVHNode *)MEM_callocN(sizeof(BVHNode)* numnodes, "BVHNodeArray"); + + if(!tree->nodearray) + { + MEM_freeN(tree->nodechild); + MEM_freeN(tree->nodebv); + MEM_freeN(tree->nodes); + MEM_freeN(tree); + return NULL; + } + + //link the dynamic bv and child links + for(i=0; i< numnodes; i++) + { + tree->nodearray[i].bv = tree->nodebv + i * axis; + tree->nodearray[i].children = tree->nodechild + i * tree_type; + } + + } + + return tree; +} + +void BLI_bvhtree_free(BVHTree *tree) +{ + if(tree) + { + MEM_freeN(tree->nodes); + MEM_freeN(tree->nodearray); + MEM_freeN(tree->nodebv); + MEM_freeN(tree->nodechild); + MEM_freeN(tree); + } +} + void BLI_bvhtree_balance(BVHTree *tree) { - BVHNode *node; + int i; + + BVHNode* branches_array = tree->nodearray + tree->totleaf; + BVHNode** leafs_array = tree->nodes; + + //This function should only be called once (some big bug goes here if its being called more than once per tree) + assert(tree->totbranch == 0); + + //Build the implicit tree + non_recursive_bvh_div_nodes(tree, branches_array, leafs_array, tree->totleaf); + + //current code expects the branches to be linked to the nodes array + //we perform that linkage here + tree->totbranch = implicit_needed_branches(tree->tree_type, tree->totleaf); + for(i = 0; i < tree->totbranch; i++) + tree->nodes[tree->totleaf + i] = branches_array + i; + + //bvhtree_info(tree); +} + +int BLI_bvhtree_insert(BVHTree *tree, int index, float *co, int numpoints) +{ + int i; + BVHNode *node = NULL; - if(tree->totleaf == 0) - return; + // insert should only possible as long as tree->totbranch is 0 + if(tree->totbranch > 0) + return 0; + + if(tree->totleaf+1 >= MEM_allocN_len(tree->nodes)/sizeof(*(tree->nodes))) + return 0; + + // TODO check if have enough nodes in array - // create root node node = tree->nodes[tree->totleaf] = &(tree->nodearray[tree->totleaf]); - tree->totbranch++; + tree->totleaf++; + + create_kdop_hull(tree, node, co, numpoints, 0); + node->index= index; + + // inflate the bv with some epsilon + for (i = tree->start_axis; i < tree->stop_axis; i++) + { + node->bv[(2 * i)] -= tree->epsilon; // minimum + node->bv[(2 * i) + 1] += tree->epsilon; // maximum + } + + return 1; +} + + +// call before BLI_bvhtree_update_tree() +int BLI_bvhtree_update_node(BVHTree *tree, int index, float *co, float *co_moving, int numpoints) +{ + int i; + BVHNode *node= NULL; + + // check if index exists + if(index > tree->totleaf) + return 0; - // refit root bvh node - refit_kdop_hull(tree, tree->nodes[tree->totleaf], 0, tree->totleaf); - // create + balance tree - bvh_div_nodes(tree, tree->nodes[tree->totleaf], 0, tree->totleaf); + node = tree->nodearray + index; + + create_kdop_hull(tree, node, co, numpoints, 0); - // verify_tree(tree); + if(co_moving) + create_kdop_hull(tree, node, co_moving, numpoints, 1); + + // inflate the bv with some epsilon + for (i = tree->start_axis; i < tree->stop_axis; i++) + { + node->bv[(2 * i)] -= tree->epsilon; // minimum + node->bv[(2 * i) + 1] += tree->epsilon; // maximum + } + + return 1; +} + +// call BLI_bvhtree_update_node() first for every node/point/triangle +void BLI_bvhtree_update_tree(BVHTree *tree) +{ + //Update bottom=>top + //TRICKY: the way we build the tree all the childs have an index greater than the parent + //This allows us todo a bottom up update by starting on the biger numbered branch + + BVHNode** root = tree->nodes + tree->totleaf; + BVHNode** index = tree->nodes + tree->totleaf + tree->totbranch-1; + + for (; index >= root; index--) + node_join(tree, *index); } +float BLI_bvhtree_getepsilon(BVHTree *tree) +{ + return tree->epsilon; +} + + +/* + * BLI_bvhtree_overlap + */ // overlap - is it possbile for 2 bv's to collide ? -static int tree_overlap(float *bv1, float *bv2, int start_axis, int stop_axis) +static int tree_overlap(BVHNode *node1, BVHNode *node2, int start_axis, int stop_axis) { + float *bv1 = node1->bv; + float *bv2 = node2->bv; + float *bv1_end = bv1 + (stop_axis<<1); bv1 += start_axis<<1; @@ -612,7 +950,7 @@ static void traverse(BVHOverlapData *data, BVHNode *node1, BVHNode *node2) { int j; - if(tree_overlap(node1->bv, node2->bv, MIN2(data->tree1->start_axis, data->tree2->start_axis), MIN2(data->tree1->stop_axis, data->tree2->stop_axis))) + if(tree_overlap(node1, node2, data->start_axis, data->stop_axis)) { // check if node1 is a leaf if(!node1->totnode) @@ -678,7 +1016,7 @@ BVHTreeOverlap *BLI_bvhtree_overlap(BVHTree *tree1, BVHTree *tree2, int *result) return 0; // fast check root nodes for collision before doing big splitting + traversal - if(!tree_overlap(tree1->nodes[tree1->totleaf]->bv, tree2->nodes[tree2->totleaf]->bv, MIN2(tree1->start_axis, tree2->start_axis), MIN2(tree1->stop_axis, tree2->stop_axis))) + 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))) return 0; data = MEM_callocN(sizeof(BVHOverlapData *)* tree1->tree_type, "BVHOverlapData_star"); @@ -693,6 +1031,8 @@ BVHTreeOverlap *BLI_bvhtree_overlap(BVHTree *tree1, BVHTree *tree2, int *result) data[j]->tree2 = tree2; data[j]->max_overlap = MAX2(tree1->totleaf, tree2->totleaf); data[j]->i = 0; + data[j]->start_axis = MIN2(tree1->start_axis, tree2->start_axis); + data[j]->stop_axis = MIN2(tree1->stop_axis, tree2->stop_axis ); } #pragma omp parallel for private(j) schedule(static) @@ -724,88 +1064,251 @@ BVHTreeOverlap *BLI_bvhtree_overlap(BVHTree *tree1, BVHTree *tree2, int *result) } -// bottom up update of bvh tree: -// join the 4 children here -static void node_join(BVHTree *tree, BVHNode *node) +/* + * Nearest neighbour - BLI_bvhtree_find_nearest + */ +static float squared_dist(const float *a, const float *b) { - int i, j; - - for (i = tree->start_axis; i < tree->stop_axis; i++) + float tmp[3]; + VECSUB(tmp, a, b); + return INPR(tmp, tmp); +} + +//Determines the nearest point of the given node BV. Returns the squared distance to that point. +static float calc_nearest_point(BVHNearestData *data, BVHNode *node, float *nearest) +{ + int i; + const float *bv = node->bv; + + //nearest on AABB hull + for(i=0; i != 3; i++, bv += 2) { - node->bv[2*i] = FLT_MAX; - node->bv[2*i + 1] = -FLT_MAX; + if(bv[0] > data->proj[i]) + nearest[i] = bv[0]; + else if(bv[1] < data->proj[i]) + nearest[i] = bv[1]; + else + nearest[i] = data->proj[i]; } - - for (i = 0; i < tree->tree_type; i++) + +/* + //nearest on a general hull + VECCOPY(nearest, data->co); + for(i = data->tree->start_axis; i != data->tree->stop_axis; i++, bv+=2) { - if (node->children[i]) + float proj = INPR( nearest, KDOP_AXES[i]); + float dl = bv[0] - proj; + float du = bv[1] - proj; + + if(dl > 0) { - for (j = tree->start_axis; j < tree->stop_axis; j++) - { - // update minimum - if (node->children[i]->bv[(2 * j)] < node->bv[(2 * j)]) - node->bv[(2 * j)] = node->children[i]->bv[(2 * j)]; - - // update maximum - if (node->children[i]->bv[(2 * j) + 1] > node->bv[(2 * j) + 1]) - node->bv[(2 * j) + 1] = node->children[i]->bv[(2 * j) + 1]; - } + VECADDFAC(nearest, nearest, KDOP_AXES[i], dl); + } + else if(du < 0) + { + VECADDFAC(nearest, nearest, KDOP_AXES[i], du); } + } +*/ + return squared_dist(data->co, nearest); +} + + +// TODO: use a priority queue to reduce the number of nodes looked on +static void dfs_find_nearest(BVHNearestData *data, BVHNode *node) +{ + int i; + float nearest[3], sdist; + + sdist = calc_nearest_point(data, node, nearest); + if(sdist >= data->nearest.dist) return; + + if(node->totnode == 0) + { + if(data->callback) + data->callback(data->userdata , node->index, data->co, &data->nearest); else - break; + { + data->nearest.index = node->index; + VECCOPY(data->nearest.co, nearest); + data->nearest.dist = sdist; + } + } + else + { + for(i=0; i != node->totnode; i++) + dfs_find_nearest(data, node->children[i]); } } -// call before BLI_bvhtree_update_tree() -int BLI_bvhtree_update_node(BVHTree *tree, int index, float *co, float *co_moving, int numpoints) +int BLI_bvhtree_find_nearest(BVHTree *tree, const float *co, BVHTreeNearest *nearest, BVHTree_NearestPointCallback callback, void *userdata) { - BVHNode *node= NULL; - int i = 0; - - // check if index exists - if(index > tree->totleaf) - return 0; - - node = tree->nodearray + index; - - create_kdop_hull(tree, node, co, numpoints, 0); - - if(co_moving) - create_kdop_hull(tree, node, co_moving, numpoints, 1); - - // inflate the bv with some epsilon - for (i = tree->start_axis; i < tree->stop_axis; i++) + int i; + + BVHNearestData data; + BVHNode* root = tree->nodes[tree->totleaf]; + + //init data to search + data.tree = tree; + data.co = co; + + data.callback = callback; + data.userdata = userdata; + + for(i = data.tree->start_axis; i != data.tree->stop_axis; i++) { - node->bv[(2 * i)] -= tree->epsilon; // minimum - node->bv[(2 * i) + 1] += tree->epsilon; // maximum + data.proj[i] = INPR(data.co, KDOP_AXES[i]); } - - return 1; + + if(nearest) + { + memcpy( &data.nearest , nearest, sizeof(*nearest) ); + } + else + { + data.nearest.index = -1; + data.nearest.dist = FLT_MAX; + } + + //dfs search + if(root) + dfs_find_nearest(&data, root); + + //copy back results + if(nearest) + { + memcpy(nearest, &data.nearest, sizeof(*nearest)); + } + + return data.nearest.index; } -// call BLI_bvhtree_update_node() first for every node/point/triangle -void BLI_bvhtree_update_tree(BVHTree *tree) + +/* + * Raycast - BLI_bvhtree_ray_cast + * + * raycast is done by performing a DFS on the BVHTree and saving the closest hit + */ + +//Determines the distance that the ray must travel to hit the bounding volume of the given node +static float ray_nearest_hit(BVHRayCastData *data, BVHNode *node) { - BVHNode *leaf, *parent; - - // reset tree traversing flag - for (leaf = tree->nodearray + tree->totleaf; leaf != tree->nodearray + tree->totleaf + tree->totbranch; leaf++) - leaf->traversed = 0; + int i; + const float *bv = node->bv; + + float low = 0, upper = data->hit.dist; + + for(i=0; i != 3; i++, bv += 2) + { + if(data->ray_dot_axis[i] == 0.0f) + { + //axis aligned ray + if(data->ray.origin[i] < bv[0] + || data->ray.origin[i] > bv[1]) + return FLT_MAX; + } + else + { + float ll = (bv[0] - data->ray.origin[i]) / data->ray_dot_axis[i]; + float lu = (bv[1] - data->ray.origin[i]) / data->ray_dot_axis[i]; + + if(data->ray_dot_axis[i] > 0) + { + if(ll > low) low = ll; + if(lu < upper) upper = lu; + } + else + { + if(lu > low) low = lu; + if(ll < upper) upper = ll; + } - for (leaf = tree->nodearray; leaf != tree->nodearray + tree->totleaf; leaf++) + if(low > upper) return FLT_MAX; + } + } + return low; +} + +static void dfs_raycast(BVHRayCastData *data, BVHNode *node) +{ + int i; + + //ray-bv is really fast.. and simple tests revealed its worth to test it + //before calling the ray-primitive functions + float dist = ray_nearest_hit(data, node); + if(dist >= data->hit.dist) return; + + if(node->totnode == 0) { - for (parent = leaf->parent; parent; parent = parent->parent) + if(data->callback) + data->callback(data->userdata, node->index, &data->ray, &data->hit); + else { - parent->traversed++; // we tried to go up in hierarchy - if (parent->traversed < parent->totnode) - break; // we do not need to check further - else - node_join(tree, parent); + data->hit.index = node->index; + data->hit.dist = dist; + VECADDFAC(data->hit.co, data->ray.origin, data->ray.direction, dist); + } + } + else + { + //pick loop direction to dive into the tree (based on ray direction and split axis) + if(data->ray_dot_axis[ node->main_axis ] > 0) + { + for(i=0; i != node->totnode; i++) + { + dfs_raycast(data, node->children[i]); + } + } + else + { + for(i=node->totnode-1; i >= 0; i--) + { + dfs_raycast(data, node->children[i]); + } } } } -float BLI_bvhtree_getepsilon(BVHTree *tree) +int BLI_bvhtree_ray_cast(BVHTree *tree, const float *co, const float *dir, BVHTreeRayHit *hit, BVHTree_RayCastCallback callback, void *userdata) { - return tree->epsilon; + int i; + BVHRayCastData data; + BVHNode * root = tree->nodes[tree->totleaf]; + + data.tree = tree; + + data.callback = callback; + data.userdata = userdata; + + VECCOPY(data.ray.origin, co); + VECCOPY(data.ray.direction, dir); + + Normalize(data.ray.direction); + + for(i=0; i<3; i++) + { + data.ray_dot_axis[i] = INPR( data.ray.direction, KDOP_AXES[i]); + + if(fabs(data.ray_dot_axis[i]) < 1e-7) + data.ray_dot_axis[i] = 0.0; + } + + + if(hit) + memcpy( &data.hit, hit, sizeof(*hit) ); + else + { + data.hit.index = -1; + data.hit.dist = FLT_MAX; + } + + if(root) + dfs_raycast(&data, root); + + + if(hit) + memcpy( hit, &data.hit, sizeof(*hit) ); + + return data.hit.index; } + |