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