cgma
KdTree.cpp
Go to the documentation of this file.
00001 /*
00002  *
00003  *
00004  * Copyright (C) 2004 Sandia Corporation.  Under the terms of Contract DE-AC04-94AL85000
00005  * with Sandia Corporation, the U.S. Government retains certain rights in this software.
00006  *
00007  * This file is part of facetbool--contact via cubit@sandia.gov
00008  *
00009  * This library is free software; you can redistribute it and/or
00010  * modify it under the terms of the GNU Lesser General Public
00011  * License as published by the Free Software Foundation; either
00012  * version 2.1 of the License, or (at your option) any later version.
00013  *
00014  * This library is distributed in the hope that it will be useful,
00015  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00016  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00017  * Lesser General Public License for more details.
00018  *
00019  * You should have received a copy of the GNU Lesser General Public
00020  * License along with this library; if not, write to the Free Software
00021  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00022  *
00023  *
00024  *
00025  */
00026 
00027 #include "KdTree.hpp"
00028 #include <stack>
00029 #include <vector>
00030 #include <math.h>
00031 
00032 const int XCUT = 0;
00033 const int YCUT = 1;
00034 const int ZCUT = 2;
00035 
00036 KDTree::KDTree()
00037 {
00038   epsilonkd = 1.e-6;
00039 }
00040 
00041 KDTree::~KDTree()
00042 {
00043 int i;
00044 FSBoundingBox* fsbb;
00045  for ( i = 0; i < 2*numtris-1; i++ ) {
00046     fsbb = treebox[i];
00047     delete fsbb;
00048   }
00049   delete [] treebox;
00050   delete [] nextbranch; 
00051 }
00052 
00053 int KDTree::makeKDTree(int npoly, const FSBOXVECTOR& boxlist)
00054 {
00055 double *xcenterdata, *ycenterdata, *zcenterdata, *whichcut[3], *cutarray; 
00056 int i, cuttingdir, min_tri_sequence_value, max_tri_sequence_value, 
00057     median_value;
00058 double xcen, ycen, zcen;
00059 std::vector<TreeStack* > mytreestack;
00060 int icnt;
00061 TreeStack *thistreestack;
00062 FSBoundingBox *thisbox;
00063 
00064   numtris = npoly;
00065   boxlistptr = boxlist;
00066   isequence = new int[npoly];
00067   xcenterdata = new double[npoly];
00068   ycenterdata = new double[npoly];
00069   zcenterdata = new double[npoly];
00070   nextbranch = new int[2*npoly];
00071   treebox = new FSBoundingBox*[2*npoly];
00072   
00073 //  Get the arrays of bounding box center points for x, y, and z.  We will use 
00074 //  these arrays to find the median values along each direction.
00075   for ( i = 0; i < npoly; i++ ) {
00076     isequence[i] = i;
00077     xcen = 0.5*(boxlist[i]->xmin + boxlist[i]->xmax);
00078     ycen = 0.5*(boxlist[i]->ymin + boxlist[i]->ymax);
00079     zcen = 0.5*(boxlist[i]->zmin + boxlist[i]->zmax);
00080     xcenterdata[i] = xcen;
00081     ycenterdata[i] = ycen;
00082     zcenterdata[i] = zcen;  
00083   }
00084   whichcut[0] = xcenterdata; whichcut[1] = ycenterdata; whichcut[2] = zcenterdata;
00085   
00086   //  Get the bounding box of the root node -- the entire set of bounding boxes.
00087   thisbox = getbox(0,npoly-1);
00088   icnt = 0;
00089   treebox[icnt] = thisbox;
00090   nextbranch[icnt] = icnt;
00091    
00092   max_tri_sequence_value = npoly-1;
00093   min_tri_sequence_value = 0;
00094  
00095   if ( max_tri_sequence_value == 0 ) return 1;  //  Just one triangle in the data set.
00096 
00097   //  Get the cutting direction for the next branch.  
00098   cuttingdir = getcuttingdirection(thisbox); 
00099      
00100   thistreestack = new TreeStack(min_tri_sequence_value,
00101                                        max_tri_sequence_value,
00102                        cuttingdir,icnt);
00103   icnt++;                                         
00104   mytreestack.push_back(thistreestack);
00105     
00106   while ( mytreestack.size() > 0 ) {
00107     TreeStack* thisone = mytreestack[mytreestack.size()-1];
00108     mytreestack.pop_back();
00109     cuttingdir = thisone->cuttingdir;
00110     min_tri_sequence_value = thisone->min;
00111     max_tri_sequence_value = thisone->max;
00112 
00113 
00114     median_value = (min_tri_sequence_value + max_tri_sequence_value)/2;
00115     cutarray = whichcut[cuttingdir];
00116     
00117     find_the_median(median_value-min_tri_sequence_value,0,
00118                     max_tri_sequence_value-min_tri_sequence_value,
00119                     cutarray,&isequence[min_tri_sequence_value]);
00120     if ( min_tri_sequence_value == median_value ) {  //  This is a leaf.
00121 //      FSBoundingBox *thisbox = boxlist[isequence[median_value]];
00122       thisbox = new FSBoundingBox(
00123                              boxlist[isequence[median_value]]->xmin,
00124                  boxlist[isequence[median_value]]->ymin, 
00125                              boxlist[isequence[median_value]]->zmin,
00126                  boxlist[isequence[median_value]]->xmax,
00127                  boxlist[isequence[median_value]]->ymax,
00128                  boxlist[isequence[median_value]]->zmax);
00129 
00130 
00131       treebox[icnt] = thisbox;
00132       nextbranch[icnt] = -isequence[median_value];
00133       nextbranch[thisone->sequence] = icnt;
00134       icnt++;
00135     } else {
00136       thisbox = getbox(min_tri_sequence_value,median_value);
00137         nextbranch[thisone->sequence] = icnt;
00138       treebox[icnt] = thisbox;
00139 //      nextbranch[icnt] = 11111;
00140       cuttingdir = getcuttingdirection(thisbox);
00141 
00142       thistreestack = new TreeStack(min_tri_sequence_value,
00143                                        median_value,cuttingdir,icnt);
00144       icnt++;                                    
00145       mytreestack.push_back(thistreestack);
00146     } 
00147     //  The delete that follows is because we need to delete items that
00148     //  were created and placed on mytreestack.  This one comes from the pop()
00149     
00150     delete thisone;
00151     
00152     if ( median_value+1 == max_tri_sequence_value ) {  //  This is a leaf.
00153 //      FSBoundingBox *thisbox = boxlist[isequence[max_tri_sequence_value]];
00154       thisbox = new FSBoundingBox(
00155                              boxlist[isequence[max_tri_sequence_value]]->xmin,
00156                  boxlist[isequence[max_tri_sequence_value]]->ymin, 
00157                              boxlist[isequence[max_tri_sequence_value]]->zmin,
00158                  boxlist[isequence[max_tri_sequence_value]]->xmax,
00159                  boxlist[isequence[max_tri_sequence_value]]->ymax,
00160                  boxlist[isequence[max_tri_sequence_value]]->zmax);
00161 
00162 
00163       treebox[icnt] = thisbox;
00164       nextbranch[icnt] = -isequence[max_tri_sequence_value];
00165       icnt++;
00166     } else {
00167       thisbox = getbox(median_value+1,max_tri_sequence_value);
00168       treebox[icnt] = thisbox;
00169 //      nextbranch[icnt] = 22222;
00170       cuttingdir = getcuttingdirection(thisbox);
00171       thistreestack = new TreeStack(median_value+1,
00172                                        max_tri_sequence_value,
00173                        cuttingdir,icnt);
00174       icnt++;                                     
00175       mytreestack.push_back(thistreestack);    
00176     }     
00177 
00178   } 
00179  
00180   delete [] isequence;
00181   delete [] xcenterdata;
00182   delete [] ycenterdata;
00183   delete [] zcenterdata;
00184    
00185   return 1;
00186 }
00187 
00188 FSBoundingBox* KDTree::getbox(int min, int max)
00189 {
00190 FSBoundingBox *thisbox;
00191 int i;
00192 //  Makes a bounding box and sets its size to include all of the boxes in the
00193 //  sequence of bounding boxes from min to max of index isequence[].  Returns pointer
00194 //  to this box.
00195 
00196   thisbox = new FSBoundingBox(1.e20,1.e20,1.e20,-1.e20,-1.e20,-1.e20);
00197 double xmin, ymin, zmin, xmax, ymax, zmax;
00198   
00199   for ( i = min; i <= max; i++ ) {
00200     xmin = boxlistptr[isequence[i]]->xmin;
00201     ymin = boxlistptr[isequence[i]]->ymin;
00202     zmin = boxlistptr[isequence[i]]->zmin;
00203     xmax = boxlistptr[isequence[i]]->xmax;
00204     ymax = boxlistptr[isequence[i]]->ymax;
00205     zmax = boxlistptr[isequence[i]]->zmax;
00206 
00207     if ( xmin < thisbox->xmin ) thisbox->xmin = xmin;
00208     if ( ymin < thisbox->ymin ) thisbox->ymin = ymin;
00209     if ( zmin < thisbox->zmin ) thisbox->zmin = zmin;    
00210     if ( xmax > thisbox->xmax ) thisbox->xmax = xmax;
00211     if ( ymax > thisbox->ymax ) thisbox->ymax = ymax;
00212     if ( zmax > thisbox->zmax ) thisbox->zmax = zmax;
00213      
00214   }
00215 
00216   return thisbox;
00217 }
00218 
00219 void KDTree::box_kdtree_intersect(FSBoundingBox& bbox, int *count, int *indexlist) const
00220 {
00221 int index, i, child;
00222 std::stack<int> mystack;
00223 
00224   *count = 0;
00225   if ( (bbox.xmax < (treebox[0]->xmin - epsilonkd) ) ||
00226        (bbox.xmin > (treebox[0]->xmax + epsilonkd) ) ||
00227        (bbox.ymax < (treebox[0]->ymin - epsilonkd) ) ||
00228        (bbox.ymin > (treebox[0]->ymax + epsilonkd) ) ||
00229        (bbox.zmax < (treebox[0]->zmin - epsilonkd) ) ||
00230        (bbox.zmin > (treebox[0]->zmax + epsilonkd) ) )
00231     return;
00232   //  Gotta put something on the stack, so do the root node 
00233   //  evaluation here.
00234   if ( nextbranch[0] <= 0 ) {
00235     indexlist[*count] = -nextbranch[0]; *count += 1;
00236     return;
00237   }
00238   mystack.push(0);
00239 
00240   while ( mystack.size() > 0 ) {
00241     index = mystack.top();
00242     mystack.pop();
00243     child = nextbranch[index];
00244     
00245     for ( i = 0; i < 2; i++ ) {
00246       if ( (bbox.xmax < (treebox[child+i]->xmin - epsilonkd) ) ||
00247            (bbox.xmin > (treebox[child+i]->xmax + epsilonkd) ) ||
00248            (bbox.ymax < (treebox[child+i]->ymin - epsilonkd) ) ||
00249            (bbox.ymin > (treebox[child+i]->ymax + epsilonkd) ) ||
00250            (bbox.zmax < (treebox[child+i]->zmin - epsilonkd) ) ||
00251            (bbox.zmin > (treebox[child+i]->zmax + epsilonkd) ) )
00252       continue;
00253       if ( nextbranch[child+i] <= 0 ) {        
00254         indexlist[*count] = -nextbranch[child+i]; *count += 1;  
00255       } else {            
00256         mystack.push(child+i);   
00257       }  
00258     }
00259   }  
00260   return;
00261 }
00262 
00263 
00264 void KDTree::find_the_median(int k, int l, int r, double *array, int *ia)
00265 {
00266 int i, j;
00267 double t;
00268 
00269   while ( r > l ) {
00270     t = array[ia[k]];
00271     i = l;
00272     j = r;
00273 
00274     SWAP(ia[l],ia[k]);
00275     if ( array[ia[r]] > t ) SWAP(ia[l],ia[r]);
00276     while ( i < j ) {
00277       SWAP(ia[i],ia[j]);
00278       i += 1; j -= 1;
00279       while ( array[ia[i]] < t ) i++;
00280       while ( array[ia[j]] > t ) j--;
00281     }
00282     if ( array[ia[l]] == t ) SWAP(ia[l],ia[j]);
00283     else {
00284       j += 1;
00285       SWAP(ia[r],ia[j]);
00286     }
00287     if ( j <= k ) l = j + 1;
00288     if ( k <= j ) r = j - 1;
00289   }
00290 
00291 }
00292 
00293 int KDTree::getcuttingdirection(FSBoundingBox* box)
00294 {
00295 double xlen, ylen, zlen;
00296 
00297   xlen = box->xmax - box->xmin;
00298   ylen = box->ymax - box->ymin;
00299   zlen = box->zmax - box->zmin;
00300   
00301   if ( (xlen >= ylen) && (xlen >= zlen) ) return XCUT; 
00302   else if ( ylen >= zlen ) return YCUT;
00303   else return ZCUT;
00304 }
00305 
00306 
00307 bool KDTree::rayintersectsbox(FSBoundingBox *box)
00308 {
00309 double pmin, pmax, tmin, tmax;
00310 double dtemp;
00311 
00312 //  Get the parametric distance along each direction from the min point to
00313 //  the min and max box planes.  If the min dist is grater than the max
00314 //  dist, we have to swap them.  Keep a running total of the max min and the
00315 //  min max.  The ray intersects the box iff tmin <= tmax and tmax >= 0.0.
00316 //  Otherwise, the ray misses the box or points away from the box (with the
00317 //  starting point outside).
00318 
00319   tmin = (box->xmin - rayxstart)/dx;
00320   tmax = (box->xmax - rayxstart)/dx;
00321   if ( tmin > tmax ) {
00322   dtemp = tmin; tmin = tmax; tmax = dtemp;
00323   }
00324   pmin = (box->ymin - rayystart)/dy;
00325   pmax = (box->ymax - rayystart)/dy;
00326   if ( pmin > pmax ) {
00327   dtemp = pmin; pmin = pmax; pmax = dtemp;
00328   }
00329   tmin = MAXX(pmin,tmin);
00330   tmax = MINN(pmax,tmax);
00331 
00332   if ( tmin > tmax ) return false;
00333   
00334   pmin = (box->zmin - rayzstart)/dz;
00335   pmax = (box->zmax - rayzstart)/dz;
00336   if ( pmin > pmax ) {
00337   dtemp = pmin; pmin = pmax; pmax = dtemp;
00338   }
00339   tmin = MAXX(pmin,tmin);
00340   tmax = MINN(pmax,tmax);
00341   
00342   if ( (tmax < 0.0) || (tmin > tmax) ) return false;
00343   
00344   return true;
00345 }
00346   
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines