]>
Commit | Line | Data |
---|---|---|
f2040a8f | 1 | #include "TKDTree.h" |
2 | ||
3 | #include "TString.h" | |
4 | #include <string.h> | |
5 | ||
6 | #ifndef R__ALPHA | |
7 | templateClassImp(TKDTree) | |
8 | #endif | |
9 | ////////////////////////////////////////////////////////////////////// | |
10 | // Memory setup of protected data members: | |
11 | // | |
12 | // kDataOwner; // Toggle ownership of the data | |
13 | // fNnodes: | |
14 | // size of branch node array, and index of first terminal node | |
15 | // fNDim; // number of variables | |
16 | // fNpoints; // number of multidimensional points | |
17 | // fBucketSize; // number of data points per terminal node | |
18 | // | |
19 | // fIndPoints : array containing rearanged indexes according to nodes: | |
20 | // | point indexes from 1st terminal node (fBucketSize elements) | ... | point indexes from the last terminal node (<= fBucketSize elements) | |
21 | // | |
22 | // Value **fData; | |
23 | // fRange : array containing the boundaries of the domain: | |
24 | // | 1st dimension (min + max) | 2nd dimension (min + max) | ... | |
25 | // fBoundaries : nodes boundaries | |
26 | // | 1st node {1st dim * 2 elements | 2nd dim * 2 elements | ...} | 2nd node {...} | ... | |
27 | // the nodes are arranged in the following order : | |
28 | // - first fNnodes are primary nodes | |
29 | // - after are the terminal nodes | |
30 | // fNodes : array of primary nodes | |
31 | /////////////////////////////////////////////////////////////////////// | |
117c62ab | 32 | |
33 | #include "malloc.h" | |
34 | int memory() | |
35 | { | |
36 | static struct mallinfo debug; | |
37 | debug = mallinfo(); | |
38 | //printf("arena[%d] usmblks[%d] uordblks[%d] fordblks[%d]\n", debug.arena, debug.usmblks, debug.uordblks, debug.fordblks); | |
39 | return debug.uordblks; | |
40 | } | |
41 | ||
f2040a8f | 42 | //_________________________________________________________________ |
43 | template <typename Index, typename Value> | |
44 | TKDTree<Index, Value>::TKDTree() : | |
45 | TObject() | |
46 | ,kDataOwner(kFALSE) | |
47 | ,fNnodes(0) | |
48 | ,fNDim(0) | |
316a7f5a | 49 | ,fNDimm(0) |
f2040a8f | 50 | ,fNpoints(0) |
51 | ,fBucketSize(0) | |
117c62ab | 52 | ,fAxis(0x0) |
53 | ,fValue(0x0) | |
f2040a8f | 54 | ,fRange(0x0) |
316a7f5a | 55 | ,fData(0x0) |
f2040a8f | 56 | ,fBoundaries(0x0) |
316a7f5a | 57 | ,fkNNdim(0) |
f2040a8f | 58 | ,fkNN(0x0) |
316a7f5a | 59 | ,fkNNdist(0x0) |
60 | ,fDistBuffer(0x0) | |
61 | ,fIndBuffer(0x0) | |
a9c20b1f | 62 | ,fIndPoints(0x0) |
63 | ,fRowT0(0) | |
64 | ,fCrossNode(0) | |
65 | ,fOffset(0) | |
f2040a8f | 66 | { |
67 | // Default constructor. Nothing is built | |
68 | } | |
69 | ||
70 | ||
71 | //_________________________________________________________________ | |
72 | template <typename Index, typename Value> | |
73 | TKDTree<Index, Value>::TKDTree(Index npoints, Index ndim, UInt_t bsize, Value **data) : | |
74 | TObject() | |
75 | ,kDataOwner(kFALSE) | |
76 | ,fNnodes(0) | |
77 | ,fNDim(ndim) | |
316a7f5a | 78 | ,fNDimm(2*ndim) |
f2040a8f | 79 | ,fNpoints(npoints) |
80 | ,fBucketSize(bsize) | |
117c62ab | 81 | ,fAxis(0x0) |
82 | ,fValue(0x0) | |
f2040a8f | 83 | ,fRange(0x0) |
316a7f5a | 84 | ,fData(data) //Columnwise!!!!! |
f2040a8f | 85 | ,fBoundaries(0x0) |
316a7f5a | 86 | ,fkNNdim(0) |
f2040a8f | 87 | ,fkNN(0x0) |
316a7f5a | 88 | ,fkNNdist(0x0) |
89 | ,fDistBuffer(0x0) | |
90 | ,fIndBuffer(0x0) | |
a9c20b1f | 91 | ,fIndPoints(0x0) |
92 | ,fRowT0(0) | |
93 | ,fCrossNode(0) | |
94 | ,fOffset(0) | |
f2040a8f | 95 | { |
316a7f5a | 96 | // Allocate data by hand. See TKDTree(TTree*, const Char_t*) for an automatic method. |
97 | ||
117c62ab | 98 | //printf("start building kdTree %d\n", memory()); |
f2040a8f | 99 | Build(); |
117c62ab | 100 | //printf("finish building kdTree %d\n", memory()); |
f2040a8f | 101 | } |
102 | ||
f2040a8f | 103 | //_________________________________________________________________ |
104 | template <typename Index, typename Value> | |
105 | TKDTree<Index, Value>::~TKDTree() | |
106 | { | |
316a7f5a | 107 | if (fIndBuffer) delete [] fIndBuffer; |
108 | if (fDistBuffer) delete [] fDistBuffer; | |
109 | if (fkNNdist) delete [] fkNNdist; | |
f2040a8f | 110 | if (fkNN) delete [] fkNN; |
117c62ab | 111 | if (fAxis) delete [] fAxis; |
112 | if (fValue) delete [] fValue; | |
f2040a8f | 113 | if (fIndPoints) delete [] fIndPoints; |
114 | if (fRange) delete [] fRange; | |
115 | if (fBoundaries) delete [] fBoundaries; | |
116 | if (kDataOwner && fData){ | |
117 | for(int idim=0; idim<fNDim; idim++) delete [] fData[idim]; | |
118 | delete [] fData; | |
119 | } | |
120 | } | |
121 | ||
122 | ||
123 | //_________________________________________________________________ | |
124 | template <typename Index, typename Value> | |
125 | void TKDTree<Index, Value>::Build(){ | |
126 | // | |
127 | // Build binning | |
128 | // | |
129 | // 1. calculate number of nodes | |
130 | // 2. calculate first terminal row | |
131 | // 3. initialize index array | |
132 | // 4. non recursive building of the binary tree | |
133 | // | |
134 | //1. | |
135 | fNnodes = fNpoints/fBucketSize-1; | |
136 | if (fNpoints%fBucketSize) fNnodes++; | |
137 | ||
138 | //2. | |
139 | fRowT0=0; | |
140 | for (;(fNnodes+1)>(1<<fRowT0);fRowT0++); | |
141 | fRowT0-=1; | |
142 | // 2 = 2**0 + 1 | |
143 | // 3 = 2**1 + 1 | |
144 | // 4 = 2**1 + 2 | |
145 | // 5 = 2**2 + 1 | |
146 | // 6 = 2**2 + 2 | |
147 | // 7 = 2**2 + 3 | |
148 | // 8 = 2**2 + 4 | |
149 | ||
150 | //3. | |
151 | // allocate space for boundaries | |
152 | fRange = new Value[2*fNDim]; | |
153 | fIndPoints= new Index[fNpoints]; | |
154 | for (Index i=0; i<fNpoints; i++) fIndPoints[i] = i; | |
117c62ab | 155 | fAxis = new UChar_t[fNnodes]; |
156 | fValue = new Value[fNnodes]; | |
f2040a8f | 157 | // |
158 | fCrossNode = (1<<(fRowT0+1))-1; | |
159 | if (fCrossNode<fNnodes) fCrossNode = 2*fCrossNode+1; | |
160 | // | |
161 | // fOffset = (((fNnodes+1)-(1<<fRowT0)))*2; | |
162 | Int_t over = (fNnodes+1)-(1<<fRowT0); | |
163 | Int_t filled = ((1<<fRowT0)-over)*fBucketSize; | |
164 | fOffset = fNpoints-filled; | |
165 | // | |
166 | // printf("Row0 %d\n", fRowT0); | |
167 | // printf("CrossNode %d\n", fCrossNode); | |
168 | // printf("Offset %d\n", fOffset); | |
169 | // | |
170 | // | |
171 | //4. | |
172 | // stack for non recursive build - size 128 bytes enough | |
173 | Int_t rowStack[128]; | |
174 | Int_t nodeStack[128]; | |
175 | Int_t npointStack[128]; | |
176 | Int_t posStack[128]; | |
177 | Int_t currentIndex = 0; | |
178 | Int_t iter =0; | |
179 | rowStack[0] = 0; | |
180 | nodeStack[0] = 0; | |
181 | npointStack[0] = fNpoints; | |
182 | posStack[0] = 0; | |
183 | // | |
184 | Int_t nbucketsall =0; | |
185 | while (currentIndex>=0){ | |
186 | iter++; | |
187 | // | |
188 | Int_t npoints = npointStack[currentIndex]; | |
189 | if (npoints<=fBucketSize) { | |
190 | //printf("terminal node : index %d iter %d\n", currentIndex, iter); | |
191 | currentIndex--; | |
192 | nbucketsall++; | |
193 | continue; // terminal node | |
194 | } | |
195 | Int_t crow = rowStack[currentIndex]; | |
196 | Int_t cpos = posStack[currentIndex]; | |
197 | Int_t cnode = nodeStack[currentIndex]; | |
f2040a8f | 198 | //printf("currentIndex %d npoints %d node %d\n", currentIndex, npoints, cnode); |
199 | // | |
200 | // divide points | |
201 | Int_t nbuckets0 = npoints/fBucketSize; //current number of buckets | |
202 | if (npoints%fBucketSize) nbuckets0++; // | |
203 | Int_t restRows = fRowT0-rowStack[currentIndex]; // rest of fully occupied node row | |
204 | if (restRows<0) restRows =0; | |
205 | for (;nbuckets0>(2<<restRows); restRows++); | |
206 | Int_t nfull = 1<<restRows; | |
207 | Int_t nrest = nbuckets0-nfull; | |
208 | Int_t nleft =0, nright =0; | |
209 | // | |
210 | if (nrest>(nfull/2)){ | |
211 | nleft = nfull*fBucketSize; | |
212 | nright = npoints-nleft; | |
213 | }else{ | |
214 | nright = nfull*fBucketSize/2; | |
215 | nleft = npoints-nright; | |
216 | } | |
217 | ||
218 | // | |
219 | //find the axis with biggest spread | |
220 | Value maxspread=0; | |
221 | Value tempspread, min, max; | |
222 | Index axspread=0; | |
223 | Value *array; | |
224 | for (Int_t idim=0; idim<fNDim; idim++){ | |
225 | array = fData[idim]; | |
226 | Spread(npoints, array, fIndPoints+cpos, min, max); | |
227 | tempspread = max - min; | |
228 | if (maxspread < tempspread) { | |
229 | maxspread=tempspread; | |
230 | axspread = idim; | |
231 | } | |
117c62ab | 232 | if(cnode) continue; |
233 | //printf("set %d %6.3f %6.3f\n", idim, min, max); | |
234 | fRange[2*idim] = min; fRange[2*idim+1] = max; | |
f2040a8f | 235 | } |
236 | array = fData[axspread]; | |
237 | KOrdStat(npoints, array, nleft, fIndPoints+cpos); | |
117c62ab | 238 | fAxis[cnode] = axspread; |
239 | fValue[cnode] = array[fIndPoints[cpos+nleft]]; | |
f2040a8f | 240 | //printf("Set node %d : ax %d val %f\n", cnode, node->fAxis, node->fValue); |
241 | // | |
242 | // | |
243 | npointStack[currentIndex] = nleft; | |
244 | rowStack[currentIndex] = crow+1; | |
245 | posStack[currentIndex] = cpos; | |
246 | nodeStack[currentIndex] = cnode*2+1; | |
247 | currentIndex++; | |
248 | npointStack[currentIndex] = nright; | |
249 | rowStack[currentIndex] = crow+1; | |
250 | posStack[currentIndex] = cpos+nleft; | |
251 | nodeStack[currentIndex] = (cnode*2)+2; | |
252 | // | |
253 | if (0){ | |
254 | // consistency check | |
255 | Info("Build()", Form("points %d left %d right %d", npoints, nleft, nright)); | |
256 | if (nleft<nright) Warning("Build", "Problem Left-Right"); | |
257 | if (nleft<0 || nright<0) Warning("Build()", "Problem Negative number"); | |
258 | } | |
259 | } | |
f2040a8f | 260 | |
261 | //printf("NBuckets\t%d\n", nbucketsall); | |
262 | //fData = 0; | |
263 | } | |
264 | ||
265 | ||
266 | //_________________________________________________________________ | |
267 | template <typename Index, typename Value> | |
117c62ab | 268 | Bool_t TKDTree<Index, Value>::FindNearestNeighbors(const Value *point, const Int_t k, Index *&in, Value *&d) |
f2040a8f | 269 | { |
270 | // Find "k" nearest neighbors to "point". | |
271 | // | |
272 | // Return true in case of success and false in case of failure. | |
273 | // The indexes of the nearest k points are stored in the array "in" in | |
274 | // increasing order of the distance to "point" and the maxim distance | |
275 | // in "d". | |
276 | // | |
117c62ab | 277 | // The arrays "in" and "d" are managed internally by the TKDTree. |
f2040a8f | 278 | |
279 | Index inode = FindNode(point); | |
280 | if(inode < fNnodes){ | |
281 | Error("FindNearestNeighbors()", "Point outside data range."); | |
282 | return kFALSE; | |
283 | } | |
284 | ||
316a7f5a | 285 | UInt_t debug = 0; |
286 | Int_t nCalculations = 0; | |
287 | ||
f2040a8f | 288 | // allocate working memory |
316a7f5a | 289 | if(!fDistBuffer){ |
290 | fDistBuffer = new Value[fBucketSize]; | |
291 | fIndBuffer = new Index[fBucketSize]; | |
292 | } | |
293 | if(fkNNdim < k){ | |
117c62ab | 294 | if(debug>=1){ |
316a7f5a | 295 | if(fkNN) printf("Reallocate memory %d -> %d \n", fkNNdim, 2*k); |
296 | else printf("Allocate %d memory\n", 2*k); | |
117c62ab | 297 | } |
316a7f5a | 298 | fkNNdim = 2*k; |
299 | if(fkNN){ | |
300 | delete [] fkNN; | |
301 | delete [] fkNNdist; | |
302 | } | |
303 | fkNN = new Index[fkNNdim]; | |
304 | fkNNdist = new Value[fkNNdim]; | |
305 | } | |
306 | memset(fkNN, -1, k*sizeof(Index)); | |
307 | for(int i=0; i<k; i++) fkNNdist[i] = 9999.; | |
308 | Index itmp, jtmp; Value ftmp, gtmp; | |
309 | ||
f2040a8f | 310 | // calculate number of boundaries with the data domain. |
f2040a8f | 311 | if(!fBoundaries) MakeBoundaries(); |
312 | Value *bounds = &fBoundaries[inode*2*fNDim]; | |
117c62ab | 313 | Index nBounds = 0; |
f2040a8f | 314 | for(int idim=0; idim<fNDim; idim++){ |
117c62ab | 315 | if((bounds[2*idim] - fRange[2*idim]) < 1.E-10) nBounds++; |
316 | if((bounds[2*idim+1] - fRange[2*idim+1]) < 1.E-10) nBounds++; | |
f2040a8f | 317 | } |
316a7f5a | 318 | if(debug>=1) printf("Calculate boundaries [nBounds = %d]\n", nBounds); |
117c62ab | 319 | |
320 | ||
f2040a8f | 321 | // traverse tree |
117c62ab | 322 | UChar_t ax; |
323 | Value val, dif; | |
324 | Int_t nAllNodes = fNnodes + GetNTNodes(); | |
f2040a8f | 325 | Int_t nodeStack[128], nodeIn[128]; |
326 | Index currentIndex = 0; | |
327 | nodeStack[0] = inode; | |
328 | nodeIn[0] = inode; | |
329 | while(currentIndex>=0){ | |
330 | Int_t tnode = nodeStack[currentIndex]; | |
331 | Int_t entry = nodeIn[currentIndex]; | |
332 | currentIndex--; | |
316a7f5a | 333 | if(debug>=1) printf("Analyse tnode %d entry %d\n", tnode, entry); |
334 | ||
f2040a8f | 335 | // check if node is still eligible |
117c62ab | 336 | Int_t inode = (tnode-1)>>1; //tnode/2 + (tnode%2) - 1; |
337 | ax = fAxis[inode]; | |
338 | val = fValue[inode]; | |
339 | dif = point[ax] - val; | |
340 | if((TMath::Abs(dif) > fkNNdist[k-1]) && | |
341 | ((dif > 0. && (tnode&1)) || (dif < 0. && !(tnode&1)))) { | |
342 | if(debug>=1) printf("\tremove %d\n", (tnode-1)>>1); | |
f2040a8f | 343 | |
344 | // mark bound | |
345 | nBounds++; | |
346 | // end all recursions | |
347 | if(nBounds==2 * fNDim) break; | |
348 | continue; | |
349 | } | |
350 | ||
351 | if(IsTerminal(tnode)){ | |
316a7f5a | 352 | if(debug>=2) printf("\tProcess terminal node %d\n", tnode); |
f2040a8f | 353 | // Link data to terminal node |
354 | Int_t offset = (tnode >= fCrossNode) ? (tnode-fCrossNode)*fBucketSize : fOffset+(tnode-fNnodes)*fBucketSize; | |
355 | Index *indexPoints = &fIndPoints[offset]; | |
316a7f5a | 356 | Int_t nbs = (tnode == 2*fNnodes) ? fNpoints%fBucketSize : fBucketSize; |
357 | nbs = nbs ? nbs : fBucketSize; | |
358 | nCalculations += nbs; | |
359 | ||
360 | Int_t npoints = 0; | |
361 | for(int idx=0; idx<nbs; idx++){ | |
f2040a8f | 362 | // calculate distance in the L1 metric |
316a7f5a | 363 | fDistBuffer[npoints] = 0.; |
364 | for(int idim=0; idim<fNDim; idim++) fDistBuffer[npoints] += TMath::Abs(point[idim] - fData[idim][indexPoints[idx]]); | |
365 | // register candidate neighbor | |
366 | if(fDistBuffer[npoints] < fkNNdist[k-1]){ | |
367 | fIndBuffer[npoints] = indexPoints[idx]; | |
368 | npoints++; | |
369 | } | |
f2040a8f | 370 | } |
316a7f5a | 371 | for(int ibrowse=0; ibrowse<npoints; ibrowse++){ |
372 | if(fDistBuffer[ibrowse] >= fkNNdist[k-1]) continue; | |
373 | //insert neighbor | |
374 | int iNN=0; | |
375 | while(fDistBuffer[ibrowse] >= fkNNdist[iNN]) iNN++; | |
376 | if(debug>=2) printf("\t\tinsert data %d @ %d distance %f\n", fIndBuffer[ibrowse], iNN, fDistBuffer[ibrowse]); | |
377 | ||
378 | itmp = fkNN[iNN]; ftmp = fkNNdist[iNN]; | |
379 | fkNN[iNN] = fIndBuffer[ibrowse]; | |
380 | fkNNdist[iNN] = fDistBuffer[ibrowse]; | |
381 | for(int ireplace=iNN+1; ireplace<k; ireplace++){ | |
382 | jtmp = fkNN[ireplace]; gtmp = fkNNdist[ireplace]; | |
383 | fkNN[ireplace] = itmp; fkNNdist[ireplace] = ftmp; | |
384 | itmp = jtmp; ftmp = gtmp; | |
385 | if(ftmp == 9999.) break; | |
f2040a8f | 386 | } |
316a7f5a | 387 | if(debug>=3){ |
388 | for(int i=0; i<k; i++){ | |
389 | if(fkNNdist[i] == 9999.) break; | |
390 | printf("\t\tfkNNdist[%d] = %f\n", i, fkNNdist[i]); | |
391 | } | |
392 | } | |
f2040a8f | 393 | } |
394 | } | |
395 | ||
396 | // register parent | |
117c62ab | 397 | Int_t pn = (tnode-1)>>1;//tnode/2 + (tnode%2) - 1; |
398 | if(pn >= 0 && entry != pn){ | |
f2040a8f | 399 | // check if parent node is eligible at all |
117c62ab | 400 | if(TMath::Abs(point[fAxis[pn]] - fValue[pn]) > fkNNdist[k-1]){ |
f2040a8f | 401 | // mark bound |
402 | nBounds++; | |
403 | // end all recursions | |
404 | if(nBounds==2 * fNDim) break; | |
405 | } | |
406 | currentIndex++; | |
117c62ab | 407 | nodeStack[currentIndex]=pn; |
f2040a8f | 408 | nodeIn[currentIndex]=tnode; |
316a7f5a | 409 | if(debug>=2) printf("\tregister %d\n", nodeStack[currentIndex]); |
f2040a8f | 410 | } |
117c62ab | 411 | if(IsTerminal(tnode)) continue; |
316a7f5a | 412 | |
f2040a8f | 413 | // register children nodes |
117c62ab | 414 | Int_t cn; |
f2040a8f | 415 | Bool_t kAllow[] = {kTRUE, kTRUE}; |
117c62ab | 416 | ax = fAxis[tnode]; |
417 | val = fValue[tnode]; | |
418 | if(TMath::Abs(point[ax] - val) > fkNNdist[k-1]){ | |
419 | if(point[ax] > val) kAllow[0] = kFALSE; | |
f2040a8f | 420 | else kAllow[1] = kFALSE; |
421 | } | |
422 | for(int ic=1; ic<=2; ic++){ | |
423 | if(!kAllow[ic-1]) continue; | |
117c62ab | 424 | cn = (tnode<<1)+ic; |
425 | if(cn < nAllNodes && entry != cn){ | |
f2040a8f | 426 | currentIndex++; |
117c62ab | 427 | nodeStack[currentIndex] = cn; |
f2040a8f | 428 | nodeIn[currentIndex]=tnode; |
316a7f5a | 429 | if(debug>=2) printf("\tregister %d\n", nodeStack[currentIndex]); |
f2040a8f | 430 | } |
316a7f5a | 431 | } |
f2040a8f | 432 | } |
433 | // save results | |
434 | in = fkNN; | |
316a7f5a | 435 | d = fkNNdist; |
f2040a8f | 436 | |
117c62ab | 437 | return kTRUE; |
f2040a8f | 438 | } |
439 | ||
440 | ||
441 | ||
442 | //_________________________________________________________________ | |
443 | template <typename Index, typename Value> | |
444 | Index TKDTree<Index, Value>::FindNode(const Value * point){ | |
445 | // | |
446 | // find the terminal node to which point belongs | |
117c62ab | 447 | |
f2040a8f | 448 | Index stackNode[128], inode; |
449 | Int_t currentIndex =0; | |
450 | stackNode[0] = 0; | |
f2040a8f | 451 | while (currentIndex>=0){ |
452 | inode = stackNode[currentIndex]; | |
f2040a8f | 453 | if (IsTerminal(inode)) return inode; |
454 | ||
117c62ab | 455 | currentIndex--; |
456 | if (point[fAxis[inode]]<=fValue[inode]){ | |
f2040a8f | 457 | currentIndex++; |
117c62ab | 458 | stackNode[currentIndex]=(inode<<1)+1; |
f2040a8f | 459 | } |
117c62ab | 460 | if (point[fAxis[inode]]>=fValue[inode]){ |
f2040a8f | 461 | currentIndex++; |
117c62ab | 462 | stackNode[currentIndex]=(inode+1)<<1; |
f2040a8f | 463 | } |
464 | } | |
a9c20b1f | 465 | |
466 | return -1; | |
f2040a8f | 467 | } |
468 | ||
469 | ||
a9c20b1f | 470 | |
f2040a8f | 471 | //_________________________________________________________________ |
472 | template <typename Index, typename Value> | |
473 | void TKDTree<Index, Value>::FindPoint(Value * point, Index &index, Int_t &iter){ | |
474 | // | |
475 | // find the index of point | |
476 | // works only if we keep fData pointers | |
477 | ||
478 | Int_t stackNode[128]; | |
479 | Int_t currentIndex =0; | |
480 | stackNode[0] = 0; | |
481 | iter =0; | |
482 | // | |
483 | while (currentIndex>=0){ | |
484 | iter++; | |
485 | Int_t inode = stackNode[currentIndex]; | |
486 | currentIndex--; | |
487 | if (IsTerminal(inode)){ | |
488 | // investigate terminal node | |
489 | Int_t indexIP = (inode >= fCrossNode) ? (inode-fCrossNode)*fBucketSize : (inode-fNnodes)*fBucketSize+fOffset; | |
490 | printf("terminal %d indexP %d\n", inode, indexIP); | |
491 | for (Int_t ibucket=0;ibucket<fBucketSize;ibucket++){ | |
492 | Bool_t isOK = kTRUE; | |
493 | indexIP+=ibucket; | |
494 | printf("ibucket %d index %d\n", ibucket, indexIP); | |
495 | if (indexIP>=fNpoints) continue; | |
496 | Int_t index0 = fIndPoints[indexIP]; | |
497 | for (Int_t idim=0;idim<fNDim;idim++) if (fData[idim][index0]!=point[idim]) isOK = kFALSE; | |
498 | if (isOK) index = index0; | |
499 | } | |
500 | continue; | |
501 | } | |
502 | ||
117c62ab | 503 | if (point[fAxis[inode]]<=fValue[inode]){ |
f2040a8f | 504 | currentIndex++; |
505 | stackNode[currentIndex]=(inode*2)+1; | |
506 | } | |
117c62ab | 507 | if (point[fAxis[inode]]>=fValue[inode]){ |
f2040a8f | 508 | currentIndex++; |
509 | stackNode[currentIndex]=(inode*2)+2; | |
510 | } | |
511 | } | |
512 | // | |
513 | // printf("Iter\t%d\n",iter); | |
514 | } | |
515 | ||
516 | //_________________________________________________________________ | |
517 | template <typename Index, typename Value> | |
518 | void TKDTree<Index, Value>::FindInRangeA(Value * point, Value * delta, Index *res , Index &npoints, Index & iter, Int_t bnode) | |
519 | { | |
520 | // | |
521 | // Find all points in the range specified by (point +- range) | |
522 | // res - Resulting indexes are stored in res array | |
523 | // npoints - Number of selected indexes in range | |
524 | // NOTICE: | |
525 | // For some cases it is better to don't keep data - because of memory consumption | |
526 | // If the data are not kept - only boundary conditions are investigated | |
527 | // some of the data can be outside of the range | |
528 | // What is guranteed in this mode: All of the points in the range are selected + some fraction of others (but close) | |
529 | ||
530 | Index stackNode[128]; | |
531 | iter=0; | |
532 | Index currentIndex = 0; | |
533 | stackNode[currentIndex] = bnode; | |
534 | while (currentIndex>=0){ | |
535 | iter++; | |
536 | Int_t inode = stackNode[currentIndex]; | |
537 | // | |
538 | currentIndex--; | |
539 | if (!IsTerminal(inode)){ | |
540 | // not terminal | |
117c62ab | 541 | //TKDNode<Index, Value> * node = &(fNodes[inode]); |
542 | if (point[fAxis[inode]] - delta[fAxis[inode]] < fValue[inode]) { | |
f2040a8f | 543 | currentIndex++; |
544 | stackNode[currentIndex]= (inode*2)+1; | |
545 | } | |
117c62ab | 546 | if (point[fAxis[inode]] + delta[fAxis[inode]] >= fValue[inode]){ |
f2040a8f | 547 | currentIndex++; |
548 | stackNode[currentIndex]= (inode*2)+2; | |
549 | } | |
550 | }else{ | |
551 | Int_t indexIP = | |
552 | (inode >= fCrossNode) ? (inode-fCrossNode)*fBucketSize : (inode-fNnodes)*fBucketSize+fOffset; | |
553 | for (Int_t ibucket=0;ibucket<fBucketSize;ibucket++){ | |
554 | if (indexIP+ibucket>=fNpoints) break; | |
555 | res[npoints] = fIndPoints[indexIP+ibucket]; | |
556 | npoints++; | |
557 | } | |
558 | } | |
559 | } | |
560 | if (fData){ | |
561 | // | |
562 | // compress rest if data still accesible | |
563 | // | |
564 | Index npoints2 = npoints; | |
565 | npoints=0; | |
566 | for (Index i=0; i<npoints2;i++){ | |
567 | Bool_t isOK = kTRUE; | |
568 | for (Index idim = 0; idim< fNDim; idim++){ | |
569 | if (TMath::Abs(fData[idim][res[i]]- point[idim])>delta[idim]) | |
570 | isOK = kFALSE; | |
571 | } | |
572 | if (isOK){ | |
573 | res[npoints] = res[i]; | |
574 | npoints++; | |
575 | } | |
576 | } | |
577 | } | |
578 | } | |
579 | ||
580 | ||
581 | //_________________________________________________________________ | |
582 | template <typename Index, typename Value> | |
583 | void TKDTree<Index, Value>::FindInRangeB(Value * point, Value * delta, Index *res , Index &npoints,Index & iter, Int_t bnode) | |
584 | { | |
585 | // | |
586 | Long64_t goldStatus = (1<<(2*fNDim))-1; // gold status | |
587 | Index stackNode[128]; | |
588 | Long64_t stackStatus[128]; | |
589 | iter=0; | |
590 | Index currentIndex = 0; | |
591 | stackNode[currentIndex] = bnode; | |
592 | stackStatus[currentIndex] = 0; | |
593 | while (currentIndex>=0){ | |
594 | Int_t inode = stackNode[currentIndex]; | |
595 | Long64_t status = stackStatus[currentIndex]; | |
596 | currentIndex--; | |
597 | iter++; | |
598 | if (IsTerminal(inode)){ | |
599 | Int_t indexIP = | |
600 | (inode >= fCrossNode) ? (inode-fCrossNode)*fBucketSize : (inode-fNnodes)*fBucketSize+fOffset; | |
601 | for (Int_t ibucket=0;ibucket<fBucketSize;ibucket++){ | |
602 | if (indexIP+ibucket>=fNpoints) break; | |
603 | res[npoints] = fIndPoints[indexIP+ibucket]; | |
604 | npoints++; | |
605 | } | |
606 | continue; | |
607 | } | |
608 | // not terminal | |
609 | if (status == goldStatus){ | |
610 | Int_t ileft = inode; | |
611 | Int_t iright = inode; | |
612 | for (;ileft<fNnodes; ileft = (ileft<<1)+1); | |
613 | for (;iright<fNnodes; iright = (iright<<1)+2); | |
614 | Int_t indexL = | |
615 | (ileft >= fCrossNode) ? (ileft-fCrossNode)*fBucketSize : (ileft-fNnodes)*fBucketSize+fOffset; | |
616 | Int_t indexR = | |
617 | (iright >= fCrossNode) ? (iright-fCrossNode)*fBucketSize : (iright-fNnodes)*fBucketSize+fOffset; | |
618 | if (indexL<=indexR){ | |
619 | Int_t endpoint = indexR+fBucketSize; | |
620 | if (endpoint>fNpoints) endpoint=fNpoints; | |
621 | for (Int_t ipoint=indexL;ipoint<endpoint;ipoint++){ | |
622 | res[npoints] = fIndPoints[ipoint]; | |
623 | npoints++; | |
624 | } | |
625 | continue; | |
626 | } | |
627 | } | |
628 | // | |
117c62ab | 629 | // TKDNode<Index, Value> * node = &(fNodes[inode]); |
630 | if (point[fAxis[inode]] - delta[fAxis[inode]] < fValue[inode]) { | |
f2040a8f | 631 | currentIndex++; |
632 | stackNode[currentIndex]= (inode<<1)+1; | |
117c62ab | 633 | if (point[fAxis[inode]] + delta[fAxis[inode]] > fValue[inode]) |
634 | stackStatus[currentIndex]= status | (1<<(2*fAxis[inode])); | |
f2040a8f | 635 | } |
117c62ab | 636 | if (point[fAxis[inode]] + delta[fAxis[inode]] >= fValue[inode]){ |
f2040a8f | 637 | currentIndex++; |
638 | stackNode[currentIndex]= (inode<<1)+2; | |
117c62ab | 639 | if (point[fAxis[inode]] - delta[fAxis[inode]]<fValue[inode]) |
640 | stackStatus[currentIndex]= status | (1<<(2*fAxis[inode]+1)); | |
f2040a8f | 641 | } |
642 | } | |
643 | if (fData){ | |
644 | // compress rest | |
645 | Index npoints2 = npoints; | |
646 | npoints=0; | |
647 | for (Index i=0; i<npoints2;i++){ | |
648 | Bool_t isOK = kTRUE; | |
649 | for (Index idim = 0; idim< fNDim; idim++){ | |
650 | if (TMath::Abs(fData[idim][res[i]]- point[idim])>delta[idim]) | |
651 | isOK = kFALSE; | |
652 | } | |
653 | if (isOK){ | |
654 | res[npoints] = res[i]; | |
655 | npoints++; | |
656 | } | |
657 | } | |
658 | } | |
659 | } | |
660 | ||
661 | ||
662 | ||
663 | //_________________________________________________________________ | |
664 | template <typename Index, typename Value> | |
665 | void TKDTree<Index, Value>::SetData(Index npoints, Index ndim, UInt_t bsize, Value **data) | |
666 | { | |
667 | // TO DO | |
668 | // | |
669 | // Check reconstruction/reallocation of memory of data. Maybe it is not | |
670 | // necessary to delete and realocate space but only to use the same space | |
671 | ||
672 | Clear(); | |
673 | ||
674 | //Columnwise!!!! | |
675 | fData = data; | |
676 | fNpoints = npoints; | |
677 | fNDim = ndim; | |
678 | fBucketSize = bsize; | |
679 | ||
680 | Build(); | |
681 | } | |
682 | ||
683 | ||
684 | ||
685 | //_________________________________________________________________ | |
686 | template <typename Index, typename Value> | |
687 | void TKDTree<Index, Value>::Spread(Index ntotal, Value *a, Index *index, Value &min, Value &max) | |
688 | { | |
689 | //Value min, max; | |
690 | Index i; | |
691 | min = a[index[0]]; | |
692 | max = a[index[0]]; | |
693 | for (i=0; i<ntotal; i++){ | |
694 | if (a[index[i]]<min) min = a[index[i]]; | |
695 | if (a[index[i]]>max) max = a[index[i]]; | |
696 | } | |
697 | } | |
698 | ||
699 | ||
700 | //_________________________________________________________________ | |
701 | template <typename Index, typename Value> | |
702 | Value TKDTree<Index, Value>::KOrdStat(Index ntotal, Value *a, Index k, Index *index) | |
703 | { | |
704 | // | |
705 | //copy of the TMath::KOrdStat because I need an Index work array | |
706 | ||
707 | Index i, ir, j, l, mid; | |
708 | Index arr; | |
709 | Index temp; | |
710 | ||
711 | Index rk = k; | |
712 | l=0; | |
713 | ir = ntotal-1; | |
714 | for(;;) { | |
715 | if (ir<=l+1) { //active partition contains 1 or 2 elements | |
716 | if (ir == l+1 && a[index[ir]]<a[index[l]]) | |
717 | {temp = index[l]; index[l]=index[ir]; index[ir]=temp;} | |
718 | Value tmp = a[index[rk]]; | |
719 | return tmp; | |
720 | } else { | |
721 | mid = (l+ir) >> 1; //choose median of left, center and right | |
722 | {temp = index[mid]; index[mid]=index[l+1]; index[l+1]=temp;}//elements as partitioning element arr. | |
723 | if (a[index[l]]>a[index[ir]]) //also rearrange so that a[l]<=a[l+1] | |
724 | {temp = index[l]; index[l]=index[ir]; index[ir]=temp;} | |
725 | ||
726 | if (a[index[l+1]]>a[index[ir]]) | |
727 | {temp=index[l+1]; index[l+1]=index[ir]; index[ir]=temp;} | |
728 | ||
729 | if (a[index[l]]>a[index[l+1]]) | |
730 | {temp = index[l]; index[l]=index[l+1]; index[l+1]=temp;} | |
731 | ||
732 | i=l+1; //initialize pointers for partitioning | |
733 | j=ir; | |
734 | arr = index[l+1]; | |
735 | for (;;) { | |
736 | do i++; while (a[index[i]]<a[arr]); | |
737 | do j--; while (a[index[j]]>a[arr]); | |
738 | if (j<i) break; //pointers crossed, partitioning complete | |
739 | {temp=index[i]; index[i]=index[j]; index[j]=temp;} | |
740 | } | |
741 | index[l+1]=index[j]; | |
742 | index[j]=arr; | |
743 | if (j>=rk) ir = j-1; //keep active the partition that | |
744 | if (j<=rk) l=i; //contains the k_th element | |
745 | } | |
746 | } | |
747 | } | |
748 | ||
749 | //_________________________________________________________________ | |
750 | template <typename Index, typename Value> | |
751 | void TKDTree<Index, Value>::MakeBoundaries(Value *range) | |
752 | { | |
753 | // Build boundaries for each node. | |
754 | ||
755 | ||
117c62ab | 756 | if(range) memcpy(fRange, range, fNDimm*sizeof(Value)); |
f2040a8f | 757 | // total number of nodes including terminal nodes |
117c62ab | 758 | Int_t totNodes = fNnodes + GetNTNodes(); |
759 | fBoundaries = new Value[totNodes*fNDimm]; | |
760 | //Info("MakeBoundaries(Value*)", Form("Allocate boundaries for %d nodes", totNodes)); | |
761 | ||
316a7f5a | 762 | |
f2040a8f | 763 | // loop |
117c62ab | 764 | Value *tbounds = 0x0, *cbounds = 0x0; |
765 | Int_t cn; | |
f2040a8f | 766 | for(int inode=fNnodes-1; inode>=0; inode--){ |
117c62ab | 767 | tbounds = &fBoundaries[inode*fNDimm]; |
768 | memcpy(tbounds, fRange, fNDimm*sizeof(Value)); | |
f2040a8f | 769 | |
770 | // check left child node | |
117c62ab | 771 | cn = (inode<<1)+1; |
772 | if(IsTerminal(cn)) CookBoundaries(inode, kTRUE); | |
773 | cbounds = &fBoundaries[fNDimm*cn]; | |
774 | for(int idim=0; idim<fNDim; idim++) tbounds[idim<<1] = cbounds[idim<<1]; | |
f2040a8f | 775 | |
776 | // check right child node | |
117c62ab | 777 | cn = (inode+1)<<1; |
778 | if(IsTerminal(cn)) CookBoundaries(inode, kFALSE); | |
779 | cbounds = &fBoundaries[fNDimm*cn]; | |
780 | for(int idim=0; idim<fNDim; idim++) tbounds[(idim<<1)+1] = cbounds[(idim<<1)+1]; | |
f2040a8f | 781 | } |
782 | } | |
783 | ||
784 | //_________________________________________________________________ | |
785 | template <typename Index, typename Value> | |
117c62ab | 786 | void TKDTree<Index, Value>::CookBoundaries(const Int_t node, Bool_t LEFT) |
f2040a8f | 787 | { |
788 | // define index of this terminal node | |
117c62ab | 789 | Int_t index = (node<<1) + (LEFT ? 1 : 2); |
790 | //Info("CookBoundaries()", Form("Node %d", index)); | |
f2040a8f | 791 | |
792 | // build and initialize boundaries for this node | |
117c62ab | 793 | Value *tbounds = &fBoundaries[fNDimm*index]; |
794 | memcpy(tbounds, fRange, fNDimm*sizeof(Value)); | |
f2040a8f | 795 | Bool_t flag[256]; // cope with up to 128 dimensions |
117c62ab | 796 | memset(flag, kFALSE, fNDimm); |
f2040a8f | 797 | Int_t nvals = 0; |
798 | ||
799 | // recurse parent nodes | |
117c62ab | 800 | Int_t pn = node; |
801 | while(pn >= 0 && nvals < fNDimm){ | |
f2040a8f | 802 | if(LEFT){ |
117c62ab | 803 | index = (fAxis[pn]<<1)+1; |
804 | if(!flag[index]) { | |
805 | tbounds[index] = fValue[pn]; | |
806 | flag[index] = kTRUE; | |
f2040a8f | 807 | nvals++; |
808 | } | |
809 | } else { | |
117c62ab | 810 | index = fAxis[pn]<<1; |
811 | if(!flag[index]) { | |
812 | tbounds[index] = fValue[pn]; | |
813 | flag[index] = kTRUE; | |
f2040a8f | 814 | nvals++; |
815 | } | |
816 | } | |
117c62ab | 817 | LEFT = pn&1; |
818 | pn = (pn - 1)>>1; | |
f2040a8f | 819 | } |
820 | } | |
821 | ||
f2040a8f | 822 | template class TKDTree<Int_t, Float_t>; |
823 | template class TKDTree<Int_t, Double_t>; | |
824 |