Commit of selfcollisions using new kdop design. Should result in nice speedup.
[blender.git] / source / blender / blenlib / intern / BLI_kdopbvh.c
index 8be52854a7bf29e79db38a8284714f622fce9539..83afe258aad563e90d7615f51f279bdcdabfbe24 100644 (file)
@@ -25,7 +25,7 @@
 *
 * The Original Code is: all of this file.
 *
-* Contributor(s): Daniel Genrich
+* Contributor(s): Daniel Genrich, Andre Pinto
 *
 * ***** END GPL/BL DUAL LICENSE BLOCK *****
 */
 #include "BKE_utildefines.h"
 
 #include "BLI_kdopbvh.h"
+#include "BLI_arithb.h"
 
 #ifdef _OPENMP
 #include <omp.h>
 #endif
 
+#include <time.h>
+
+/* Util macros */
+#define TO_STR(a)      #a
+#define JOIN(a,b)      a##b
+
+/* Benchmark macros */
+#if 1
+
+#define BENCH(a)       \
+       do {                    \
+               clock_t _clock_init = clock();  \
+               (a);                                                    \
+               printf("%s: %fms\n", #a, (float)(clock()-_clock_init)*1000/CLOCKS_PER_SEC);     \
+} while(0)
+
+#define BENCH_VAR(name)                clock_t JOIN(_bench_step,name) = 0, JOIN(_bench_total,name) = 0
+#define BENCH_BEGIN(name)      JOIN(_bench_step, name) = clock()
+#define BENCH_END(name)                JOIN(_bench_total,name) += clock() - JOIN(_bench_step,name)
+#define BENCH_RESET(name)      JOIN(_bench_total, name) = 0
+#define BENCH_REPORT(name)     printf("%s: %fms\n", TO_STR(name), JOIN(_bench_total,name)*1000.0f/CLOCKS_PER_SEC)
+
+#else
+
+#define BENCH(a)       (a)
+#define BENCH_VAR(name)
+#define BENCH_BEGIN(name)
+#define BENCH_END(name)
+#define BENCH_RESET(name)
+#define BENCH_REPORT(name)
+
+#endif
+
+
 typedef struct BVHNode
 {
-       struct BVHNode *children[8]; // max 8 children
+       struct BVHNode **children; // max 8 children
        struct BVHNode *parent; // needed for bottom - top update
-       float bv[26]; // Bounding volume of all nodes, max 13 axis
+       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?
@@ -60,7 +95,8 @@ struct BVHTree
 {
        BVHNode **nodes;
        BVHNode *nodearray; /* pre-alloc branch nodes */
-       int     *orig_index; /* mapping for orig_index to node_index */
+       BVHNode **nodechild;    // pre-alloc childs for nodes
+       float   *nodebv;                // pre-alloc bounding-volumes for nodes
        float   epsilon; /* epslion is used for inflation of the k-dop     */
        int     totleaf; // leafs
        int     totbranch;
@@ -102,12 +138,6 @@ static int size_threshold = 16;
 /*
 * Common methods for all algorithms
 */
-static void bvh_exchange(BVHNode **a, int i, int j)
-{
-       BVHNode *t=a[i];
-       a[i]=a[j];
-       a[j]=t;
-}
 static int floor_lg(int a)
 {
        return (int)(floor(log(a)/log(2)));
@@ -139,11 +169,11 @@ static int bvh_partition(BVHNode **a, int lo, int hi, BVHNode * x, int axis)
        while (1)
        {
                while ((a[i])->bv[axis] < x->bv[axis]) i++;
-               j=j-1;
-               while (x->bv[axis] < (a[j])->bv[axis]) j=j-1;
+               j--;
+               while (x->bv[axis] < (a[j])->bv[axis]) j--;
                if(!(i < j))
                        return i;
-               bvh_exchange(a, i,j);
+               SWAP( BVHNode* , a[i], a[j]);
                i++;
        }
 }
@@ -178,7 +208,7 @@ static void bvh_heapsort(BVHNode **a, int lo, int hi, int axis)
        }
        for (i=n; i>1; i=i-1)
        {
-               bvh_exchange(a, lo,lo+i-1);
+               SWAP(BVHNode*, a[lo],a[lo+i-1]);
                bvh_downheap(a, 1,i-1,lo, axis);
        }
 }
@@ -245,6 +275,25 @@ void sort_along_axis(BVHTree *tree, int start, int end, int axis)
        sort(tree->nodes, start, end, axis);
 }
 
+//after a call to this function you can expect one of:
+//     every node to left of a[n] are smaller than it
+//     every node to the right of a[n-1] are greater than it
+void partition_nth_element(BVHNode **a, int _begin, int _end, int n, int axis)
+{
+       int begin = _begin, end = _end;
+       while(begin < n && end >= n)
+       {
+               int mid = bvh_partition(a, begin, end, bvh_medianof3(a, begin, (begin+end-1)/2, end-1, axis), axis );
+
+               if(mid >= n)
+                       end = n-1;
+               else
+                       begin = n+1;
+       }
+
+}
+
+
 //////////////////////////////////////////////////////////////////////////////////////////////////////
 
 void BLI_bvhtree_free(BVHTree *tree)
@@ -253,7 +302,8 @@ void BLI_bvhtree_free(BVHTree *tree)
        {
                MEM_freeN(tree->nodes);
                MEM_freeN(tree->nodearray);
-               MEM_freeN(tree->orig_index);
+               MEM_freeN(tree->nodebv);
+               MEM_freeN(tree->nodechild);
                MEM_freeN(tree);
        }
 }
@@ -271,37 +321,6 @@ BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
        
        if(tree)
        {
-               // 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->nodearray = (BVHNode *)MEM_callocN(sizeof(BVHNode)*(numbranches+maxsize + tree_type), "BVHNodeArray");
-               
-               if(!tree->nodearray)
-               {
-                       MEM_freeN(tree);
-                       MEM_freeN(tree->nodes);
-                       return NULL;
-               }
-               
-               tree->orig_index = (int *)MEM_callocN(sizeof(int)*(numbranches+maxsize + tree_type), "BVHIndexArray");
-               
-               if(!tree->orig_index)
-               {
-                       MEM_freeN(tree);
-                       MEM_freeN(tree->nodes);
-                       MEM_freeN(tree->nodearray);
-                       return NULL;
-               }
-               
                tree->epsilon = epsilon;
                tree->tree_type = tree_type; 
                tree->axis = axis;
@@ -333,14 +352,62 @@ BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
                }
                else
                {
-                       BLI_bvhtree_free(tree);
+                       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;
 }
 
+
 static void create_kdop_hull(BVHTree *tree, BVHNode *node, float *co, int numpoints, int moving)
 {
        float newminmax;
@@ -371,10 +438,11 @@ static void create_kdop_hull(BVHTree *tree, BVHNode *node, float *co, int numpoi
 }
 
 // depends on the fact that the BVH's for each face is already build
-static void refit_kdop_hull(BVHTree *tree, int start, int end, float *bv)
+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++)
        {
@@ -463,10 +531,6 @@ static void bvh_div_nodes(BVHTree *tree, BVHNode *node, int start, int end, char
        
        // Determine which axis to split along
        laxis = get_largest_axis(node->bv);
-
-       // Sort along longest axis
-       if(laxis!=lastaxis)
-               sort_along_axis(tree, start, end, laxis);
        
        // split nodes along longest axis
        for (i=0; start < end; start += slice, i++) //i counts the current child
@@ -485,8 +549,9 @@ static void bvh_div_nodes(BVHTree *tree, BVHNode *node, int start, int end, char
                        tnode = node->children[i] = tree->nodes[tree->totleaf  + tree->totbranch] = &(tree->nodearray[tree->totbranch + tree->totleaf]);
                        tree->totbranch++;
                        tnode->parent = node;
-
-                       refit_kdop_hull(tree, start, tend, tnode->bv);
+                       
+                       partition_nth_element(tree->nodes, start, end, tend, laxis);
+                       refit_kdop_hull(tree, tnode, start, tend);
                        bvh_div_nodes(tree, tnode, start, tend, laxis);
                }
                node->totnode++;
@@ -545,7 +610,6 @@ static void verify_tree(BVHTree *tree)
 void BLI_bvhtree_balance(BVHTree *tree)
 {
        BVHNode *node;
-       int i;
        
        if(tree->totleaf == 0)
                return;
@@ -555,17 +619,11 @@ void BLI_bvhtree_balance(BVHTree *tree)
        tree->totbranch++;
        
        // refit root bvh node
-       refit_kdop_hull(tree, 0, tree->totleaf, tree->nodes[tree->totleaf]->bv);
+       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, 0);
        
-       // put indices into array for O(1) access
-       for(i = 0; i < tree->totleaf; i++)
-       {
-               tree->orig_index[tree->nodes[i]->index] = i;
-       }
-       
-       verify_tree(tree);
+       // verify_tree(tree);
 }
 
 // overlap - is it possbile for 2 bv's to collide ?
@@ -741,7 +799,7 @@ int BLI_bvhtree_update_node(BVHTree *tree, int index, float *co, float *co_movin
        if(index > tree->totleaf)
                return 0;
        
-       node = tree->nodes[tree->orig_index[index]];
+       node = tree->nodearray + index;
        
        create_kdop_hull(tree, node, co, numpoints, 0);