Blender V2.61 - r43446

BLI_kdopbvh.c

Go to the documentation of this file.
00001 /*
00002  * ***** BEGIN GPL LICENSE BLOCK *****
00003  *
00004  * This program is free software; you can redistribute it and/or
00005  * modify it under the terms of the GNU General Public License
00006  * as published by the Free Software Foundation; either version 2
00007  * of the License, or (at your option) any later version.
00008  *
00009  * This program is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  * GNU General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU General Public License
00015  * along with this program; if not, write to the Free Software Foundation,
00016  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
00017  *
00018  * The Original Code is Copyright (C) 2006 by NaN Holding BV.
00019  * All rights reserved.
00020  *
00021  * The Original Code is: all of this file.
00022  *
00023  * Contributor(s): Daniel Genrich, Andre Pinto
00024  *
00025  * ***** END GPL LICENSE BLOCK *****
00026  */
00027 
00033 #include <assert.h>
00034 
00035 #include "MEM_guardedalloc.h"
00036 
00037 #include "BLI_utildefines.h"
00038 
00039 
00040 
00041 #include "BLI_kdopbvh.h"
00042 #include "BLI_math.h"
00043 
00044 #ifdef _OPENMP
00045 #include <omp.h>
00046 #endif
00047 
00048 
00049 
00050 #define MAX_TREETYPE 32
00051 #define DEFAULT_FIND_NEAREST_HEAP_SIZE 1024
00052 
00053 typedef struct BVHNode
00054 {
00055     struct BVHNode **children;
00056     struct BVHNode *parent; // some user defined traversed need that
00057     struct BVHNode *skip[2];
00058     float *bv;      // Bounding volume of all nodes, max 13 axis
00059     int index;      // face, edge, vertex index
00060     char totnode;   // how many nodes are used, used for speedup
00061     char main_axis; // Axis used to split this node
00062 } BVHNode;
00063 
00064 struct BVHTree
00065 {
00066     BVHNode **nodes;
00067     BVHNode *nodearray; /* pre-alloc branch nodes */
00068     BVHNode **nodechild;    // pre-alloc childs for nodes
00069     float   *nodebv;        // pre-alloc bounding-volumes for nodes
00070     float   epsilon; /* epslion is used for inflation of the k-dop     */
00071     int     totleaf; // leafs
00072     int     totbranch;
00073     char    tree_type; // type of tree (4 => quadtree)
00074     char    axis; // kdop type (6 => OBB, 7 => AABB, ...)
00075     char    start_axis, stop_axis; // KDOP_AXES array indices according to axis
00076 };
00077 
00078 typedef struct BVHOverlapData 
00079 {  
00080     BVHTree *tree1, *tree2; 
00081     BVHTreeOverlap *overlap; 
00082     int i, max_overlap; /* i is number of overlaps */
00083     int start_axis, stop_axis;
00084 } BVHOverlapData;
00085 
00086 typedef struct BVHNearestData
00087 {
00088     BVHTree *tree;
00089     const float *co;
00090     BVHTree_NearestPointCallback callback;
00091     void    *userdata;
00092     float proj[13];         //coordinates projection over axis
00093     BVHTreeNearest nearest;
00094 
00095 } BVHNearestData;
00096 
00097 typedef struct BVHRayCastData
00098 {
00099     BVHTree *tree;
00100 
00101     BVHTree_RayCastCallback callback;
00102     void    *userdata;
00103 
00104 
00105     BVHTreeRay    ray;
00106     float ray_dot_axis[13];
00107     float idot_axis[13];
00108     int index[6];
00109 
00110     BVHTreeRayHit hit;
00111 } BVHRayCastData;
00113 
00114 
00116 // Bounding Volume Hierarchy Definition
00117 // 
00118 // Notes: From OBB until 26-DOP --> all bounding volumes possible, just choose type below
00119 // Notes: You have to choose the type at compile time ITM
00120 // Notes: You can choose the tree type --> binary, quad, octree, choose below
00122 
00123 static float KDOP_AXES[13][3] =
00124 { {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},
00125 {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},
00126 {0, 1.0, -1.0}
00127 };
00128 
00129 /*
00130  * Generic push and pop heap
00131  */
00132 #define PUSH_HEAP_BODY(HEAP_TYPE,PRIORITY,heap,heap_size)   \
00133 {                                                   \
00134     HEAP_TYPE element = heap[heap_size-1];          \
00135     int child = heap_size-1;                        \
00136     while(child != 0)                               \
00137     {                                               \
00138         int parent = (child-1) / 2;                 \
00139         if(PRIORITY(element, heap[parent]))         \
00140         {                                           \
00141             heap[child] = heap[parent];             \
00142             child = parent;                         \
00143         }                                           \
00144         else break;                                 \
00145     }                                               \
00146     heap[child] = element;                          \
00147 }
00148 
00149 #define POP_HEAP_BODY(HEAP_TYPE, PRIORITY,heap,heap_size)   \
00150 {                                                   \
00151     HEAP_TYPE element = heap[heap_size-1];          \
00152     int parent = 0;                                 \
00153     while(parent < (heap_size-1)/2 )                \
00154     {                                               \
00155         int child2 = (parent+1)*2;                  \
00156         if(PRIORITY(heap[child2-1], heap[child2]))  \
00157             --child2;                               \
00158                                                     \
00159         if(PRIORITY(element, heap[child2]))         \
00160             break;                                  \
00161                                                     \
00162         heap[parent] = heap[child2];                \
00163         parent = child2;                            \
00164     }                                               \
00165     heap[parent] = element;                         \
00166 }
00167 
00168 #if 0
00169 static int ADJUST_MEMORY(void *local_memblock, void **memblock, int new_size, int *max_size, int size_per_item)
00170 {
00171     int   new_max_size = *max_size * 2;
00172     void *new_memblock = NULL;
00173 
00174     if(new_size <= *max_size)
00175         return TRUE;
00176 
00177     if(*memblock == local_memblock)
00178     {
00179         new_memblock = malloc( size_per_item * new_max_size );
00180         memcpy( new_memblock, *memblock, size_per_item * *max_size );
00181     }
00182     else
00183         new_memblock = realloc(*memblock, size_per_item * new_max_size );
00184 
00185     if(new_memblock)
00186     {
00187         *memblock = new_memblock;
00188         *max_size = new_max_size;
00189         return TRUE;
00190     }
00191     else
00192         return FALSE;
00193 }
00194 #endif
00195 
00197 // Introsort 
00198 // with permission deriven from the following Java code:
00199 // http://ralphunden.net/content/tutorials/a-guide-to-introsort/
00200 // and he derived it from the SUN STL 
00202 //static int size_threshold = 16;
00203 /*
00204 * Common methods for all algorithms
00205 */
00206 /*static int floor_lg(int a)
00207 {
00208     return (int)(floor(log(a)/log(2)));
00209 }*/
00210 
00211 /*
00212 * Insertion sort algorithm
00213 */
00214 static void bvh_insertionsort(BVHNode **a, int lo, int hi, int axis)
00215 {
00216     int i,j;
00217     BVHNode *t;
00218     for (i=lo; i < hi; i++)
00219     {
00220         j=i;
00221         t = a[i];
00222         while((j!=lo) && (t->bv[axis] < (a[j-1])->bv[axis]))
00223         {
00224             a[j] = a[j-1];
00225             j--;
00226         }
00227         a[j] = t;
00228     }
00229 }
00230 
00231 static int bvh_partition(BVHNode **a, int lo, int hi, BVHNode * x, int axis)
00232 {
00233     int i=lo, j=hi;
00234     while (1)
00235     {
00236         while ((a[i])->bv[axis] < x->bv[axis]) i++;
00237         j--;
00238         while (x->bv[axis] < (a[j])->bv[axis]) j--;
00239         if(!(i < j))
00240             return i;
00241         SWAP( BVHNode* , a[i], a[j]);
00242         i++;
00243     }
00244 }
00245 
00246 /*
00247 * Heapsort algorithm
00248 */
00249 #if 0
00250 static void bvh_downheap(BVHNode **a, int i, int n, int lo, int axis)
00251 {
00252     BVHNode * d = a[lo+i-1];
00253     int child;
00254     while (i<=n/2)
00255     {
00256         child = 2*i;
00257         if ((child < n) && ((a[lo+child-1])->bv[axis] < (a[lo+child])->bv[axis]))
00258         {
00259             child++;
00260         }
00261         if (!(d->bv[axis] < (a[lo+child-1])->bv[axis])) break;
00262         a[lo+i-1] = a[lo+child-1];
00263         i = child;
00264     }
00265     a[lo+i-1] = d;
00266 }
00267 
00268 static void bvh_heapsort(BVHNode **a, int lo, int hi, int axis)
00269 {
00270     int n = hi-lo, i;
00271     for (i=n/2; i>=1; i=i-1)
00272     {
00273         bvh_downheap(a, i,n,lo, axis);
00274     }
00275     for (i=n; i>1; i=i-1)
00276     {
00277         SWAP(BVHNode*, a[lo],a[lo+i-1]);
00278         bvh_downheap(a, 1,i-1,lo, axis);
00279     }
00280 }
00281 #endif
00282 
00283 static BVHNode *bvh_medianof3(BVHNode **a, int lo, int mid, int hi, int axis) // returns Sortable
00284 {
00285     if ((a[mid])->bv[axis] < (a[lo])->bv[axis])
00286     {
00287         if ((a[hi])->bv[axis] < (a[mid])->bv[axis])
00288             return a[mid];
00289         else
00290         {
00291             if ((a[hi])->bv[axis] < (a[lo])->bv[axis])
00292                 return a[hi];
00293             else
00294                 return a[lo];
00295         }
00296     }
00297     else
00298     {
00299         if ((a[hi])->bv[axis] < (a[mid])->bv[axis])
00300         {
00301             if ((a[hi])->bv[axis] < (a[lo])->bv[axis])
00302                 return a[lo];
00303             else
00304                 return a[hi];
00305         }
00306         else
00307             return a[mid];
00308     }
00309 }
00310 
00311 #if 0
00312 /*
00313 * Quicksort algorithm modified for Introsort
00314 */
00315 static void bvh_introsort_loop (BVHNode **a, int lo, int hi, int depth_limit, int axis)
00316 {
00317     int p;
00318 
00319     while (hi-lo > size_threshold)
00320     {
00321         if (depth_limit == 0)
00322         {
00323             bvh_heapsort(a, lo, hi, axis);
00324             return;
00325         }
00326         depth_limit=depth_limit-1;
00327         p=bvh_partition(a, lo, hi, bvh_medianof3(a, lo, lo+((hi-lo)/2)+1, hi-1, axis), axis);
00328         bvh_introsort_loop(a, p, hi, depth_limit, axis);
00329         hi=p;
00330     }
00331 }
00332 
00333 static void sort(BVHNode **a0, int begin, int end, int axis)
00334 {
00335     if (begin < end)
00336     {
00337         BVHNode **a=a0;
00338         bvh_introsort_loop(a, begin, end, 2*floor_lg(end-begin), axis);
00339         bvh_insertionsort(a, begin, end, axis);
00340     }
00341 }
00342 
00343 static void sort_along_axis(BVHTree *tree, int start, int end, int axis)
00344 {
00345     sort(tree->nodes, start, end, axis);
00346 }
00347 #endif
00348 
00349 //after a call to this function you can expect one of:
00350 //      every node to left of a[n] are smaller or equal to it
00351 //      every node to the right of a[n] are greater or equal to it
00352 static int partition_nth_element(BVHNode **a, int _begin, int _end, int n, int axis)
00353 {
00354     int begin = _begin, end = _end, cut;
00355     while(end-begin > 3)
00356     {
00357         cut = bvh_partition(a, begin, end, bvh_medianof3(a, begin, (begin+end)/2, end-1, axis), axis );
00358         if(cut <= n)
00359             begin = cut;
00360         else
00361             end = cut;
00362     }
00363     bvh_insertionsort(a, begin, end, axis);
00364 
00365     return n;
00366 }
00367 
00369 static void build_skip_links(BVHTree *tree, BVHNode *node, BVHNode *left, BVHNode *right)
00370 {
00371     int i;
00372     
00373     node->skip[0] = left;
00374     node->skip[1] = right;
00375     
00376     for (i = 0; i < node->totnode; i++)
00377     {
00378         if(i+1 < node->totnode)
00379             build_skip_links(tree, node->children[i], left, node->children[i+1] );
00380         else
00381             build_skip_links(tree, node->children[i], left, right );
00382 
00383         left = node->children[i];
00384     }
00385 }
00386 
00387 /*
00388  * BVHTree bounding volumes functions
00389  */
00390 static void create_kdop_hull(BVHTree *tree, BVHNode *node, const float *co, int numpoints, int moving)
00391 {
00392     float newminmax;
00393     float *bv = node->bv;
00394     int i, k;
00395     
00396     // don't init boudings for the moving case
00397     if(!moving)
00398     {
00399         for (i = tree->start_axis; i < tree->stop_axis; i++)
00400         {
00401             bv[2*i] = FLT_MAX;
00402             bv[2*i + 1] = -FLT_MAX;
00403         }
00404     }
00405     
00406     for(k = 0; k < numpoints; k++)
00407     {
00408         // for all Axes.
00409         for (i = tree->start_axis; i < tree->stop_axis; i++)
00410         {
00411             newminmax = dot_v3v3(&co[k * 3], KDOP_AXES[i]);
00412             if (newminmax < bv[2 * i])
00413                 bv[2 * i] = newminmax;
00414             if (newminmax > bv[(2 * i) + 1])
00415                 bv[(2 * i) + 1] = newminmax;
00416         }
00417     }
00418 }
00419 
00420 // depends on the fact that the BVH's for each face is already build
00421 static void refit_kdop_hull(BVHTree *tree, BVHNode *node, int start, int end)
00422 {
00423     float newmin,newmax;
00424     int i, j;
00425     float *bv = node->bv;
00426 
00427     
00428     for (i = tree->start_axis; i < tree->stop_axis; i++)
00429     {
00430         bv[2*i] = FLT_MAX;
00431         bv[2*i + 1] = -FLT_MAX;
00432     }
00433 
00434     for (j = start; j < end; j++)
00435     {
00436 // for all Axes.
00437         for (i = tree->start_axis; i < tree->stop_axis; i++)
00438         {
00439             newmin = tree->nodes[j]->bv[(2 * i)];   
00440             if ((newmin < bv[(2 * i)]))
00441                 bv[(2 * i)] = newmin;
00442  
00443             newmax = tree->nodes[j]->bv[(2 * i) + 1];
00444             if ((newmax > bv[(2 * i) + 1]))
00445                 bv[(2 * i) + 1] = newmax;
00446         }
00447     }
00448 
00449 }
00450 
00451 // only supports x,y,z axis in the moment
00452 // but we should use a plain and simple function here for speed sake
00453 static char get_largest_axis(float *bv)
00454 {
00455     float middle_point[3];
00456 
00457     middle_point[0] = (bv[1]) - (bv[0]); // x axis
00458     middle_point[1] = (bv[3]) - (bv[2]); // y axis
00459     middle_point[2] = (bv[5]) - (bv[4]); // z axis
00460     if (middle_point[0] > middle_point[1]) 
00461     {
00462         if (middle_point[0] > middle_point[2])
00463             return 1; // max x axis
00464         else
00465             return 5; // max z axis
00466     }
00467     else 
00468     {
00469         if (middle_point[1] > middle_point[2])
00470             return 3; // max y axis
00471         else
00472             return 5; // max z axis
00473     }
00474 }
00475 
00476 // bottom-up update of bvh node BV
00477 // join the children on the parent BV
00478 static void node_join(BVHTree *tree, BVHNode *node)
00479 {
00480     int i, j;
00481     
00482     for (i = tree->start_axis; i < tree->stop_axis; i++)
00483     {
00484         node->bv[2*i] = FLT_MAX;
00485         node->bv[2*i + 1] = -FLT_MAX;
00486     }
00487     
00488     for (i = 0; i < tree->tree_type; i++)
00489     {
00490         if (node->children[i]) 
00491         {
00492             for (j = tree->start_axis; j < tree->stop_axis; j++)
00493             {
00494                 // update minimum 
00495                 if (node->children[i]->bv[(2 * j)] < node->bv[(2 * j)]) 
00496                     node->bv[(2 * j)] = node->children[i]->bv[(2 * j)];
00497                 
00498                 // update maximum 
00499                 if (node->children[i]->bv[(2 * j) + 1] > node->bv[(2 * j) + 1])
00500                     node->bv[(2 * j) + 1] = node->children[i]->bv[(2 * j) + 1];
00501             }
00502         }
00503         else
00504             break;
00505     }
00506 }
00507 
00508 /*
00509  * Debug and information functions
00510  */
00511 #if 0
00512 static void bvhtree_print_tree(BVHTree *tree, BVHNode *node, int depth)
00513 {
00514     int i;
00515     for(i=0; i<depth; i++) printf(" ");
00516     printf(" - %d (%ld): ", node->index, node - tree->nodearray);
00517     for(i=2*tree->start_axis; i<2*tree->stop_axis; i++)
00518         printf("%.3f ", node->bv[i]);
00519     printf("\n");
00520 
00521     for(i=0; i<tree->tree_type; i++)
00522         if(node->children[i])
00523             bvhtree_print_tree(tree, node->children[i], depth+1);
00524 }
00525 
00526 static void bvhtree_info(BVHTree *tree)
00527 {
00528     printf("BVHTree info\n");
00529     printf("tree_type = %d, axis = %d, epsilon = %f\n", tree->tree_type, tree->axis, tree->epsilon);
00530     printf("nodes = %d, branches = %d, leafs = %d\n", tree->totbranch + tree->totleaf,  tree->totbranch, tree->totleaf);
00531     printf("Memory per node = %ldbytes\n", sizeof(BVHNode) + sizeof(BVHNode*)*tree->tree_type + sizeof(float)*tree->axis);
00532     printf("BV memory = %dbytes\n", MEM_allocN_len(tree->nodebv));
00533 
00534     printf("Total memory = %ldbytes\n", sizeof(BVHTree)
00535         + MEM_allocN_len(tree->nodes)
00536         + MEM_allocN_len(tree->nodearray)
00537         + MEM_allocN_len(tree->nodechild)
00538         + MEM_allocN_len(tree->nodebv)
00539         );
00540 
00541 //  bvhtree_print_tree(tree, tree->nodes[tree->totleaf], 0);
00542 }
00543 #endif
00544 
00545 #if 0
00546 
00547 
00548 static void verify_tree(BVHTree *tree)
00549 {
00550     int i, j, check = 0;
00551     
00552     // check the pointer list
00553     for(i = 0; i < tree->totleaf; i++)
00554     {
00555         if(tree->nodes[i]->parent == NULL)
00556             printf("Leaf has no parent: %d\n", i);
00557         else
00558         {
00559             for(j = 0; j < tree->tree_type; j++)
00560             {
00561                 if(tree->nodes[i]->parent->children[j] == tree->nodes[i])
00562                     check = 1;
00563             }
00564             if(!check)
00565             {
00566                 printf("Parent child relationship doesn't match: %d\n", i);
00567             }
00568             check = 0;
00569         }
00570     }
00571     
00572     // check the leaf list
00573     for(i = 0; i < tree->totleaf; i++)
00574     {
00575         if(tree->nodearray[i].parent == NULL)
00576             printf("Leaf has no parent: %d\n", i);
00577         else
00578         {
00579             for(j = 0; j < tree->tree_type; j++)
00580             {
00581                 if(tree->nodearray[i].parent->children[j] == &tree->nodearray[i])
00582                     check = 1;
00583             }
00584             if(!check)
00585             {
00586                 printf("Parent child relationship doesn't match: %d\n", i);
00587             }
00588             check = 0;
00589         }
00590     }
00591     
00592     printf("branches: %d, leafs: %d, total: %d\n", tree->totbranch, tree->totleaf, tree->totbranch + tree->totleaf);
00593 }
00594 #endif
00595 
00596 //Helper data and structures to build a min-leaf generalized implicit tree
00597 //This code can be easily reduced (basicly this is only method to calculate pow(k, n) in O(1).. and stuff like that)
00598 typedef struct BVHBuildHelper
00599 {
00600     int tree_type;              //
00601     int totleafs;               //
00602 
00603     int leafs_per_child  [32];  //Min number of leafs that are archievable from a node at depth N
00604     int branches_on_level[32];  //Number of nodes at depth N (tree_type^N)
00605 
00606     int remain_leafs;           //Number of leafs that are placed on the level that is not 100% filled
00607 
00608 } BVHBuildHelper;
00609 
00610 static void build_implicit_tree_helper(BVHTree *tree, BVHBuildHelper *data)
00611 {
00612     int depth = 0;
00613     int remain;
00614     int nnodes;
00615 
00616     data->totleafs = tree->totleaf;
00617     data->tree_type= tree->tree_type;
00618 
00619     //Calculate the smallest tree_type^n such that tree_type^n >= num_leafs
00620     for(
00621         data->leafs_per_child[0] = 1;
00622         data->leafs_per_child[0] <  data->totleafs;
00623         data->leafs_per_child[0] *= data->tree_type
00624     );
00625 
00626     data->branches_on_level[0] = 1;
00627 
00628     //We could stop the loop first (but I am lazy to find out when)
00629     for(depth = 1; depth < 32; depth++)
00630     {
00631         data->branches_on_level[depth] = data->branches_on_level[depth-1] * data->tree_type;
00632         data->leafs_per_child  [depth] = data->leafs_per_child  [depth-1] / data->tree_type;
00633     }
00634 
00635     remain = data->totleafs - data->leafs_per_child[1];
00636     nnodes = (remain + data->tree_type - 2) / (data->tree_type - 1);
00637     data->remain_leafs = remain + nnodes;
00638 }
00639 
00640 // return the min index of all the leafs archivable with the given branch
00641 static int implicit_leafs_index(BVHBuildHelper *data, int depth, int child_index)
00642 {
00643     int min_leaf_index = child_index * data->leafs_per_child[depth-1];
00644     if(min_leaf_index <= data->remain_leafs)
00645         return min_leaf_index;
00646     else if(data->leafs_per_child[depth])
00647         return data->totleafs - (data->branches_on_level[depth-1] - child_index) * data->leafs_per_child[depth];
00648     else
00649         return data->remain_leafs;
00650 }
00651 
00680 // This functions returns the number of branches needed to have the requested number of leafs.
00681 static int implicit_needed_branches(int tree_type, int leafs)
00682 {
00683     return MAX2(1, (leafs + tree_type - 3) / (tree_type-1) );
00684 }
00685 
00686 /*
00687  * This function handles the problem of "sorting" the leafs (along the split_axis).
00688  *
00689  * It arranges the elements in the given partitions such that:
00690  *  - any element in partition N is less or equal to any element in partition N+1.
00691  *  - if all elements are diferent all partition will get the same subset of elements
00692  *    as if the array was sorted.
00693  *
00694  * partition P is described as the elements in the range ( nth[P] , nth[P+1] ]
00695  *
00696  * TODO: This can be optimized a bit by doing a specialized nth_element instead of K nth_elements
00697  */
00698 static void split_leafs(BVHNode **leafs_array, int *nth, int partitions, int split_axis)
00699 {
00700     int i;
00701     for(i=0; i < partitions-1; i++)
00702     {
00703         if(nth[i] >= nth[partitions])
00704             break;
00705 
00706         partition_nth_element(leafs_array, nth[i], nth[partitions], nth[i+1], split_axis);
00707     }
00708 }
00709 
00710 /*
00711  * This functions builds an optimal implicit tree from the given leafs.
00712  * Where optimal stands for:
00713  *  - The resulting tree will have the smallest number of branches;
00714  *  - At most only one branch will have NULL childs;
00715  *  - All leafs will be stored at level N or N+1.
00716  *
00717  * This function creates an implicit tree on branches_array, the leafs are given on the leafs_array.
00718  *
00719  * The tree is built per depth levels. First branchs at depth 1.. then branches at depth 2.. etc..
00720  * The reason is that we can build level N+1 from level N witouth any data dependencies.. thus it allows
00721  * to use multithread building.
00722  *
00723  * To archieve this is necessary to find how much leafs are accessible from a certain branch, BVHBuildHelper
00724  * implicit_needed_branches and implicit_leafs_index are auxiliar functions to solve that "optimal-split".
00725  */
00726 static void non_recursive_bvh_div_nodes(BVHTree *tree, BVHNode *branches_array, BVHNode **leafs_array, int num_leafs)
00727 {
00728     int i;
00729 
00730     const int tree_type   = tree->tree_type;
00731     const int tree_offset = 2 - tree->tree_type; //this value is 0 (on binary trees) and negative on the others
00732     const int num_branches= implicit_needed_branches(tree_type, num_leafs);
00733 
00734     BVHBuildHelper data;
00735     int depth;
00736     
00737     // set parent from root node to NULL
00738     BVHNode *tmp = branches_array+0;
00739     tmp->parent = NULL;
00740 
00741     //Most of bvhtree code relies on 1-leaf trees having at least one branch
00742     //We handle that special case here
00743     if(num_leafs == 1)
00744     {
00745         BVHNode *root = branches_array+0;
00746         refit_kdop_hull(tree, root, 0, num_leafs);
00747         root->main_axis = get_largest_axis(root->bv) / 2;
00748         root->totnode = 1;
00749         root->children[0] = leafs_array[0];
00750         root->children[0]->parent = root;
00751         return;
00752     }
00753 
00754     branches_array--;   //Implicit trees use 1-based indexs
00755     
00756     build_implicit_tree_helper(tree, &data);
00757 
00758     //Loop tree levels (log N) loops
00759     for(i=1, depth = 1; i <= num_branches; i = i*tree_type + tree_offset, depth++)
00760     {
00761         const int first_of_next_level = i*tree_type + tree_offset;
00762         const int  end_j = MIN2(first_of_next_level, num_branches + 1); //index of last branch on this level
00763         int j;
00764 
00765         //Loop all branches on this level
00766 #pragma omp parallel for private(j) schedule(static)
00767         for(j = i; j < end_j; j++)
00768         {
00769             int k;
00770             const int parent_level_index= j-i;
00771             BVHNode* parent = branches_array + j;
00772             int nth_positions[ MAX_TREETYPE + 1];
00773             char split_axis;
00774 
00775             int parent_leafs_begin = implicit_leafs_index(&data, depth, parent_level_index);
00776             int parent_leafs_end   = implicit_leafs_index(&data, depth, parent_level_index+1);
00777 
00778             //This calculates the bounding box of this branch
00779             //and chooses the largest axis as the axis to divide leafs
00780             refit_kdop_hull(tree, parent, parent_leafs_begin, parent_leafs_end);
00781             split_axis = get_largest_axis(parent->bv);
00782 
00783             //Save split axis (this can be used on raytracing to speedup the query time)
00784             parent->main_axis = split_axis / 2;
00785 
00786             //Split the childs along the split_axis, note: its not needed to sort the whole leafs array
00787             //Only to assure that the elements are partioned on a way that each child takes the elements
00788             //it would take in case the whole array was sorted.
00789             //Split_leafs takes care of that "sort" problem.
00790             nth_positions[        0] = parent_leafs_begin;
00791             nth_positions[tree_type] = parent_leafs_end;
00792             for(k = 1; k < tree_type; k++)
00793             {
00794                 int child_index = j * tree_type + tree_offset + k;
00795                 int child_level_index = child_index - first_of_next_level; //child level index
00796                 nth_positions[k] = implicit_leafs_index(&data, depth+1, child_level_index);
00797             }
00798 
00799             split_leafs(leafs_array, nth_positions, tree_type, split_axis);
00800 
00801 
00802             //Setup children and totnode counters
00803             //Not really needed but currently most of BVH code relies on having an explicit children structure
00804             for(k = 0; k < tree_type; k++)
00805             {
00806                 int child_index = j * tree_type + tree_offset + k;
00807                 int child_level_index = child_index - first_of_next_level; //child level index
00808 
00809                 int child_leafs_begin = implicit_leafs_index(&data, depth+1, child_level_index);
00810                 int child_leafs_end   = implicit_leafs_index(&data, depth+1, child_level_index+1);
00811 
00812                 if(child_leafs_end - child_leafs_begin > 1)
00813                 {
00814                     parent->children[k] = branches_array + child_index;
00815                     parent->children[k]->parent = parent;
00816                 }
00817                 else if(child_leafs_end - child_leafs_begin == 1)
00818                 {
00819                     parent->children[k] = leafs_array[ child_leafs_begin ];
00820                     parent->children[k]->parent = parent;
00821                 }
00822                 else
00823                     break;
00824 
00825                 parent->totnode = k+1;
00826             }
00827         }
00828     }
00829 }
00830 
00831 
00832 /*
00833  * BLI_bvhtree api
00834  */
00835 BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
00836 {
00837     BVHTree *tree;
00838     int numnodes, i;
00839     
00840     // theres not support for trees below binary-trees :P
00841     if(tree_type < 2)
00842         return NULL;
00843     
00844     if(tree_type > MAX_TREETYPE)
00845         return NULL;
00846 
00847     tree = (BVHTree *)MEM_callocN(sizeof(BVHTree), "BVHTree");
00848 
00849     //tree epsilon must be >= FLT_EPSILON
00850     //so that tangent rays can still hit a bounding volume..
00851     //this bug would show up when casting a ray aligned with a kdop-axis and with an edge of 2 faces
00852     epsilon = MAX2(FLT_EPSILON, epsilon);
00853     
00854     if(tree)
00855     {
00856         tree->epsilon = epsilon;
00857         tree->tree_type = tree_type; 
00858         tree->axis = axis;
00859         
00860         if(axis == 26)
00861         {
00862             tree->start_axis = 0;
00863             tree->stop_axis = 13;
00864         }
00865         else if(axis == 18)
00866         {
00867             tree->start_axis = 7;
00868             tree->stop_axis = 13;
00869         }
00870         else if(axis == 14)
00871         {
00872             tree->start_axis = 0;
00873             tree->stop_axis = 7;
00874         }
00875         else if(axis == 8) // AABB
00876         {
00877             tree->start_axis = 0;
00878             tree->stop_axis = 4;
00879         }
00880         else if(axis == 6) // OBB
00881         {
00882             tree->start_axis = 0;
00883             tree->stop_axis = 3;
00884         }
00885         else
00886         {
00887             MEM_freeN(tree);
00888             return NULL;
00889         }
00890 
00891 
00892         //Allocate arrays
00893         numnodes = maxsize + implicit_needed_branches(tree_type, maxsize) + tree_type;
00894 
00895         tree->nodes = (BVHNode **)MEM_callocN(sizeof(BVHNode *)*numnodes, "BVHNodes");
00896         
00897         if(!tree->nodes)
00898         {
00899             MEM_freeN(tree);
00900             return NULL;
00901         }
00902         
00903         tree->nodebv = (float*)MEM_callocN(sizeof(float)* axis * numnodes, "BVHNodeBV");
00904         if(!tree->nodebv)
00905         {
00906             MEM_freeN(tree->nodes);
00907             MEM_freeN(tree);
00908         }
00909 
00910         tree->nodechild = (BVHNode**)MEM_callocN(sizeof(BVHNode*) * tree_type * numnodes, "BVHNodeBV");
00911         if(!tree->nodechild)
00912         {
00913             MEM_freeN(tree->nodebv);
00914             MEM_freeN(tree->nodes);
00915             MEM_freeN(tree);
00916         }
00917 
00918         tree->nodearray = (BVHNode *)MEM_callocN(sizeof(BVHNode)* numnodes, "BVHNodeArray");
00919         
00920         if(!tree->nodearray)
00921         {
00922             MEM_freeN(tree->nodechild);
00923             MEM_freeN(tree->nodebv);
00924             MEM_freeN(tree->nodes);
00925             MEM_freeN(tree);
00926             return NULL;
00927         }
00928 
00929         //link the dynamic bv and child links
00930         for(i=0; i< numnodes; i++)
00931         {
00932             tree->nodearray[i].bv = tree->nodebv + i * axis;
00933             tree->nodearray[i].children = tree->nodechild + i * tree_type;
00934         }
00935         
00936     }
00937 
00938     return tree;
00939 }
00940 
00941 void BLI_bvhtree_free(BVHTree *tree)
00942 {   
00943     if(tree)
00944     {
00945         MEM_freeN(tree->nodes);
00946         MEM_freeN(tree->nodearray);
00947         MEM_freeN(tree->nodebv);
00948         MEM_freeN(tree->nodechild);
00949         MEM_freeN(tree);
00950     }
00951 }
00952 
00953 void BLI_bvhtree_balance(BVHTree *tree)
00954 {
00955     int i;
00956 
00957     BVHNode*  branches_array = tree->nodearray + tree->totleaf;
00958     BVHNode** leafs_array    = tree->nodes;
00959 
00960     //This function should only be called once (some big bug goes here if its being called more than once per tree)
00961     assert(tree->totbranch == 0);
00962 
00963     //Build the implicit tree
00964     non_recursive_bvh_div_nodes(tree, branches_array, leafs_array, tree->totleaf);
00965 
00966     //current code expects the branches to be linked to the nodes array
00967     //we perform that linkage here
00968     tree->totbranch = implicit_needed_branches(tree->tree_type, tree->totleaf);
00969     for(i = 0; i < tree->totbranch; i++)
00970         tree->nodes[tree->totleaf + i] = branches_array + i;
00971 
00972     build_skip_links(tree, tree->nodes[tree->totleaf], NULL, NULL);
00973     //bvhtree_info(tree);
00974 }
00975 
00976 int BLI_bvhtree_insert(BVHTree *tree, int index, const float *co, int numpoints)
00977 {
00978     int i;
00979     BVHNode *node = NULL;
00980     
00981     // insert should only possible as long as tree->totbranch is 0
00982     if(tree->totbranch > 0)
00983         return 0;
00984     
00985     if(tree->totleaf+1 >= MEM_allocN_len(tree->nodes)/sizeof(*(tree->nodes)))
00986         return 0;
00987     
00988     // TODO check if have enough nodes in array
00989     
00990     node = tree->nodes[tree->totleaf] = &(tree->nodearray[tree->totleaf]);
00991     tree->totleaf++;
00992     
00993     create_kdop_hull(tree, node, co, numpoints, 0);
00994     node->index= index;
00995     
00996     // inflate the bv with some epsilon
00997     for (i = tree->start_axis; i < tree->stop_axis; i++)
00998     {
00999         node->bv[(2 * i)] -= tree->epsilon; // minimum 
01000         node->bv[(2 * i) + 1] += tree->epsilon; // maximum 
01001     }
01002 
01003     return 1;
01004 }
01005 
01006 
01007 // call before BLI_bvhtree_update_tree()
01008 int BLI_bvhtree_update_node(BVHTree *tree, int index, const float *co, const float *co_moving, int numpoints)
01009 {
01010     int i;
01011     BVHNode *node= NULL;
01012     
01013     // check if index exists
01014     if(index > tree->totleaf)
01015         return 0;
01016     
01017     node = tree->nodearray + index;
01018     
01019     create_kdop_hull(tree, node, co, numpoints, 0);
01020     
01021     if(co_moving)
01022         create_kdop_hull(tree, node, co_moving, numpoints, 1);
01023     
01024     // inflate the bv with some epsilon
01025     for (i = tree->start_axis; i < tree->stop_axis; i++)
01026     {
01027         node->bv[(2 * i)] -= tree->epsilon; // minimum 
01028         node->bv[(2 * i) + 1] += tree->epsilon; // maximum 
01029     }
01030 
01031     return 1;
01032 }
01033 
01034 // call BLI_bvhtree_update_node() first for every node/point/triangle
01035 void BLI_bvhtree_update_tree(BVHTree *tree)
01036 {
01037     //Update bottom=>top
01038     //TRICKY: the way we build the tree all the childs have an index greater than the parent
01039     //This allows us todo a bottom up update by starting on the biger numbered branch
01040 
01041     BVHNode** root  = tree->nodes + tree->totleaf;
01042     BVHNode** index = tree->nodes + tree->totleaf + tree->totbranch-1;
01043 
01044     for (; index >= root; index--)
01045         node_join(tree, *index);
01046 }
01047 
01048 float BLI_bvhtree_getepsilon(BVHTree *tree)
01049 {
01050     return tree->epsilon;
01051 }
01052 
01053 
01054 /*
01055  * BLI_bvhtree_overlap
01056  */
01057 // overlap - is it possbile for 2 bv's to collide ?
01058 static int tree_overlap(BVHNode *node1, BVHNode *node2, int start_axis, int stop_axis)
01059 {
01060     float *bv1 = node1->bv;
01061     float *bv2 = node2->bv;
01062 
01063     float *bv1_end = bv1 + (stop_axis<<1);
01064         
01065     bv1 += start_axis<<1;
01066     bv2 += start_axis<<1;
01067     
01068     // test all axis if min + max overlap
01069     for (; bv1 != bv1_end; bv1+=2, bv2+=2)
01070     {
01071         if ((*(bv1) > *(bv2 + 1)) || (*(bv2) > *(bv1 + 1))) 
01072             return 0;
01073     }
01074     
01075     return 1;
01076 }
01077 
01078 static void traverse(BVHOverlapData *data, BVHNode *node1, BVHNode *node2)
01079 {
01080     int j;
01081     
01082     if(tree_overlap(node1, node2, data->start_axis, data->stop_axis))
01083     {
01084         // check if node1 is a leaf
01085         if(!node1->totnode)
01086         {
01087             // check if node2 is a leaf
01088             if(!node2->totnode)
01089             {
01090                 
01091                 if(node1 == node2)
01092                 {
01093                     return;
01094                 }
01095                     
01096                 if(data->i >= data->max_overlap)
01097                 {   
01098                     // try to make alloc'ed memory bigger
01099                     data->overlap = realloc(data->overlap, sizeof(BVHTreeOverlap)*data->max_overlap*2);
01100                     
01101                     if(!data->overlap)
01102                     {
01103                         printf("Out of Memory in traverse\n");
01104                         return;
01105                     }
01106                     data->max_overlap *= 2;
01107                 }
01108                 
01109                 // both leafs, insert overlap!
01110                 data->overlap[data->i].indexA = node1->index;
01111                 data->overlap[data->i].indexB = node2->index;
01112 
01113                 data->i++;
01114             }
01115             else
01116             {
01117                 for(j = 0; j < data->tree2->tree_type; j++)
01118                 {
01119                     if(node2->children[j])
01120                         traverse(data, node1, node2->children[j]);
01121                 }
01122             }
01123         }
01124         else
01125         {
01126             
01127             for(j = 0; j < data->tree2->tree_type; j++)
01128             {
01129                 if(node1->children[j])
01130                     traverse(data, node1->children[j], node2);
01131             }
01132         }
01133     }
01134     return;
01135 }
01136 
01137 BVHTreeOverlap *BLI_bvhtree_overlap(BVHTree *tree1, BVHTree *tree2, unsigned int *result)
01138 {
01139     int j;
01140     unsigned int total = 0;
01141     BVHTreeOverlap *overlap = NULL, *to = NULL;
01142     BVHOverlapData **data;
01143     
01144     // check for compatibility of both trees (can't compare 14-DOP with 18-DOP)
01145     if((tree1->axis != tree2->axis) && (tree1->axis == 14 || tree2->axis == 14) && (tree1->axis == 18 || tree2->axis == 18))
01146         return NULL;
01147     
01148     // fast check root nodes for collision before doing big splitting + traversal
01149     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)))
01150         return NULL;
01151 
01152     data = MEM_callocN(sizeof(BVHOverlapData *)* tree1->tree_type, "BVHOverlapData_star");
01153     
01154     for(j = 0; j < tree1->tree_type; j++)
01155     {
01156         data[j] = (BVHOverlapData *)MEM_callocN(sizeof(BVHOverlapData), "BVHOverlapData");
01157         
01158         // init BVHOverlapData
01159         data[j]->overlap = (BVHTreeOverlap *)malloc(sizeof(BVHTreeOverlap)*MAX2(tree1->totleaf, tree2->totleaf));
01160         data[j]->tree1 = tree1;
01161         data[j]->tree2 = tree2;
01162         data[j]->max_overlap = MAX2(tree1->totleaf, tree2->totleaf);
01163         data[j]->i = 0;
01164         data[j]->start_axis = MIN2(tree1->start_axis, tree2->start_axis);
01165         data[j]->stop_axis  = MIN2(tree1->stop_axis,  tree2->stop_axis );
01166     }
01167 
01168 #pragma omp parallel for private(j) schedule(static)
01169     for(j = 0; j < MIN2(tree1->tree_type, tree1->nodes[tree1->totleaf]->totnode); j++)
01170     {
01171         traverse(data[j], tree1->nodes[tree1->totleaf]->children[j], tree2->nodes[tree2->totleaf]);
01172     }
01173     
01174     for(j = 0; j < tree1->tree_type; j++)
01175         total += data[j]->i;
01176     
01177     to = overlap = (BVHTreeOverlap *)MEM_callocN(sizeof(BVHTreeOverlap)*total, "BVHTreeOverlap");
01178     
01179     for(j = 0; j < tree1->tree_type; j++)
01180     {
01181         memcpy(to, data[j]->overlap, data[j]->i*sizeof(BVHTreeOverlap));
01182         to+=data[j]->i;
01183     }
01184     
01185     for(j = 0; j < tree1->tree_type; j++)
01186     {
01187         free(data[j]->overlap);
01188         MEM_freeN(data[j]);
01189     }
01190     MEM_freeN(data);
01191     
01192     (*result) = total;
01193     return overlap;
01194 }
01195 
01196 //Determines the nearest point of the given node BV. Returns the squared distance to that point.
01197 static float calc_nearest_point(const float proj[3], BVHNode *node, float *nearest)
01198 {
01199     int i;
01200     const float *bv = node->bv;
01201 
01202     //nearest on AABB hull
01203     for(i=0; i != 3; i++, bv += 2)
01204     {
01205         if(bv[0] > proj[i])
01206             nearest[i] = bv[0];
01207         else if(bv[1] < proj[i])
01208             nearest[i] = bv[1];
01209         else
01210             nearest[i] = proj[i]; 
01211     }
01212 
01213 /*
01214     //nearest on a general hull
01215     copy_v3_v3(nearest, data->co);
01216     for(i = data->tree->start_axis; i != data->tree->stop_axis; i++, bv+=2)
01217     {
01218         float proj = dot_v3v3( nearest, KDOP_AXES[i]);
01219         float dl = bv[0] - proj;
01220         float du = bv[1] - proj;
01221 
01222         if(dl > 0)
01223         {
01224             madd_v3_v3fl(nearest, KDOP_AXES[i], dl);
01225         }
01226         else if(du < 0)
01227         {
01228             madd_v3_v3fl(nearest, KDOP_AXES[i], du);
01229         }
01230     }
01231 */
01232     return len_squared_v3v3(proj, nearest);
01233 }
01234 
01235 
01236 typedef struct NodeDistance
01237 {
01238     BVHNode *node;
01239     float dist;
01240 
01241 } NodeDistance;
01242 
01243 #define NodeDistance_priority(a,b)  ( (a).dist < (b).dist )
01244 
01245 // TODO: use a priority queue to reduce the number of nodes looked on
01246 static void dfs_find_nearest_dfs(BVHNearestData *data, BVHNode *node)
01247 {
01248     if(node->totnode == 0)
01249     {
01250         if(data->callback)
01251             data->callback(data->userdata , node->index, data->co, &data->nearest);
01252         else
01253         {
01254             data->nearest.index = node->index;
01255             data->nearest.dist  = calc_nearest_point(data->proj, node, data->nearest.co);
01256         }
01257     }
01258     else
01259     {
01260         //Better heuristic to pick the closest node to dive on
01261         int i;
01262         float nearest[3];
01263 
01264         if(data->proj[ node->main_axis ] <= node->children[0]->bv[node->main_axis*2+1])
01265         {
01266 
01267             for(i=0; i != node->totnode; i++)
01268             {
01269                 if( calc_nearest_point(data->proj, node->children[i], nearest) >= data->nearest.dist) continue;
01270                 dfs_find_nearest_dfs(data, node->children[i]);
01271             }
01272         }
01273         else
01274         {
01275             for(i=node->totnode-1; i >= 0 ; i--)
01276             {
01277                 if( calc_nearest_point(data->proj, node->children[i], nearest) >= data->nearest.dist) continue;
01278                 dfs_find_nearest_dfs(data, node->children[i]);
01279             }
01280         }
01281     }
01282 }
01283 
01284 static void dfs_find_nearest_begin(BVHNearestData *data, BVHNode *node)
01285 {
01286     float nearest[3], sdist;
01287     sdist = calc_nearest_point(data->proj, node, nearest);
01288     if(sdist >= data->nearest.dist) return;
01289     dfs_find_nearest_dfs(data, node);
01290 }
01291 
01292 
01293 #if 0
01294 static void NodeDistance_push_heap(NodeDistance *heap, int heap_size)
01295 PUSH_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size)
01296 
01297 static void NodeDistance_pop_heap(NodeDistance *heap, int heap_size)
01298 POP_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size)
01299 
01300 //NN function that uses an heap.. this functions leads to an optimal number of min-distance
01301 //but for normal tri-faces and BV 6-dop.. a simple dfs with local heuristics (as implemented
01302 //in source/blender/blenkernel/intern/shrinkwrap.c) works faster.
01303 //
01304 //It may make sense to use this function if the callback queries are very slow.. or if its impossible
01305 //to get a nice heuristic
01306 //
01307 //this function uses "malloc/free" instead of the MEM_* because it intends to be openmp safe
01308 static void bfs_find_nearest(BVHNearestData *data, BVHNode *node)
01309 {
01310     int i;
01311     NodeDistance default_heap[DEFAULT_FIND_NEAREST_HEAP_SIZE];
01312     NodeDistance *heap=default_heap, current;
01313     int heap_size = 0, max_heap_size = sizeof(default_heap)/sizeof(default_heap[0]);
01314     float nearest[3];
01315 
01316     int callbacks = 0, push_heaps = 0;
01317 
01318     if(node->totnode == 0)
01319     {
01320         dfs_find_nearest_dfs(data, node);
01321         return;
01322     }
01323 
01324     current.node = node;
01325     current.dist = calc_nearest_point(data->proj, node, nearest);
01326 
01327     while(current.dist < data->nearest.dist)
01328     {
01329 //      printf("%f : %f\n", current.dist, data->nearest.dist);
01330         for(i=0; i< current.node->totnode; i++)
01331         {
01332             BVHNode *child = current.node->children[i];
01333             if(child->totnode == 0)
01334             {
01335                 callbacks++;
01336                 dfs_find_nearest_dfs(data, child);
01337             }
01338             else
01339             {
01340                 //adjust heap size
01341                 if(heap_size >= max_heap_size
01342                 && ADJUST_MEMORY(default_heap, (void**)&heap, heap_size+1, &max_heap_size, sizeof(heap[0])) == FALSE)
01343                 {
01344                     printf("WARNING: bvh_find_nearest got out of memory\n");
01345 
01346                     if(heap != default_heap)
01347                         free(heap);
01348 
01349                     return;
01350                 }
01351 
01352                 heap[heap_size].node = current.node->children[i];
01353                 heap[heap_size].dist = calc_nearest_point(data->proj, current.node->children[i], nearest);
01354 
01355                 if(heap[heap_size].dist >= data->nearest.dist) continue;
01356                 heap_size++;
01357 
01358                 NodeDistance_push_heap(heap, heap_size);
01359     //          PUSH_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size);
01360                 push_heaps++;
01361             }
01362         }
01363         
01364         if(heap_size == 0) break;
01365 
01366         current = heap[0];
01367         NodeDistance_pop_heap(heap, heap_size);
01368 //      POP_HEAP_BODY(NodeDistance, NodeDistance_priority, heap, heap_size);
01369         heap_size--;
01370     }
01371 
01372 //  printf("hsize=%d, callbacks=%d, pushs=%d\n", heap_size, callbacks, push_heaps);
01373 
01374     if(heap != default_heap)
01375         free(heap);
01376 }
01377 #endif
01378 
01379 
01380 int BLI_bvhtree_find_nearest(BVHTree *tree, const float co[3], BVHTreeNearest *nearest, BVHTree_NearestPointCallback callback, void *userdata)
01381 {
01382     int i;
01383 
01384     BVHNearestData data;
01385     BVHNode* root = tree->nodes[tree->totleaf];
01386 
01387     //init data to search
01388     data.tree = tree;
01389     data.co = co;
01390 
01391     data.callback = callback;
01392     data.userdata = userdata;
01393 
01394     for(i = data.tree->start_axis; i != data.tree->stop_axis; i++)
01395     {
01396         data.proj[i] = dot_v3v3(data.co, KDOP_AXES[i]);
01397     }
01398 
01399     if(nearest)
01400     {
01401         memcpy( &data.nearest , nearest, sizeof(*nearest) );
01402     }
01403     else
01404     {
01405         data.nearest.index = -1;
01406         data.nearest.dist = FLT_MAX;
01407     }
01408 
01409     //dfs search
01410     if(root)
01411         dfs_find_nearest_begin(&data, root);
01412 
01413     //copy back results
01414     if(nearest)
01415     {
01416         memcpy(nearest, &data.nearest, sizeof(*nearest));
01417     }
01418 
01419     return data.nearest.index;
01420 }
01421 
01422 
01423 /*
01424  * Raycast - BLI_bvhtree_ray_cast
01425  *
01426  * raycast is done by performing a DFS on the BVHTree and saving the closest hit
01427  */
01428 
01429 
01430 //Determines the distance that the ray must travel to hit the bounding volume of the given node
01431 static float ray_nearest_hit(BVHRayCastData *data, float *bv)
01432 {
01433     int i;
01434 
01435     float low = 0, upper = data->hit.dist;
01436 
01437     for(i=0; i != 3; i++, bv += 2)
01438     {
01439         if(data->ray_dot_axis[i] == 0.0f)
01440         {
01441             //axis aligned ray
01442             if(data->ray.origin[i] < bv[0] - data->ray.radius
01443             || data->ray.origin[i] > bv[1] + data->ray.radius)
01444                 return FLT_MAX;
01445         }
01446         else
01447         {
01448             float ll = (bv[0] - data->ray.radius - data->ray.origin[i]) / data->ray_dot_axis[i];
01449             float lu = (bv[1] + data->ray.radius - data->ray.origin[i]) / data->ray_dot_axis[i];
01450 
01451             if(data->ray_dot_axis[i] > 0.0f)
01452             {
01453                 if(ll > low)   low = ll;
01454                 if(lu < upper) upper = lu;
01455             }
01456             else
01457             {
01458                 if(lu > low)   low = lu;
01459                 if(ll < upper) upper = ll;
01460             }
01461     
01462             if(low > upper) return FLT_MAX;
01463         }
01464     }
01465     return low;
01466 }
01467 
01468 //Determines the distance that the ray must travel to hit the bounding volume of the given node
01469 //Based on Tactical Optimization of Ray/Box Intersection, by Graham Fyffe
01470 //[http://tog.acm.org/resources/RTNews/html/rtnv21n1.html#art9]
01471 //
01472 //TODO this doens't has data->ray.radius in consideration
01473 static float fast_ray_nearest_hit(const BVHRayCastData *data, const BVHNode *node)
01474 {
01475     const float *bv = node->bv;
01476     float dist;
01477     
01478     float t1x = (bv[data->index[0]] - data->ray.origin[0]) * data->idot_axis[0];
01479     float t2x = (bv[data->index[1]] - data->ray.origin[0]) * data->idot_axis[0];
01480     float t1y = (bv[data->index[2]] - data->ray.origin[1]) * data->idot_axis[1];
01481     float t2y = (bv[data->index[3]] - data->ray.origin[1]) * data->idot_axis[1];
01482     float t1z = (bv[data->index[4]] - data->ray.origin[2]) * data->idot_axis[2];
01483     float t2z = (bv[data->index[5]] - data->ray.origin[2]) * data->idot_axis[2];
01484 
01485     if(t1x > t2y || t2x < t1y || t1x > t2z || t2x < t1z || t1y > t2z || t2y < t1z) return FLT_MAX;
01486     if(t2x < 0.0f || t2y < 0.0f || t2z < 0.0f) return FLT_MAX;
01487     if(t1x > data->hit.dist || t1y > data->hit.dist || t1z > data->hit.dist) return FLT_MAX;
01488 
01489     dist = t1x;
01490     if (t1y > dist) dist = t1y;
01491     if (t1z > dist) dist = t1z;
01492     return dist;
01493 }
01494 
01495 static void dfs_raycast(BVHRayCastData *data, BVHNode *node)
01496 {
01497     int i;
01498 
01499     //ray-bv is really fast.. and simple tests revealed its worth to test it
01500     //before calling the ray-primitive functions
01501     /* XXX: temporary solution for particles untill fast_ray_nearest_hit supports ray.radius */
01502     float dist = (data->ray.radius > 0.0f) ? ray_nearest_hit(data, node->bv) : fast_ray_nearest_hit(data, node);
01503     if(dist >= data->hit.dist) return;
01504 
01505     if(node->totnode == 0)
01506     {
01507         if(data->callback)
01508             data->callback(data->userdata, node->index, &data->ray, &data->hit);
01509         else
01510         {
01511             data->hit.index = node->index;
01512             data->hit.dist  = dist;
01513             madd_v3_v3v3fl(data->hit.co, data->ray.origin, data->ray.direction, dist);
01514         }
01515     }
01516     else
01517     {
01518         //pick loop direction to dive into the tree (based on ray direction and split axis)
01519         if(data->ray_dot_axis[ (int)node->main_axis ] > 0.0f)
01520         {
01521             for(i=0; i != node->totnode; i++)
01522             {
01523                 dfs_raycast(data, node->children[i]);
01524             }
01525         }
01526         else
01527         {
01528             for(i=node->totnode-1; i >= 0; i--)
01529             {
01530                 dfs_raycast(data, node->children[i]);
01531             }
01532         }
01533     }
01534 }
01535 
01536 #if 0
01537 static void iterative_raycast(BVHRayCastData *data, BVHNode *node)
01538 {
01539     while(node)
01540     {
01541         float dist = fast_ray_nearest_hit(data, node);
01542         if(dist >= data->hit.dist)
01543         {
01544             node = node->skip[1];
01545             continue;
01546         }
01547 
01548         if(node->totnode == 0)
01549         {
01550             if(data->callback)
01551                 data->callback(data->userdata, node->index, &data->ray, &data->hit);
01552             else
01553             {
01554                 data->hit.index = node->index;
01555                 data->hit.dist  = dist;
01556                 madd_v3_v3v3fl(data->hit.co, data->ray.origin, data->ray.direction, dist);
01557             }
01558             
01559             node = node->skip[1];
01560         }
01561         else
01562         {
01563             node = node->children[0];
01564         }   
01565     }
01566 }
01567 #endif
01568 
01569 int BLI_bvhtree_ray_cast(BVHTree *tree, const float co[3], const float dir[3], float radius, BVHTreeRayHit *hit, BVHTree_RayCastCallback callback, void *userdata)
01570 {
01571     int i;
01572     BVHRayCastData data;
01573     BVHNode * root = tree->nodes[tree->totleaf];
01574 
01575     data.tree = tree;
01576 
01577     data.callback = callback;
01578     data.userdata = userdata;
01579 
01580     copy_v3_v3(data.ray.origin,    co);
01581     copy_v3_v3(data.ray.direction, dir);
01582     data.ray.radius = radius;
01583 
01584     normalize_v3(data.ray.direction);
01585 
01586     for(i=0; i<3; i++)
01587     {
01588         data.ray_dot_axis[i] = dot_v3v3(data.ray.direction, KDOP_AXES[i]);
01589         data.idot_axis[i] = 1.0f / data.ray_dot_axis[i];
01590 
01591         if(fabsf(data.ray_dot_axis[i]) < FLT_EPSILON)
01592         {
01593             data.ray_dot_axis[i] = 0.0;
01594         }
01595         data.index[2*i] = data.idot_axis[i] < 0.0f ? 1 : 0;
01596         data.index[2*i+1] = 1 - data.index[2*i];
01597         data.index[2*i]   += 2*i;
01598         data.index[2*i+1] += 2*i;
01599     }
01600 
01601 
01602     if(hit)
01603         memcpy( &data.hit, hit, sizeof(*hit) );
01604     else
01605     {
01606         data.hit.index = -1;
01607         data.hit.dist = FLT_MAX;
01608     }
01609 
01610     if(root)
01611     {
01612         dfs_raycast(&data, root);
01613 //      iterative_raycast(&data, root);
01614      }
01615 
01616 
01617     if(hit)
01618         memcpy( hit, &data.hit, sizeof(*hit) );
01619 
01620     return data.hit.index;
01621 }
01622 
01623 float BLI_bvhtree_bb_raycast(float *bv, const float light_start[3], const float light_end[3], float pos[3])
01624 {
01625     BVHRayCastData data;
01626     float dist = 0.0;
01627 
01628     data.hit.dist = FLT_MAX;
01629     
01630     // get light direction
01631     data.ray.direction[0] = light_end[0] - light_start[0];
01632     data.ray.direction[1] = light_end[1] - light_start[1];
01633     data.ray.direction[2] = light_end[2] - light_start[2];
01634     
01635     data.ray.radius = 0.0;
01636     
01637     data.ray.origin[0] = light_start[0];
01638     data.ray.origin[1] = light_start[1];
01639     data.ray.origin[2] = light_start[2];
01640     
01641     normalize_v3(data.ray.direction);
01642     copy_v3_v3(data.ray_dot_axis, data.ray.direction);
01643     
01644     dist = ray_nearest_hit(&data, bv);
01645     
01646     if(dist > 0.0f)
01647     {
01648         madd_v3_v3v3fl(pos, light_start, data.ray.direction, dist);
01649     }
01650     return dist;
01651     
01652 }
01653 
01654 /*
01655  * Range Query - as request by broken :P
01656  *
01657  * Allocs and fills an array with the indexs of node that are on the given spherical range (center, radius) 
01658  * Returns the size of the array.
01659  */
01660 typedef struct RangeQueryData
01661 {
01662     BVHTree *tree;
01663     const float *center;
01664     float radius;           //squared radius
01665 
01666     int hits;
01667 
01668     BVHTree_RangeQuery callback;
01669     void *userdata;
01670 
01671 
01672 } RangeQueryData;
01673 
01674 
01675 static void dfs_range_query(RangeQueryData *data, BVHNode *node)
01676 {
01677     if(node->totnode == 0)
01678     {
01679 #if 0   /*UNUSED*/
01680         //Calculate the node min-coords (if the node was a point then this is the point coordinates)
01681         float co[3];
01682         co[0] = node->bv[0];
01683         co[1] = node->bv[2];
01684         co[2] = node->bv[4];
01685 #endif
01686     }
01687     else
01688     {
01689         int i;
01690         for(i=0; i != node->totnode; i++)
01691         {
01692             float nearest[3];
01693             float dist = calc_nearest_point(data->center, node->children[i], nearest);
01694             if(dist < data->radius)
01695             {
01696                 //Its a leaf.. call the callback
01697                 if(node->children[i]->totnode == 0)
01698                 {
01699                     data->hits++;
01700                     data->callback( data->userdata, node->children[i]->index, dist );
01701                 }
01702                 else
01703                     dfs_range_query( data, node->children[i] );
01704             }
01705         }
01706     }
01707 }
01708 
01709 int BLI_bvhtree_range_query(BVHTree *tree, const float co[3], float radius, BVHTree_RangeQuery callback, void *userdata)
01710 {
01711     BVHNode * root = tree->nodes[tree->totleaf];
01712 
01713     RangeQueryData data;
01714     data.tree = tree;
01715     data.center = co;
01716     data.radius = radius*radius;
01717     data.hits = 0;
01718 
01719     data.callback = callback;
01720     data.userdata = userdata;
01721 
01722     if(root != NULL)
01723     {
01724         float nearest[3];
01725         float dist = calc_nearest_point(data.center, root, nearest);
01726         if(dist < data.radius)
01727         {
01728             //Its a leaf.. call the callback
01729             if(root->totnode == 0)
01730             {
01731                 data.hits++;
01732                 data.callback( data.userdata, root->index, dist );
01733             }
01734             else
01735                 dfs_range_query( &data, root );
01736         }
01737     }
01738 
01739     return data.hits;
01740 }