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