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