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 | /////////////////////////////////////////////////////////////////////// |
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 | |