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