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