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