Branch data Line data Source code
1 : : /*
2 : : *
3 : : *
4 : : * Copyright (C) 2004 Sandia Corporation. Under the terms of Contract DE-AC04-94AL85000
5 : : * with Sandia Corporation, the U.S. Government retains certain rights in this software.
6 : : *
7 : : * This file is part of facetbool--contact via [email protected]
8 : : *
9 : : * This library is free software; you can redistribute it and/or
10 : : * modify it under the terms of the GNU Lesser General Public
11 : : * License as published by the Free Software Foundation; either
12 : : * version 2.1 of the License, or (at your option) any later version.
13 : : *
14 : : * This library is distributed in the hope that it will be useful,
15 : : * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 : : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
17 : : * Lesser General Public License for more details.
18 : : *
19 : : * You should have received a copy of the GNU Lesser General Public
20 : : * License along with this library; if not, write to the Free Software
21 : : * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
22 : : *
23 : : *
24 : : *
25 : : */
26 : :
27 : : #include "KdTree.hpp"
28 : : #include <stack>
29 : : #include <vector>
30 : : #include <math.h>
31 : :
32 : : const int XCUT = 0;
33 : : const int YCUT = 1;
34 : : const int ZCUT = 2;
35 : :
36 : 0 : KDTree::KDTree()
37 : : {
38 : 0 : epsilonkd = 1.e-6;
39 : 0 : }
40 : :
41 : 0 : KDTree::~KDTree()
42 : : {
43 : : int i;
44 : : FSBoundingBox* fsbb;
45 [ # # ]: 0 : for ( i = 0; i < 2*numtris-1; i++ ) {
46 : 0 : fsbb = treebox[i];
47 [ # # ][ # # ]: 0 : delete fsbb;
48 : : }
49 [ # # ]: 0 : delete [] treebox;
50 [ # # ]: 0 : delete [] nextbranch;
51 : 0 : }
52 : :
53 : 0 : int KDTree::makeKDTree(int npoly, const FSBOXVECTOR& boxlist)
54 : : {
55 : : double *xcenterdata, *ycenterdata, *zcenterdata, *whichcut[3], *cutarray;
56 : : int i, cuttingdir, min_tri_sequence_value, max_tri_sequence_value,
57 : : median_value;
58 : : double xcen, ycen, zcen;
59 [ # # ]: 0 : std::vector<TreeStack* > mytreestack;
60 : : int icnt;
61 : : TreeStack *thistreestack;
62 : : FSBoundingBox *thisbox;
63 : :
64 : 0 : numtris = npoly;
65 [ # # ]: 0 : boxlistptr = boxlist;
66 [ # # ][ # # ]: 0 : isequence = new int[npoly];
67 [ # # ][ # # ]: 0 : xcenterdata = new double[npoly];
68 [ # # ][ # # ]: 0 : ycenterdata = new double[npoly];
69 [ # # ][ # # ]: 0 : zcenterdata = new double[npoly];
70 [ # # ][ # # ]: 0 : nextbranch = new int[2*npoly];
71 [ # # ][ # # ]: 0 : treebox = new FSBoundingBox*[2*npoly];
72 : :
73 : : // Get the arrays of bounding box center points for x, y, and z. We will use
74 : : // these arrays to find the median values along each direction.
75 [ # # ]: 0 : for ( i = 0; i < npoly; i++ ) {
76 : 0 : isequence[i] = i;
77 [ # # ][ # # ]: 0 : xcen = 0.5*(boxlist[i]->xmin + boxlist[i]->xmax);
78 [ # # ][ # # ]: 0 : ycen = 0.5*(boxlist[i]->ymin + boxlist[i]->ymax);
79 [ # # ][ # # ]: 0 : zcen = 0.5*(boxlist[i]->zmin + boxlist[i]->zmax);
80 : 0 : xcenterdata[i] = xcen;
81 : 0 : ycenterdata[i] = ycen;
82 : 0 : zcenterdata[i] = zcen;
83 : : }
84 : 0 : whichcut[0] = xcenterdata; whichcut[1] = ycenterdata; whichcut[2] = zcenterdata;
85 : :
86 : : // Get the bounding box of the root node -- the entire set of bounding boxes.
87 [ # # ]: 0 : thisbox = getbox(0,npoly-1);
88 : 0 : icnt = 0;
89 : 0 : treebox[icnt] = thisbox;
90 : 0 : nextbranch[icnt] = icnt;
91 : :
92 : 0 : max_tri_sequence_value = npoly-1;
93 : 0 : min_tri_sequence_value = 0;
94 : :
95 [ # # ]: 0 : if ( max_tri_sequence_value == 0 ) return 1; // Just one triangle in the data set.
96 : :
97 : : // Get the cutting direction for the next branch.
98 [ # # ]: 0 : cuttingdir = getcuttingdirection(thisbox);
99 : :
100 : : thistreestack = new TreeStack(min_tri_sequence_value,
101 : : max_tri_sequence_value,
102 [ # # ][ # # ]: 0 : cuttingdir,icnt);
103 : 0 : icnt++;
104 [ # # ]: 0 : mytreestack.push_back(thistreestack);
105 : :
106 [ # # ][ # # ]: 0 : while ( mytreestack.size() > 0 ) {
107 [ # # ][ # # ]: 0 : TreeStack* thisone = mytreestack[mytreestack.size()-1];
108 [ # # ]: 0 : mytreestack.pop_back();
109 : 0 : cuttingdir = thisone->cuttingdir;
110 : 0 : min_tri_sequence_value = thisone->min;
111 : 0 : max_tri_sequence_value = thisone->max;
112 : :
113 : :
114 : 0 : median_value = (min_tri_sequence_value + max_tri_sequence_value)/2;
115 : 0 : cutarray = whichcut[cuttingdir];
116 : :
117 : : find_the_median(median_value-min_tri_sequence_value,0,
118 : : max_tri_sequence_value-min_tri_sequence_value,
119 [ # # ]: 0 : cutarray,&isequence[min_tri_sequence_value]);
120 [ # # ]: 0 : if ( min_tri_sequence_value == median_value ) { // This is a leaf.
121 : : // FSBoundingBox *thisbox = boxlist[isequence[median_value]];
122 : : thisbox = new FSBoundingBox(
123 [ # # ]: 0 : boxlist[isequence[median_value]]->xmin,
124 [ # # ]: 0 : boxlist[isequence[median_value]]->ymin,
125 [ # # ]: 0 : boxlist[isequence[median_value]]->zmin,
126 [ # # ]: 0 : boxlist[isequence[median_value]]->xmax,
127 [ # # ]: 0 : boxlist[isequence[median_value]]->ymax,
128 [ # # ][ # # ]: 0 : boxlist[isequence[median_value]]->zmax);
[ # # ]
129 : :
130 : :
131 : 0 : treebox[icnt] = thisbox;
132 : 0 : nextbranch[icnt] = -isequence[median_value];
133 : 0 : nextbranch[thisone->sequence] = icnt;
134 : 0 : icnt++;
135 : : } else {
136 [ # # ]: 0 : thisbox = getbox(min_tri_sequence_value,median_value);
137 : 0 : nextbranch[thisone->sequence] = icnt;
138 : 0 : treebox[icnt] = thisbox;
139 : : // nextbranch[icnt] = 11111;
140 [ # # ]: 0 : cuttingdir = getcuttingdirection(thisbox);
141 : :
142 : : thistreestack = new TreeStack(min_tri_sequence_value,
143 [ # # ][ # # ]: 0 : median_value,cuttingdir,icnt);
144 : 0 : icnt++;
145 [ # # ]: 0 : mytreestack.push_back(thistreestack);
146 : : }
147 : : // The delete that follows is because we need to delete items that
148 : : // were created and placed on mytreestack. This one comes from the pop()
149 : :
150 [ # # ][ # # ]: 0 : delete thisone;
151 : :
152 [ # # ]: 0 : if ( median_value+1 == max_tri_sequence_value ) { // This is a leaf.
153 : : // FSBoundingBox *thisbox = boxlist[isequence[max_tri_sequence_value]];
154 : : thisbox = new FSBoundingBox(
155 [ # # ]: 0 : boxlist[isequence[max_tri_sequence_value]]->xmin,
156 [ # # ]: 0 : boxlist[isequence[max_tri_sequence_value]]->ymin,
157 [ # # ]: 0 : boxlist[isequence[max_tri_sequence_value]]->zmin,
158 [ # # ]: 0 : boxlist[isequence[max_tri_sequence_value]]->xmax,
159 [ # # ]: 0 : boxlist[isequence[max_tri_sequence_value]]->ymax,
160 [ # # ][ # # ]: 0 : boxlist[isequence[max_tri_sequence_value]]->zmax);
[ # # ]
161 : :
162 : :
163 : 0 : treebox[icnt] = thisbox;
164 : 0 : nextbranch[icnt] = -isequence[max_tri_sequence_value];
165 : 0 : icnt++;
166 : : } else {
167 [ # # ]: 0 : thisbox = getbox(median_value+1,max_tri_sequence_value);
168 : 0 : treebox[icnt] = thisbox;
169 : : // nextbranch[icnt] = 22222;
170 [ # # ]: 0 : cuttingdir = getcuttingdirection(thisbox);
171 : : thistreestack = new TreeStack(median_value+1,
172 : : max_tri_sequence_value,
173 [ # # ][ # # ]: 0 : cuttingdir,icnt);
174 : 0 : icnt++;
175 [ # # ]: 0 : mytreestack.push_back(thistreestack);
176 : : }
177 : :
178 : : }
179 : :
180 [ # # ]: 0 : delete [] isequence;
181 [ # # ]: 0 : delete [] xcenterdata;
182 [ # # ]: 0 : delete [] ycenterdata;
183 [ # # ]: 0 : delete [] zcenterdata;
184 : :
185 [ # # ]: 0 : return 1;
186 : : }
187 : :
188 : 0 : FSBoundingBox* KDTree::getbox(int min, int max)
189 : : {
190 : : FSBoundingBox *thisbox;
191 : : int i;
192 : : // Makes a bounding box and sets its size to include all of the boxes in the
193 : : // sequence of bounding boxes from min to max of index isequence[]. Returns pointer
194 : : // to this box.
195 : :
196 [ # # ]: 0 : thisbox = new FSBoundingBox(1.e20,1.e20,1.e20,-1.e20,-1.e20,-1.e20);
197 : : double xmin, ymin, zmin, xmax, ymax, zmax;
198 : :
199 [ # # ]: 0 : for ( i = min; i <= max; i++ ) {
200 : 0 : xmin = boxlistptr[isequence[i]]->xmin;
201 : 0 : ymin = boxlistptr[isequence[i]]->ymin;
202 : 0 : zmin = boxlistptr[isequence[i]]->zmin;
203 : 0 : xmax = boxlistptr[isequence[i]]->xmax;
204 : 0 : ymax = boxlistptr[isequence[i]]->ymax;
205 : 0 : zmax = boxlistptr[isequence[i]]->zmax;
206 : :
207 [ # # ]: 0 : if ( xmin < thisbox->xmin ) thisbox->xmin = xmin;
208 [ # # ]: 0 : if ( ymin < thisbox->ymin ) thisbox->ymin = ymin;
209 [ # # ]: 0 : if ( zmin < thisbox->zmin ) thisbox->zmin = zmin;
210 [ # # ]: 0 : if ( xmax > thisbox->xmax ) thisbox->xmax = xmax;
211 [ # # ]: 0 : if ( ymax > thisbox->ymax ) thisbox->ymax = ymax;
212 [ # # ]: 0 : if ( zmax > thisbox->zmax ) thisbox->zmax = zmax;
213 : :
214 : : }
215 : :
216 : 0 : return thisbox;
217 : : }
218 : :
219 : 0 : void KDTree::box_kdtree_intersect(FSBoundingBox& bbox, int *count, int *indexlist) const
220 : : {
221 : : int index, i, child;
222 [ # # ][ # # ]: 0 : std::stack<int> mystack;
[ # # ]
223 : :
224 : 0 : *count = 0;
225 [ # # ][ # # ]: 0 : if ( (bbox.xmax < (treebox[0]->xmin - epsilonkd) ) ||
226 [ # # ]: 0 : (bbox.xmin > (treebox[0]->xmax + epsilonkd) ) ||
227 [ # # ]: 0 : (bbox.ymax < (treebox[0]->ymin - epsilonkd) ) ||
228 [ # # ]: 0 : (bbox.ymin > (treebox[0]->ymax + epsilonkd) ) ||
229 [ # # ]: 0 : (bbox.zmax < (treebox[0]->zmin - epsilonkd) ) ||
230 : 0 : (bbox.zmin > (treebox[0]->zmax + epsilonkd) ) )
231 : 0 : return;
232 : : // Gotta put something on the stack, so do the root node
233 : : // evaluation here.
234 [ # # ]: 0 : if ( nextbranch[0] <= 0 ) {
235 : 0 : indexlist[*count] = -nextbranch[0]; *count += 1;
236 : 0 : return;
237 : : }
238 [ # # ]: 0 : mystack.push(0);
239 : :
240 [ # # ][ # # ]: 0 : while ( mystack.size() > 0 ) {
241 [ # # ]: 0 : index = mystack.top();
242 [ # # ]: 0 : mystack.pop();
243 : 0 : child = nextbranch[index];
244 : :
245 [ # # ]: 0 : for ( i = 0; i < 2; i++ ) {
246 [ # # ][ # # ]: 0 : if ( (bbox.xmax < (treebox[child+i]->xmin - epsilonkd) ) ||
247 [ # # ]: 0 : (bbox.xmin > (treebox[child+i]->xmax + epsilonkd) ) ||
248 [ # # ]: 0 : (bbox.ymax < (treebox[child+i]->ymin - epsilonkd) ) ||
249 [ # # ]: 0 : (bbox.ymin > (treebox[child+i]->ymax + epsilonkd) ) ||
250 [ # # ]: 0 : (bbox.zmax < (treebox[child+i]->zmin - epsilonkd) ) ||
251 : 0 : (bbox.zmin > (treebox[child+i]->zmax + epsilonkd) ) )
252 : 0 : continue;
253 [ # # ]: 0 : if ( nextbranch[child+i] <= 0 ) {
254 : 0 : indexlist[*count] = -nextbranch[child+i]; *count += 1;
255 : : } else {
256 [ # # ]: 0 : mystack.push(child+i);
257 : : }
258 : : }
259 : : }
260 [ # # ]: 0 : return;
261 : : }
262 : :
263 : :
264 : 0 : void KDTree::find_the_median(int k, int l, int r, double *array, int *ia)
265 : : {
266 : : int i, j;
267 : : double t;
268 : :
269 [ # # ]: 0 : while ( r > l ) {
270 : 0 : t = array[ia[k]];
271 : 0 : i = l;
272 : 0 : j = r;
273 : :
274 : 0 : SWAP(ia[l],ia[k]);
275 [ # # ]: 0 : if ( array[ia[r]] > t ) SWAP(ia[l],ia[r]);
276 [ # # ]: 0 : while ( i < j ) {
277 : 0 : SWAP(ia[i],ia[j]);
278 : 0 : i += 1; j -= 1;
279 [ # # ]: 0 : while ( array[ia[i]] < t ) i++;
280 [ # # ]: 0 : while ( array[ia[j]] > t ) j--;
281 : : }
282 [ # # ]: 0 : if ( array[ia[l]] == t ) SWAP(ia[l],ia[j]);
283 : : else {
284 : 0 : j += 1;
285 : 0 : SWAP(ia[r],ia[j]);
286 : : }
287 [ # # ]: 0 : if ( j <= k ) l = j + 1;
288 [ # # ]: 0 : if ( k <= j ) r = j - 1;
289 : : }
290 : :
291 : 0 : }
292 : :
293 : 0 : int KDTree::getcuttingdirection(FSBoundingBox* box)
294 : : {
295 : : double xlen, ylen, zlen;
296 : :
297 : 0 : xlen = box->xmax - box->xmin;
298 : 0 : ylen = box->ymax - box->ymin;
299 : 0 : zlen = box->zmax - box->zmin;
300 : :
301 [ # # ][ # # ]: 0 : if ( (xlen >= ylen) && (xlen >= zlen) ) return XCUT;
302 [ # # ]: 0 : else if ( ylen >= zlen ) return YCUT;
303 : 0 : else return ZCUT;
304 : : }
305 : :
306 : :
307 : 0 : bool KDTree::rayintersectsbox(FSBoundingBox *box)
308 : : {
309 : : double pmin, pmax, tmin, tmax;
310 : : double dtemp;
311 : :
312 : : // Get the parametric distance along each direction from the min point to
313 : : // the min and max box planes. If the min dist is grater than the max
314 : : // dist, we have to swap them. Keep a running total of the max min and the
315 : : // min max. The ray intersects the box iff tmin <= tmax and tmax >= 0.0.
316 : : // Otherwise, the ray misses the box or points away from the box (with the
317 : : // starting point outside).
318 : :
319 : 0 : tmin = (box->xmin - rayxstart)/dx;
320 : 0 : tmax = (box->xmax - rayxstart)/dx;
321 [ # # ]: 0 : if ( tmin > tmax ) {
322 : 0 : dtemp = tmin; tmin = tmax; tmax = dtemp;
323 : : }
324 : 0 : pmin = (box->ymin - rayystart)/dy;
325 : 0 : pmax = (box->ymax - rayystart)/dy;
326 [ # # ]: 0 : if ( pmin > pmax ) {
327 : 0 : dtemp = pmin; pmin = pmax; pmax = dtemp;
328 : : }
329 : 0 : tmin = MAXX(pmin,tmin);
330 : 0 : tmax = MINN(pmax,tmax);
331 : :
332 [ # # ]: 0 : if ( tmin > tmax ) return false;
333 : :
334 : 0 : pmin = (box->zmin - rayzstart)/dz;
335 : 0 : pmax = (box->zmax - rayzstart)/dz;
336 [ # # ]: 0 : if ( pmin > pmax ) {
337 : 0 : dtemp = pmin; pmin = pmax; pmax = dtemp;
338 : : }
339 : 0 : tmin = MAXX(pmin,tmin);
340 : 0 : tmax = MINN(pmax,tmax);
341 : :
342 [ # # ][ # # ]: 0 : if ( (tmax < 0.0) || (tmin > tmax) ) return false;
343 : :
344 : 0 : return true;
345 : : }
346 : :
|
