First version of kdtree (Alexander, Marian)
[u/mrichter/AliRoot.git] / STAT / TKDTree.cxx
CommitLineData
f2040a8f 1#include "TKDTree.h"
2
3#include "TString.h"
4#include <string.h>
5
6#ifndef R__ALPHA
7templateClassImp(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//_________________________________________________________________
33template <typename Index, typename Value>
34TKDTree<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//_________________________________________________________________
53template <typename Index, typename Value>
54TKDTree<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//_________________________________________________________________
75template <typename Index, typename Value>
76TKDTree<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//_________________________________________________________________
91template <typename Index, typename Value>
92void 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//_________________________________________________________________
233template <typename Index, typename Value>
234Bool_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//_________________________________________________________________
383template <typename Index, typename Value>
384Index 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//_________________________________________________________________
411template <typename Index, typename Value>
412void 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//_________________________________________________________________
457template <typename Index, typename Value>
458void 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//_________________________________________________________________
522template <typename Index, typename Value>
523void 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//_________________________________________________________________
604template <typename Index, typename Value>
605void 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//_________________________________________________________________
626template <typename Index, typename Value>
627void 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//_________________________________________________________________
641template <typename Index, typename Value>
642Value 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//_________________________________________________________________
690template <typename Index, typename Value>
691void 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//_________________________________________________________________
720template <typename Index, typename Value>
721void 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//_________________________________________________________________
756template <typename Index, typename Value>
757void 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
767template class TKDTree<Int_t, Float_t>;
768template class TKDTree<Int_t, Double_t>;
769