STAT/TKDTree.cxx

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