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