Reason of last commits kdopbvh tree is now on trunk.

Added those files to the branch and added code for NearestNeighbour, needed for shrinkwrap

diff --git a/source/blender/blenlib/BLI_kdopbvh.h b/source/blender/blenlib/BLI_kdopbvh.h
index b81ff0ee66f9588e15bd218969553d2524490cac..41ff97d111dfddf6dff43cb792f00d78185a65e5 100644 (file)
--- a/source/blender/blenlib/BLI_kdopbvh.h
+++ b/source/blender/blenlib/BLI_kdopbvh.h
@@ -40,6 +40,17 @@ 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 nearest[3];       /* nearest coordinates (untouched it none is found within a dist radius from the given coordinates) */
+       float dist;                     /* squared distance to search arround */
+} BVHTreeNearest;
+
+/* returns square of the minimum distance from given co to the node, nearest point is stored on nearest */
+typedef float (*BVHTree_NearestPointCallback) (void *userdata, int index, const float *co, float *nearest);
+
+
 BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis);
 void BLI_bvhtree_free(BVHTree *tree);
 
@@ -56,5 +67,8 @@ 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, float *co, BVHTreeNearest *nearest, BVHTree_NearestPointCallback 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 9c4238431dcb5fcf5f8cce857bbab0b90d5e097f..2dc345e894e197158437190804d61719a386f7f0 100644 (file)
--- a/source/blender/blenlib/intern/BLI_kdopbvh.c
+++ b/source/blender/blenlib/intern/BLI_kdopbvh.c
@@ -73,6 +73,16 @@ typedef struct BVHOverlapData
        BVHTreeOverlap *overlap; 
        int i, max_overlap; /* i is number of overlaps */
 } BVHOverlapData;
+
+typedef struct BVHNearestData
+{
+       BVHTree *tree;
+       float   *co;
+       BVHTree_NearestPointCallback callback;
+       void    *userdata;
+       float proj[13];                 //coordinates projection over axis
+       BVHTreeNearest nearest;
+} BVHNearestData;
 ////////////////////////////////////////
 
 
@@ -242,7 +252,6 @@ void sort_along_axis(BVHTree *tree, int start, int end, int axis)
 //      every node to the right of a[n] are greater or equal to it
 int partition_nth_element(BVHNode **a, int _begin, int _end, int n, int axis){        
        int begin = _begin, end = _end, cut;        
-       int i;         
        while(end-begin > 3)        
        {                            
                cut = bvh_partition(a, begin, end, bvh_medianof3(a, begin, (begin+end)/2, end-1, axis), axis );                 
@@ -256,7 +265,6 @@ int partition_nth_element(BVHNode **a, int _begin, int _end, int n, int axis){
        return n;
 }
 
-
 //////////////////////////////////////////////////////////////////////////////////////////////////////
 
 void BLI_bvhtree_free(BVHTree *tree)
@@ -374,6 +382,7 @@ BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
 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
@@ -381,8 +390,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;
                }
        }
        
@@ -392,10 +401,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;
                }
        }
 }
@@ -591,8 +600,11 @@ void BLI_bvhtree_balance(BVHTree *tree)
 }
 
 // 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 +624,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, MIN2(data->tree1->start_axis, data->tree2->start_axis), MIN2(data->tree1->stop_axis, data->tree2->stop_axis)))
        {
                // check if node1 is a leaf
                if(!node1->totnode)
@@ -678,7 +690,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");
@@ -809,3 +821,123 @@ float BLI_bvhtree_getepsilon(BVHTree *tree)
 {
        return tree->epsilon;
 }
+
+
+//Nearest neighbour
+static float squared_dist(const float *a, const float *b)
+{
+       float tmp[3];
+       VECSUB(tmp, a, b);
+       return INPR(tmp, tmp);
+}
+
+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)
+       {
+               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];
+       }
+
+/*
+       //nearest on a general hull
+       VECCOPY(nearest, data->co);
+       for(i = data->tree->start_axis; i != data->tree->stop_axis; i++, bv+=2)
+       {
+               float proj = INPR( nearest, KDOP_AXES[i]);
+               float dl = bv[0] - proj;
+               float du = bv[1] - proj;
+
+               if(dl > 0)
+               {
+                       VECADDFAC(nearest, nearest, KDOP_AXES[i], dl);
+               }
+               else if(du < 0)
+               {
+                       VECADDFAC(nearest, nearest, KDOP_AXES[i], du);
+               }
+       }
+*/
+       return squared_dist(data->co, nearest);
+}
+
+
+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)
+                       sdist = data->callback(data->userdata , node->index, data->co, nearest);
+
+               if(sdist >= data->nearest.dist) return;
+
+               data->nearest.index     = node->index;
+               VECCOPY(data->nearest.nearest, nearest);
+               data->nearest.dist      = sdist;
+       }
+       else
+       {
+               if(sdist < data->nearest.dist)
+               {
+                       for(i=0; i != node->totnode; i++)
+                       {
+                               dfs_find_nearest(data, node->children[i]);
+                       }
+               }
+       }
+}
+
+int BLI_bvhtree_find_nearest(BVHTree *tree, float *co, BVHTreeNearest *nearest, BVHTree_NearestPointCallback callback, void *userdata)
+{
+       int i;
+
+       BVHNearestData data;
+
+       //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++)
+       {
+               data.proj[i] = INPR(data.co, KDOP_AXES[i]);
+       }
+
+       if(nearest)
+       {
+               memcpy( &data.nearest , nearest, sizeof(*nearest) );
+       }
+       else
+       {
+               data.nearest.index = -1;
+               data.nearest.dist = FLT_MAX;
+       }
+
+       //dfs search
+       dfs_find_nearest(&data, tree->nodes[tree->totleaf] );
+
+       //copy back results
+       if(nearest)
+       {
+               memcpy(nearest, &data.nearest, sizeof(*nearest));
+       }
+
+       return data.nearest.index;
+}
+