Preliminary files for CMake
[u/mrichter/AliRoot.git] / STAT / TKDTree.cxx
1 #include "TKDTree.h"
2 #include "TRandom.h"
3
4 #include "TString.h"
5 #include <string.h>
6
7 #ifndef R__ALPHA
8 templateClassImp(TKDTree)
9 #endif
10
11 /*
12
13
14
15 Content:
16 1. What is kd-tree
17 2. How to cosntruct kdtree - Pseudo code
18 3. TKDTree implementation
19 4. User interface
20
21
22
23 1. What is kdtree ? ( http://en.wikipedia.org/wiki/Kd-tree )
24
25 In computer science, a kd-tree (short for k-dimensional tree) is a space-partitioning data structure for organizing points in a k-dimensional space. kd-trees are a useful data structure for several applications, such as searches involving a multidimensional search key (e.g. range searches and nearest neighbour searches). kd-trees are a special case of BSP trees.
26
27 A kd-tree uses only splitting planes that are perpendicular to one of the coordinate system axes. This differs from BSP trees, in which arbitrary splitting planes can be used. In addition, in the typical definition every node of a kd-tree, from the root to the leaves, stores a point.[1] This differs from BSP trees, in which leaves are typically the only nodes that contain points (or other geometric primitives). As a consequence, each splitting plane must go through one of the points in the kd-tree. kd-tries are a variant that store data only in leaf nodes. It is worth noting that in an alternative definition of kd-tree the points are stored in its leaf nodes only, although each splitting plane still goes through one of the points.[2]
28 <img src=".....">
29
30 2. Constructing a kd-tree ( Pseudo code)
31
32 Since there are many possible ways to choose axis-aligned splitting planes, there are many different ways to construct kd-trees. The canonical method of kd-tree construction has the following constraints:
33
34     * As one moves down the tree, one cycles through the axes used to select the splitting planes. (For example, the root would have an x-aligned plane, the root's children would both have y-aligned planes, the root's grandchildren would all have z-aligned planes, and so on.)
35     * At each step, the point selected to create the splitting plane is the median of the points being put into the kd-tree, with respect to their coordinates in the axis being used. (Note the assumption that we feed the entire set of points into the algorithm up-front.)
36
37 This method leads to a balanced kd-tree, in which each leaf node is about the same distance from the root. However, balanced trees are not necessarily optimal for all applications.
38
39 Note also that it is not required to select the median point. In that case, the result is simply that there is no guarantee that the tree will be balanced. A simple heuristic to avoid coding a complex linear-time median-finding algorithm nor using an O(n log n) sort is to use sort to find the median of a fixed number of randomly selected points to serve as the cut line. Practically this technique results in nicely balanced trees.
40
41 Given a list of n points, the following algorithm will construct a balanced kd-tree containing those points.
42 <img src=".....">
43
44 function kdtree (list of points pointList, int depth)
45 {
46     if pointList is empty
47         return nil;
48     else
49     {
50         // Select axis based on depth so that axis cycles through all valid values
51         var int axis := depth mod k;
52
53         // Sort point list and choose median as pivot element
54         select median from pointList;
55
56         // Create node and construct subtrees
57         var tree_node node;
58         node.location := median;
59         node.leftChild := kdtree(points in pointList before median, depth+1);
60         node.rightChild := kdtree(points in pointList after median, depth+1);
61         return node;
62     }
63 }
64
65
66 3. TKDtree implementation. 
67
68 // TKDtree is optimized to minimize memory consumption.
69 // TKDTree is by default balanced. Nodes are mapped to the 1 dimensional arrays of size
70 // fNnodes.
71 // 
72 // fAxix[fNnodes]  - Division axis (0,1,2,3 ...)
73 // fValue[fNnodes] - Division value 
74 //
75 // Navigation to the Left and right doughter node is expressed by  analytical formula
76 // Let's parent node id  is inode
77 // Then: 
78 // Left   = inode*2+1
79 // Right  =  (inode+1)*2  
80 // Let's  daughter node id is inode
81 // Then:
82 // Parent = inode/2
83 // 
84 // 
85 // The mapping of the nodes to the 1D array enables us to use the indexing mechanism.
86 // This is important if some additional kind of information 
87 // (not directly connected to kd-tree, e.g function fit parameters in the node)
88 // follow binary tree structure. The mapping can be reused.
89 // 
90 // Drawback:   Insertion to the TKDtree is not supported.  
91 // Advantage:  Random access is supported -  simple formulas  
92 // 
93 // Number of division nodes and number of terminals :
94 // fNnodes = (fNPoints/fBucketSize)
95 // 
96 // TKDTree is mapped to one dimensional array.
97 // Mapping: (see for example the TKDTree::FindNode) 
98 // 
99 // The nodes are filled always from left side to the right side:
100 //
101 //
102 // TERMINOLOGY: 
103 // - inode  -  index of node 
104 // - irow   -  index of row
105
106 // Ideal case:
107 // Number of terminal nodes = 2^N - N=3
108 //
109 //            INode
110 // irow 0     0                                                                   -  1 inode 
111 // irow 1     1                              2                                    -  2 inodes
112 // irow 2     3              4               5               6                    -  4 inodes
113 // irow 3     7       8      9      10       11     12       13      14           -  8 inodes
114 //
115 //
116 // Non ideal case:
117 // Number of terminal nodes = 2^N+k  - N=3  k=1
118
119 //           INode
120 // irow 0     0                                                                   - 1 inode
121 // irow 1     1                              2                                    - 2 inodes
122 // irow 2     3              4               5               6                    - 3 inodes
123 // irow 3     7       8      9      10       11     12       13      14           - 8 inodes
124 // irow 4     15  16                                                              - 2 inodes 
125 //
126 //
127 // The division algorithm:
128 // The  n points are divided to tho groups.
129 //
130 // n=2^k+rest
131 //
132 // Left  = 2^k-1 +(rest>2^k-2) ?  2^k-2      : rest
133 // Right = 2^k-1 +(rest>2^k-2) ?  rest-2^k-2 : 0
134 //
135 //
136  
137
138
139
140 */
141
142
143
144 //////////////////////////////////////////////////////////////////////
145 // Memory setup of protected data members:
146 // 
147 //      fDataOwner;  // Toggle ownership of the data
148 //      fNnodes:
149 //  size of branch node array, and index of first terminal node
150 //      fNDim;       // number of variables
151 //      fNpoints;    // number of multidimensional points
152 //      fBucketSize; // number of data points per terminal node
153 // 
154 //      fIndPoints : array containing rearanged indexes according to nodes:
155 //      | point indexes from 1st terminal node (fBucketSize elements) | ... | point indexes from the last terminal node (<= fBucketSize elements)
156 // 
157 //      Value   **fData;
158 //      fRange : array containing the boundaries of the domain:
159 //      | 1st dimension (min + max) | 2nd dimension (min + max) | ...
160 //      fBoundaries : nodes boundaries
161 //      | 1st node {1st dim * 2 elements | 2nd dim * 2 elements | ...} | 2nd node {...} | ...
162 //      the nodes are arranged in the following order :
163 //       - first fNnodes are primary nodes
164 //       - after are the terminal nodes
165 //      fNodes : array of primary nodes
166 ///////////////////////////////////////////////////////////////////////
167
168
169 //_________________________________________________________________
170 template <typename  Index, typename Value>
171 TKDTree<Index, Value>::TKDTree() :
172   TObject()
173   ,fDataOwner(kFALSE)
174   ,fNnodes(0)
175   ,fNDim(0)
176   ,fNDimm(0)
177   ,fNpoints(0)
178   ,fBucketSize(0)
179   ,fAxis(0x0)
180   ,fValue(0x0)
181   ,fRange(0x0)
182   ,fData(0x0)
183   ,fBoundaries(0x0)
184   ,fkNNdim(0)
185   ,fkNN(0x0)
186   ,fkNNdist(0x0)
187   ,fDistBuffer(0x0)
188   ,fIndBuffer(0x0)
189   ,fIndPoints(0x0)
190   ,fRowT0(0)
191   ,fCrossNode(0)
192   ,fOffset(0)
193 {
194 // Default constructor. Nothing is built
195 }
196
197
198 //_________________________________________________________________
199 template <typename  Index, typename Value>
200 TKDTree<Index, Value>::TKDTree(Index npoints, Index ndim, UInt_t bsize, Value **data) :
201   TObject()
202   ,fDataOwner(kFALSE)
203   ,fNnodes(0)
204   ,fNDim(ndim)
205   ,fNDimm(2*ndim)
206   ,fNpoints(npoints)
207   ,fBucketSize(bsize)
208   ,fAxis(0x0)
209   ,fValue(0x0)
210   ,fRange(0x0)
211   ,fData(data) //Columnwise!!!!!
212   ,fBoundaries(0x0)
213   ,fkNNdim(0)
214   ,fkNN(0x0)
215   ,fkNNdist(0x0)
216   ,fDistBuffer(0x0)
217   ,fIndBuffer(0x0)
218   ,fIndPoints(0x0)
219   ,fRowT0(0)
220   ,fCrossNode(0)
221   ,fOffset(0)
222 {
223   // Allocate data by hand. See TKDTree(TTree*, const Char_t*) for an automatic method.
224   
225   Build();
226 }
227
228 //_________________________________________________________________
229 template <typename  Index, typename Value>
230 TKDTree<Index, Value>::~TKDTree()
231 {
232   // Destructor
233   if (fIndBuffer) delete [] fIndBuffer;
234   if (fDistBuffer) delete [] fDistBuffer;
235   if (fkNNdist) delete [] fkNNdist;
236   if (fkNN) delete [] fkNN;
237   if (fAxis) delete [] fAxis;
238   if (fValue) delete [] fValue;
239   if (fIndPoints) delete [] fIndPoints;
240   if (fRange) delete [] fRange;
241   if (fBoundaries) delete [] fBoundaries;
242   if (fDataOwner && fData){
243     for(int idim=0; idim<fNDim; idim++) delete [] fData[idim];
244     delete [] fData;
245   }
246 }
247
248
249 //_________________________________________________________________
250 template <typename  Index, typename Value>
251 void TKDTree<Index, Value>::Build(){
252   //
253   // Build binning
254   //
255   // 1. calculate number of nodes
256   // 2. calculate first terminal row
257   // 3. initialize index array
258   // 4. non recursive building of the binary tree
259
260   //
261   // Teh tree is recursivaly divided. The division point is choosen in such a way, that the left side 
262   // alway has 2^k points, while at the same time trying 
263   //
264   //1.
265   fNnodes = fNpoints/fBucketSize-1;
266   if (fNpoints%fBucketSize) fNnodes++;
267   
268   //2.
269   fRowT0=0;
270   for (;(fNnodes+1)>(1<<fRowT0);fRowT0++);
271   fRowT0-=1;
272   //         2 = 2**0 + 1
273   //         3 = 2**1 + 1
274   //         4 = 2**1 + 2
275   //         5 = 2**2 + 1
276   //         6 = 2**2 + 2
277   //         7 = 2**2 + 3
278   //         8 = 2**2 + 4
279   
280   //3.
281   // allocate space for boundaries
282   fRange = new Value[2*fNDim];
283   fIndPoints= new Index[fNpoints];
284   for (Index i=0; i<fNpoints; i++) fIndPoints[i] = i;
285   fAxis  = new UChar_t[fNnodes];
286   fValue = new Value[fNnodes];
287   //
288   fCrossNode = (1<<(fRowT0+1))-1;
289   if (fCrossNode<fNnodes) fCrossNode = 2*fCrossNode+1;
290   //
291   //  fOffset = (((fNnodes+1)-(1<<fRowT0)))*2;
292   Int_t   over   = (fNnodes+1)-(1<<fRowT0);
293   Int_t   filled = ((1<<fRowT0)-over)*fBucketSize;
294   fOffset = fNpoints-filled;
295   //
296   //    printf("Row0      %d\n", fRowT0);
297   //    printf("CrossNode %d\n", fCrossNode);
298   //    printf("Offset    %d\n", fOffset);
299   //
300   //
301   //4.
302   //    stack for non recursive build - size 128 bytes enough
303   Int_t rowStack[128];
304   Int_t nodeStack[128];
305   Int_t npointStack[128];
306   Int_t posStack[128];
307   Int_t currentIndex = 0;
308   Int_t iter =0;
309   rowStack[0]    = 0;
310   nodeStack[0]   = 0;
311   npointStack[0] = fNpoints;
312   posStack[0]   = 0;
313   //
314   Int_t nbucketsall =0;
315   while (currentIndex>=0){
316     iter++;
317     //
318     Int_t npoints  = npointStack[currentIndex];
319     if (npoints<=fBucketSize) {
320       //printf("terminal node : index %d iter %d\n", currentIndex, iter);
321       currentIndex--;
322       nbucketsall++;
323       continue; // terminal node
324     }
325     Int_t crow     = rowStack[currentIndex];
326     Int_t cpos     = posStack[currentIndex];
327     Int_t cnode    = nodeStack[currentIndex];           
328     //printf("currentIndex %d npoints %d node %d\n", currentIndex, npoints, cnode);
329     //
330     // divide points
331     Int_t nbuckets0 = npoints/fBucketSize;           //current number of  buckets
332     if (npoints%fBucketSize) nbuckets0++;            //
333     Int_t restRows = fRowT0-rowStack[currentIndex];  // rest of fully occupied node row
334     if (restRows<0) restRows =0;
335     for (;nbuckets0>(2<<restRows); restRows++);
336     Int_t nfull = 1<<restRows;
337     Int_t nrest = nbuckets0-nfull;
338     Int_t nleft =0, nright =0;
339     //
340     if (nrest>(nfull/2)){
341       nleft  = nfull*fBucketSize;
342       nright = npoints-nleft;
343     }else{
344       nright = nfull*fBucketSize/2;
345       nleft  = npoints-nright;
346     }
347     
348     //
349     //find the axis with biggest spread
350     Value maxspread=0;
351     Value tempspread, min, max;
352     Index axspread=0;
353     Value *array;
354     for (Int_t idim=0; idim<fNDim; idim++){
355       array = fData[idim];
356       Spread(npoints, array, fIndPoints+cpos, min, max);
357       tempspread = max - min;
358       if (maxspread < tempspread) {
359         maxspread=tempspread;
360         axspread = idim;
361       }
362       if(cnode) continue;
363       //printf("set %d %6.3f %6.3f\n", idim, min, max);
364       fRange[2*idim] = min; fRange[2*idim+1] = max;
365     }
366     array = fData[axspread];
367     KOrdStat(npoints, array, nleft, fIndPoints+cpos);
368     fAxis[cnode]  = axspread;
369     fValue[cnode] = array[fIndPoints[cpos+nleft]];
370     //printf("Set node %d : ax %d val %f\n", cnode, node->fAxis, node->fValue);
371     //
372     //
373     npointStack[currentIndex] = nleft;
374     rowStack[currentIndex]    = crow+1;
375     posStack[currentIndex]    = cpos;
376     nodeStack[currentIndex]   = cnode*2+1;
377     currentIndex++;
378     npointStack[currentIndex] = nright;
379     rowStack[currentIndex]    = crow+1;
380     posStack[currentIndex]    = cpos+nleft;
381     nodeStack[currentIndex]   = (cnode*2)+2;
382     //
383     if (0){
384       // consistency check
385       Info("Build()", Form("points %d left %d right %d", npoints, nleft, nright));
386       if (nleft<nright) Warning("Build", "Problem Left-Right");
387       if (nleft<0 || nright<0) Warning("Build()", "Problem Negative number");
388     }
389   }    
390 }
391
392
393 //_________________________________________________________________
394 template <typename  Index, typename Value>
395 Bool_t  TKDTree<Index, Value>::FindNearestNeighbors(const Value *point, const Int_t k, Index *&in, Value *&d)
396 {
397   //
398   // NOT MI - to be checked
399   //
400   //
401   // Find "k" nearest neighbors to "point".
402   //
403   // Return true in case of success and false in case of failure.
404   // The indexes of the nearest k points are stored in the array "in" in
405   // increasing order of the distance to "point" and the maxim distance
406   // in "d".
407   //
408   // The arrays "in" and "d" are managed internally by the TKDTree.
409   
410   Index inode = FindNode(point);
411   if(inode < fNnodes){
412     Error("FindNearestNeighbors()", "Point outside data range.");
413     return kFALSE;
414   }
415   
416   UInt_t debug = 0;
417   Int_t nCalculations = 0;
418   
419   // allocate working memory
420   if(!fDistBuffer){
421     fDistBuffer = new Value[fBucketSize];
422     fIndBuffer  = new Index[fBucketSize];
423   }
424   if(fkNNdim < k){
425     if(debug>=1){
426       if(fkNN) printf("Reallocate memory %d -> %d \n", fkNNdim, 2*k);
427       else printf("Allocate %d memory\n", 2*k);
428     }
429     fkNNdim  = 2*k;
430     if(fkNN){
431       delete [] fkNN; 
432       delete [] fkNNdist;
433     }
434     fkNN     = new Index[fkNNdim];
435     fkNNdist = new Value[fkNNdim];
436   }
437   memset(fkNN, -1, k*sizeof(Index));
438   for(int i=0; i<k; i++) fkNNdist[i] = 9999.;
439   Index itmp, jtmp; Value ftmp, gtmp;
440   
441   // calculate number of boundaries with the data domain.       
442   if(!fBoundaries) MakeBoundaries();
443   Value *bounds = &fBoundaries[inode*2*fNDim];
444   Index nBounds = 0;
445   for(int idim=0; idim<fNDim; idim++){
446     if((bounds[2*idim] - fRange[2*idim]) < 1.E-10) nBounds++;
447     if((bounds[2*idim+1] - fRange[2*idim+1]) < 1.E-10) nBounds++;
448   }
449   if(debug>=1) printf("Calculate boundaries [nBounds = %d]\n", nBounds);
450   
451   
452   // traverse tree
453   UChar_t ax;
454   Value   val, dif;
455   Int_t nAllNodes = fNnodes + fNpoints/fBucketSize + ((fNpoints%fBucketSize)?1:0);
456   Int_t nodeStack[128], nodeIn[128];
457   Index currentIndex = 0;
458   nodeStack[0]   = inode;
459   nodeIn[0] = inode;
460   while(currentIndex>=0){
461     Int_t tnode = nodeStack[currentIndex];
462     Int_t entry = nodeIn[currentIndex];
463     currentIndex--;
464     if(debug>=1) printf("Analyse tnode %d entry %d\n", tnode, entry);
465     
466     // check if node is still eligible
467     Int_t inode = (tnode-1)>>1; //tnode/2 + (tnode%2) - 1;
468     ax = fAxis[inode];
469     val = fValue[inode];
470     dif = point[ax] - val;
471     if((TMath::Abs(dif) > fkNNdist[k-1]) &&
472        ((dif > 0. && (tnode&1)) || (dif < 0. && !(tnode&1)))) {
473       if(debug>=1) printf("\tremove %d\n", (tnode-1)>>1);
474       
475       // mark bound
476       nBounds++;
477       // end all recursions
478       if(nBounds==2 * fNDim) break;
479       continue;
480     }
481     
482     if(IsTerminal(tnode)){
483       if(debug>=2) printf("\tProcess terminal node %d\n", tnode);
484       // Link data to terminal node
485       Int_t offset = (tnode >= fCrossNode) ? (tnode-fCrossNode)*fBucketSize : fOffset+(tnode-fNnodes)*fBucketSize;
486       Index *indexPoints = &fIndPoints[offset];
487       Int_t nbs = (tnode == 2*fNnodes) ? fNpoints%fBucketSize : fBucketSize;
488       nbs = nbs ? nbs : fBucketSize;
489       nCalculations += nbs;
490       
491       Int_t npoints = 0;
492       for(int idx=0; idx<nbs; idx++){
493         // calculate distance in the L1 metric
494         fDistBuffer[npoints] = 0.;
495         for(int idim=0; idim<fNDim; idim++) fDistBuffer[npoints] += TMath::Abs(point[idim] - fData[idim][indexPoints[idx]]);
496         // register candidate neighbor
497         if(fDistBuffer[npoints] < fkNNdist[k-1]){
498           fIndBuffer[npoints] = indexPoints[idx];
499           npoints++;
500         }
501       }
502       for(int ibrowse=0; ibrowse<npoints; ibrowse++){
503         if(fDistBuffer[ibrowse] >= fkNNdist[k-1]) continue;
504         //insert neighbor
505         int iNN=0;
506         while(fDistBuffer[ibrowse] >= fkNNdist[iNN]) iNN++;
507         if(debug>=2) printf("\t\tinsert data %d @ %d distance %f\n", fIndBuffer[ibrowse], iNN, fDistBuffer[ibrowse]);
508         
509         itmp = fkNN[iNN]; ftmp = fkNNdist[iNN];
510         fkNN[iNN]     = fIndBuffer[ibrowse];
511         fkNNdist[iNN] = fDistBuffer[ibrowse];
512         for(int ireplace=iNN+1; ireplace<k; ireplace++){
513           jtmp = fkNN[ireplace]; gtmp = fkNNdist[ireplace];
514           fkNN[ireplace]     = itmp; fkNNdist[ireplace] = ftmp;
515           itmp = jtmp; ftmp = gtmp;
516           if(ftmp == 9999.) break;
517         }
518         if(debug>=3){
519           for(int i=0; i<k; i++){
520             if(fkNNdist[i] == 9999.) break;
521             printf("\t\tfkNNdist[%d] = %f\n", i, fkNNdist[i]);
522           }
523         }                               
524       }
525     }
526     
527     // register parent
528     Int_t pn = (tnode-1)>>1;//tnode/2 + (tnode%2) - 1;
529     if(pn >= 0 && entry != pn){
530       // check if parent node is eligible at all
531       if(TMath::Abs(point[fAxis[pn]] - fValue[pn]) > fkNNdist[k-1]){
532         // mark bound
533         nBounds++;
534         // end all recursions
535         if(nBounds==2 * fNDim) break;
536       }
537       currentIndex++;
538       nodeStack[currentIndex]=pn;
539       nodeIn[currentIndex]=tnode;
540       if(debug>=2) printf("\tregister %d\n", nodeStack[currentIndex]);
541     }
542     if(IsTerminal(tnode)) continue;
543     
544     // register children nodes
545     Int_t cn;
546     Bool_t kAllow[] = {kTRUE, kTRUE};
547     ax = fAxis[tnode];
548     val = fValue[tnode];
549     if(TMath::Abs(point[ax] - val) > fkNNdist[k-1]){
550       if(point[ax] > val) kAllow[0] = kFALSE;
551       else kAllow[1] = kFALSE;
552     }
553     for(int ic=1; ic<=2; ic++){
554       if(!kAllow[ic-1]) continue;
555       cn = (tnode<<1)+ic;
556       if(cn < nAllNodes && entry != cn){
557         currentIndex++;
558         nodeStack[currentIndex] = cn;
559         nodeIn[currentIndex]=tnode;
560         if(debug>=2) printf("\tregister %d\n", nodeStack[currentIndex]);
561       }
562     }           
563   }
564   // save results
565   in = fkNN;
566   d  = fkNNdist;  
567   return kTRUE;
568 }
569
570
571
572 //_________________________________________________________________
573 template <typename  Index, typename Value>
574 Index TKDTree<Index, Value>::FindNode(const Value * point){
575   //
576   // find the terminal node to which point belongs
577
578   Index stackNode[128], inode;
579   Int_t currentIndex =0;
580   stackNode[0] = 0;
581   while (currentIndex>=0){
582     inode    = stackNode[currentIndex];
583     if (IsTerminal(inode)) return inode;
584     
585     currentIndex--;
586     if (point[fAxis[inode]]<=fValue[inode]){
587       currentIndex++;
588       stackNode[currentIndex]=(inode<<1)+1;
589     }
590     if (point[fAxis[inode]]>=fValue[inode]){
591       currentIndex++;
592       stackNode[currentIndex]=(inode+1)<<1;
593     }
594   }
595   
596   return -1;
597 }
598
599
600
601 //_________________________________________________________________
602 template <typename  Index, typename Value>
603 void TKDTree<Index, Value>::FindPoint(Value * point, Index &index, Int_t &iter){
604   //
605   // find the index of point
606   // works only if we keep fData pointers
607   
608   Int_t stackNode[128];
609   Int_t currentIndex =0;
610   stackNode[0] = 0;
611   iter =0;
612   //
613   while (currentIndex>=0){
614     iter++;
615     Int_t inode    = stackNode[currentIndex];
616     currentIndex--;
617     if (IsTerminal(inode)){
618       // investigate terminal node
619       Int_t indexIP  = (inode >= fCrossNode) ? (inode-fCrossNode)*fBucketSize : (inode-fNnodes)*fBucketSize+fOffset;
620       printf("terminal %d indexP %d\n", inode, indexIP);
621       for (Int_t ibucket=0;ibucket<fBucketSize;ibucket++){
622         Bool_t isOK    = kTRUE;
623         indexIP+=ibucket;
624         printf("ibucket %d index %d\n", ibucket, indexIP);
625         if (indexIP>=fNpoints) continue;
626         Int_t index0   = fIndPoints[indexIP];
627         for (Int_t idim=0;idim<fNDim;idim++) if (fData[idim][index0]!=point[idim]) isOK = kFALSE;
628         if (isOK) index = index0;
629       }
630       continue;
631     }
632     
633     if (point[fAxis[inode]]<=fValue[inode]){
634       currentIndex++;
635       stackNode[currentIndex]=(inode*2)+1;
636     }
637     if (point[fAxis[inode]]>=fValue[inode]){
638       currentIndex++;
639       stackNode[currentIndex]=(inode*2)+2;
640     }
641   }
642   //
643   //  printf("Iter\t%d\n",iter);
644 }
645
646 //_________________________________________________________________
647 template <typename  Index, typename Value>
648 void TKDTree<Index, Value>::FindInRangeA(Value * point, Value * delta, Index *res , Index &npoints, Index & iter, Int_t bnode)
649 {
650  //
651   // Find all points in the range specified by    (point +- range)
652   // res     - Resulting indexes are stored in res array 
653   // npoints - Number of selected indexes in range
654   // NOTICE:
655   // For some cases it is better to don't keep data - because of memory consumption
656   // If the data are not kept - only boundary conditions are investigated
657   // some of the data can be outside of the range
658   // What is guranteed in this mode: All of the points in the range are selected + some fraction of others (but close)
659
660         Index stackNode[128];
661   iter=0;
662   Index currentIndex = 0;
663   stackNode[currentIndex] = bnode; 
664   while (currentIndex>=0){
665     iter++;
666     Int_t inode    = stackNode[currentIndex];
667     //
668     currentIndex--;
669     if (!IsTerminal(inode)){
670       // not terminal
671       //TKDNode<Index, Value> * node = &(fNodes[inode]);
672       if (point[fAxis[inode]] - delta[fAxis[inode]] <  fValue[inode]) {
673         currentIndex++; 
674         stackNode[currentIndex]= (inode*2)+1;
675       }
676       if (point[fAxis[inode]] + delta[fAxis[inode]] >= fValue[inode]){
677         currentIndex++; 
678         stackNode[currentIndex]= (inode*2)+2;
679       }
680     }else{
681       Int_t indexIP  = 
682         (inode >= fCrossNode) ? (inode-fCrossNode)*fBucketSize : (inode-fNnodes)*fBucketSize+fOffset;
683       for (Int_t ibucket=0;ibucket<fBucketSize;ibucket++){
684         if (indexIP+ibucket>=fNpoints) break;
685         res[npoints]   = fIndPoints[indexIP+ibucket];
686         npoints++;
687       }
688     }
689   }
690   if (fData){
691     //
692     //  compress rest if data still accesible
693     //
694     Index npoints2 = npoints;
695     npoints=0;
696     for (Index i=0; i<npoints2;i++){
697       Bool_t isOK = kTRUE;
698       for (Index idim = 0; idim< fNDim; idim++){
699         if (TMath::Abs(fData[idim][res[i]]- point[idim])>delta[idim]) 
700           isOK = kFALSE;        
701       }
702       if (isOK){
703         res[npoints] = res[i];
704         npoints++;
705       }
706     }    
707   }
708 }
709
710
711 //_________________________________________________________________
712 template <typename  Index, typename Value>
713 void TKDTree<Index, Value>::FindInRangeB(Value * point, Value * delta, Index *res , Index &npoints,Index & iter, Int_t bnode)
714 {
715   //
716   Long64_t  goldStatus = (1<<(2*fNDim))-1;  // gold status
717   Index stackNode[128];
718   Long64_t   stackStatus[128]; 
719   iter=0;
720   Index currentIndex   = 0;
721   stackNode[currentIndex]   = bnode; 
722   stackStatus[currentIndex] = 0;
723   while (currentIndex>=0){
724     Int_t inode     = stackNode[currentIndex];
725     Long64_t status = stackStatus[currentIndex];
726     currentIndex--;
727     iter++;
728     if (IsTerminal(inode)){
729       Int_t indexIP  = 
730         (inode >= fCrossNode) ? (inode-fCrossNode)*fBucketSize : (inode-fNnodes)*fBucketSize+fOffset;
731       for (Int_t ibucket=0;ibucket<fBucketSize;ibucket++){
732         if (indexIP+ibucket>=fNpoints) break;
733         res[npoints]   = fIndPoints[indexIP+ibucket];
734         npoints++;
735       }
736       continue;
737     }      
738     // not terminal    
739     if (status == goldStatus){
740       Int_t ileft  = inode;
741       Int_t iright = inode;
742       for (;ileft<fNnodes; ileft   = (ileft<<1)+1);
743       for (;iright<fNnodes; iright = (iright<<1)+2);
744       Int_t indexL  = 
745         (ileft >= fCrossNode) ? (ileft-fCrossNode)*fBucketSize : (ileft-fNnodes)*fBucketSize+fOffset;
746       Int_t indexR  = 
747         (iright >= fCrossNode) ? (iright-fCrossNode)*fBucketSize : (iright-fNnodes)*fBucketSize+fOffset;
748       if (indexL<=indexR){
749         Int_t endpoint = indexR+fBucketSize;
750         if (endpoint>fNpoints) endpoint=fNpoints;
751         for (Int_t ipoint=indexL;ipoint<endpoint;ipoint++){
752           res[npoints]   = fIndPoints[ipoint];
753           npoints++;
754         }         
755         continue;
756       }
757     }
758     //
759     // TKDNode<Index, Value> * node = &(fNodes[inode]);
760     if (point[fAxis[inode]] - delta[fAxis[inode]] <  fValue[inode]) {
761       currentIndex++; 
762       stackNode[currentIndex]= (inode<<1)+1;
763       if (point[fAxis[inode]] + delta[fAxis[inode]] > fValue[inode])
764         stackStatus[currentIndex]= status | (1<<(2*fAxis[inode]));
765     }
766     if (point[fAxis[inode]] + delta[fAxis[inode]] >= fValue[inode]){
767       currentIndex++; 
768       stackNode[currentIndex]= (inode<<1)+2;
769       if (point[fAxis[inode]] - delta[fAxis[inode]]<fValue[inode])
770         stackStatus[currentIndex]= status | (1<<(2*fAxis[inode]+1));
771     }
772   }
773   if (fData){
774     // compress rest
775     Index npoints2 = npoints;
776     npoints=0;
777     for (Index i=0; i<npoints2;i++){
778       Bool_t isOK = kTRUE;
779       for (Index idim = 0; idim< fNDim; idim++){
780         if (TMath::Abs(fData[idim][res[i]]- point[idim])>delta[idim]) 
781           isOK = kFALSE;        
782       }
783       if (isOK){
784         res[npoints] = res[i];
785         npoints++;
786       }
787     }    
788   }
789 }
790
791
792
793 //_________________________________________________________________
794 template <typename  Index, typename Value>
795 void TKDTree<Index, Value>::SetData(Index npoints, Index ndim, UInt_t bsize, Value **data)
796 {
797 // TO DO
798 // 
799 // Check reconstruction/reallocation of memory of data. Maybe it is not
800 // necessary to delete and realocate space but only to use the same space
801
802         Clear();
803   
804         //Columnwise!!!!
805    fData = data;
806    fNpoints = npoints;
807    fNDim = ndim;
808    fBucketSize = bsize;
809
810         Build();
811 }
812
813
814
815 //_________________________________________________________________
816 template <typename  Index, typename Value>
817 void TKDTree<Index, Value>::Spread(Index ntotal, Value *a, Index *index, Value &min, Value &max) const
818 {
819   //Value min, max;
820   Index i;
821   min = a[index[0]];
822   max = a[index[0]];
823   for (i=0; i<ntotal; i++){
824     if (a[index[i]]<min) min = a[index[i]];
825     if (a[index[i]]>max) max = a[index[i]];
826   }
827 }
828
829
830 //_________________________________________________________________
831 template <typename  Index, typename Value>
832 Value TKDTree<Index, Value>::KOrdStat(Index ntotal, Value *a, Index k, Index *index) const
833 {
834   //
835    //copy of the TMath::KOrdStat because I need an Index work array
836
837    Index i, ir, j, l, mid;
838    Index arr;
839    Index temp;
840
841    Index rk = k;
842    l=0;
843    ir = ntotal-1;
844   for(;;) {
845       if (ir<=l+1) { //active partition contains 1 or 2 elements
846          if (ir == l+1 && a[index[ir]]<a[index[l]])
847             {temp = index[l]; index[l]=index[ir]; index[ir]=temp;}
848          Value tmp = a[index[rk]];
849          return tmp;
850       } else {
851          mid = (l+ir) >> 1; //choose median of left, center and right
852          {temp = index[mid]; index[mid]=index[l+1]; index[l+1]=temp;}//elements as partitioning element arr.
853          if (a[index[l]]>a[index[ir]])  //also rearrange so that a[l]<=a[l+1]
854             {temp = index[l]; index[l]=index[ir]; index[ir]=temp;}
855
856          if (a[index[l+1]]>a[index[ir]])
857             {temp=index[l+1]; index[l+1]=index[ir]; index[ir]=temp;}
858
859          if (a[index[l]]>a[index[l+1]])
860             {temp = index[l]; index[l]=index[l+1]; index[l+1]=temp;}
861
862          i=l+1;        //initialize pointers for partitioning
863          j=ir;
864          arr = index[l+1];
865          for (;;) {
866             do i++; while (a[index[i]]<a[arr]);
867             do j--; while (a[index[j]]>a[arr]);
868             if (j<i) break;  //pointers crossed, partitioning complete
869                {temp=index[i]; index[i]=index[j]; index[j]=temp;}
870          }
871          index[l+1]=index[j];
872          index[j]=arr;
873          if (j>=rk) ir = j-1; //keep active the partition that
874          if (j<=rk) l=i;      //contains the k_th element
875       }
876    }
877 }
878
879 //_________________________________________________________________
880 template <typename Index, typename Value>
881 void TKDTree<Index, Value>::MakeBoundaries(Value *range)
882 {
883 // Build boundaries for each node.
884
885
886         if(range) memcpy(fRange, range, fNDimm*sizeof(Value));
887         // total number of nodes including terminal nodes
888         Int_t totNodes = fNnodes + fNpoints/fBucketSize + ((fNpoints%fBucketSize)?1:0);
889         fBoundaries = new Value[totNodes*fNDimm];
890         //Info("MakeBoundaries(Value*)", Form("Allocate boundaries for %d nodes", totNodes));
891
892         
893         // loop
894         Value *tbounds = 0x0, *cbounds = 0x0;
895         Int_t cn;
896         for(int inode=fNnodes-1; inode>=0; inode--){
897                 tbounds = &fBoundaries[inode*fNDimm];
898                 memcpy(tbounds, fRange, fNDimm*sizeof(Value));
899                                 
900                 // check left child node
901                 cn = (inode<<1)+1;
902                 if(IsTerminal(cn)) CookBoundaries(inode, kTRUE);
903                 cbounds = &fBoundaries[fNDimm*cn];
904                 for(int idim=0; idim<fNDim; idim++) tbounds[idim<<1] = cbounds[idim<<1];
905                 
906                 // check right child node
907                 cn = (inode+1)<<1;
908                 if(IsTerminal(cn)) CookBoundaries(inode, kFALSE);
909                 cbounds = &fBoundaries[fNDimm*cn];
910                 for(int idim=0; idim<fNDim; idim++) tbounds[(idim<<1)+1] = cbounds[(idim<<1)+1];
911         }
912 }
913
914 //_________________________________________________________________
915 template <typename Index, typename Value>
916 void TKDTree<Index, Value>::CookBoundaries(const Int_t node, Bool_t LEFT)
917 {
918         // define index of this terminal node
919         Int_t index = (node<<1) + (LEFT ? 1 : 2);
920         //Info("CookBoundaries()", Form("Node %d", index));
921         
922         // build and initialize boundaries for this node
923         Value *tbounds = &fBoundaries[fNDimm*index];
924         memcpy(tbounds, fRange, fNDimm*sizeof(Value));
925         Bool_t flag[256];  // cope with up to 128 dimensions 
926         memset(flag, kFALSE, fNDimm);
927         Int_t nvals = 0;
928                 
929         // recurse parent nodes
930         Int_t pn = node;
931         while(pn >= 0 && nvals < fNDimm){
932                 if(LEFT){
933                         index = (fAxis[pn]<<1)+1;
934                         if(!flag[index]) {
935                                 tbounds[index] = fValue[pn];
936                                 flag[index] = kTRUE;
937                                 nvals++;
938                         }
939                 } else {
940                         index = fAxis[pn]<<1;
941                         if(!flag[index]) {
942                                 tbounds[index] = fValue[pn];
943                                 flag[index] = kTRUE;
944                                 nvals++;
945                         }
946                 }
947                 LEFT = pn&1;
948                 pn =  (pn - 1)>>1;
949         }
950 }
951
952
953
954 //______________________________________________________________________
955 TKDTreeIF *TKDTreeTestBuild(const Int_t npoints, const Int_t bsize){  
956   //
957   // Example function to  
958   //
959   Float_t *data0 =  new Float_t[npoints*2];
960   Float_t *data[2];
961   data[0] = &data0[0];
962   data[1] = &data0[npoints];
963   for (Int_t i=0;i<npoints;i++) {
964     data[1][i]= gRandom->Rndm();
965     data[0][i]= gRandom->Rndm();
966   }
967   TKDTree<Int_t, Float_t> *kdtree = new TKDTreeIF(npoints, 2, bsize, data);
968   return kdtree;        
969 }
970
971
972
973 template class TKDTree<Int_t, Float_t>;
974 template class TKDTree<Int_t, Double_t>;
975