Added BVH nearest neighbour code, for now only works in 6-dop and finds the node...
authorAndre Susano Pinto <andresusanopinto@gmail.com>
Sun, 25 May 2008 15:43:18 +0000 (15:43 +0000)
committerAndre Susano Pinto <andresusanopinto@gmail.com>
Sun, 25 May 2008 15:43:18 +0000 (15:43 +0000)
I'll work on making it more generic.
So far it querys faster than kdtree, but building the tree is slower.
And bvhtree NN uses an heuristic based on the last match.

Shrinkwrap (OBCube)24578 over (OBSuzanne)31658
kdtree
build: 30.000000ms
query: 1360.000000ms

bvhtree
build: 140.000000ms
query: 490.000000ms

Shrinkwrap now uses bvhtree (binary tree, 6dop) for nearest vertex.

source/blender/blenkernel/intern/shrinkwrap.c
source/blender/blenlib/BLI_kdopbvh.h
source/blender/blenlib/intern/BLI_kdopbvh.c

index dca210afb278a0ba69ffce1aea34318f3a7f40d2..c24f6e8cd8ac8cb22c54ff0dd0f684bed992285c 100644 (file)
@@ -234,6 +234,16 @@ static float raytree_cast_ray(RayTree *tree, const float *coord, const float *di
        return VecLenf((float*)coord, (float*)isec.end);
 }
 
+/*
+ * Returns the squared distance between two given points
+ */
+static float squared_dist(const float *a, const float *b)
+{
+       float tmp[3];
+       VECSUB(tmp, a, b);
+       return INPR(tmp, tmp);
+}
+
 /*
  * This calculates the distance (in dir units) that the ray must travel to intersect plane
  * It can return negative values
@@ -782,11 +792,14 @@ void shrinkwrap_calc_nearest_vertex(ShrinkwrapCalcData *calc)
        int i;
        int vgroup              = get_named_vertexgroup_num(calc->ob, calc->smd->vgroup_name);
 
+/*
        KDTree* target = NULL;
-       KDTreeNearest nearest;
+       KDTreeNearest knearest;
+*/
        float tmp_co[3];
 
        BVHTree *tree   = NULL;
+       BVHTreeNearest nearest;
 
        BENCH_VAR(build);
        BENCH_VAR(query);
@@ -798,19 +811,26 @@ void shrinkwrap_calc_nearest_vertex(ShrinkwrapCalcData *calc)
        numVerts= calc->target->getNumVerts(calc->target);
        vert = tvert    = calc->target->getVertDataArray(calc->target, CD_MVERT);       
 
+
        BENCH_RESET(build);
        BENCH_BEGIN(build);
 
-       tree = BLI_bvhtree_new(numVerts, 0, 8, 6);
+       //Create a bvh-tree of the given target
+       tree = BLI_bvhtree_new(numVerts, 0, 2, 6);
        if(tree == NULL) return OUT_OF_MEMORY();
 
        for(i = 0; i < numVerts; i++)
                BLI_bvhtree_insert(tree, i, vert[i].co, 1);
+
        BLI_bvhtree_balance(tree);
+
+       nearest.index = -1;
+       nearest.dist = FLT_MAX;
        BENCH_END(build);
        BENCH_REPORT(build);
 
 
+/*
        //Generate kd-tree with target vertexs
        BENCH_RESET(build);
        BENCH_BEGIN(build);
@@ -825,7 +845,7 @@ void shrinkwrap_calc_nearest_vertex(ShrinkwrapCalcData *calc)
 
        BENCH_END(build);
        BENCH_REPORT(build);
-
+*/
 
        //Find the nearest vertex 
        numVerts= calc->final->getNumVerts(calc->final);
@@ -835,40 +855,55 @@ void shrinkwrap_calc_nearest_vertex(ShrinkwrapCalcData *calc)
        BENCH_BEGIN(query);
        for(i=0; i<numVerts; i++)
        {
-               int t, index;
+               int index;
                float weight = vertexgroup_get_weight(dvert, i, vgroup);
                if(weight == 0.0f) continue;
 
-/*             VecMat4MulVecfl(tmp_co, calc->local2target, vert[i].co);
+               VecMat4MulVecfl(tmp_co, calc->local2target, vert[i].co);
+
+               if(nearest.index != -1)
+               {
+                       nearest.dist = squared_dist(tmp_co, tvert[nearest.index].co);
+               }
+               else nearest.dist = FLT_MAX;
 
-               index = BLI_bvhtree_find_nearest(tree, tmp_co);
+               index = BLI_bvhtree_find_nearest(tree, tmp_co, &nearest);
+
+/*
+               t = BLI_kdtree_find_nearest(target, tmp_co, 0, &knearest);
+
+
+               if(VecLenf(knearest.co, tvert[index].co) > 1e-5)
+               {
+                       printf("Nearest failed: {%f,%f,%f} - ", knearest.co[0], knearest.co[1], knearest.co[2]);
+                       printf("{%f,%f,%f}\n", tvert[index].co[0], tvert[index].co[1], tvert[index].co[2]);
+               }
+*/
                if(index != -1)
                {
                        float dist;
+
                        VecMat4MulVecfl(tmp_co, calc->target2local, tvert[index].co);
                        dist = VecLenf(vert[i].co, tmp_co);
                        if(dist > 1e-5) weight *= (dist - calc->keptDist)/dist;
-                       VecLerpf(vert[i].co, vert[i].co, nearest.co, weight);   //linear interpolation
+                       VecLerpf(vert[i].co, vert[i].co, tmp_co, weight);       //linear interpolation
                }
 
-       */      
-               t = BLI_kdtree_find_nearest(target, tmp_co, 0, &nearest);
-
-               if(t != -1)
+/*             if(t != -1)
                {
                        float dist;
 
-                       VecMat4MulVecfl(nearest.co, calc->target2local, nearest.co);
-                       dist = VecLenf(vert[i].co, tmp_co);
+                       VecMat4MulVecfl(knearest.co, calc->target2local, knearest.co);
+                       dist = VecLenf(vert[i].co, knearest.co);
                        if(dist > 1e-5) weight *= (dist - calc->keptDist)/dist;
-                       VecLerpf(vert[i].co, vert[i].co, nearest.co, weight);   //linear interpolation
+                       VecLerpf(vert[i].co, vert[i].co, knearest.co, weight);  //linear interpolation
                }
-               
+*/
        }
        BENCH_END(query);
        BENCH_REPORT(query);
 
-       BLI_kdtree_free(target);
+//     BLI_kdtree_free(target);
        BLI_bvhtree_free(tree);
 }
 
index c1240da6c3ad7be97431e16de50b8e919993e621..18ecfe7a2e9ca40fb1b6978d8e3baa642f5a3a0a 100644 (file)
@@ -40,6 +40,12 @@ 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 dist;             /* squared distance to search arround */
+} BVHTreeNearest;
+
 BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis);
 void BLI_bvhtree_free(BVHTree *tree);
 
@@ -56,6 +62,9 @@ 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);
+
 #endif // BLI_KDOPBVH_H
 
 
index 4b72085a2b117daf3cc3a917dfb57a5547f9e4db..c510f9afb00d005a72a13b424d4026f57a359f7b 100644 (file)
@@ -112,6 +112,14 @@ typedef struct BVHOverlapData
        BVHTreeOverlap *overlap; 
        int i, max_overlap; /* i is number of overlaps */
 } BVHOverlapData;
+
+typedef struct BVHNearestData
+{
+       BVHTree *tree;
+       float   *co;
+       float proj[13];                 //coordinates projection over axis
+       BVHTreeNearest nearest;
+} BVHNearestData;
 ////////////////////////////////////////
 
 
@@ -340,12 +348,12 @@ BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
                        tree->start_axis = 0;
                        tree->stop_axis = 7;
                }
-               else if(axis == 8) // AABB
+               else if(axis == 8)
                {
                        tree->start_axis = 0;
                        tree->stop_axis = 4;
                }
-               else if(axis == 6) // OBB
+               else if(axis == 6) // AABB
                {
                        tree->start_axis = 0;
                        tree->stop_axis = 3;
@@ -857,3 +865,111 @@ 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_get_bv(data->tree, node);
+
+       //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)
+       {
+               data->nearest.dist      = sdist;
+               data->nearest.index     = node->index;
+       }
+       else
+       {
+               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)
+{
+       int i;
+
+       BVHNearestData data;
+
+       //init data to search
+       data.tree = tree;
+       data.co = co;
+
+       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;
+}
+