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