--- /dev/null
+#include "TKDInterpolator.h"
+
+#include "TLinearFitter.h"
+#include "TVector.h"
+#include "TTree.h"
+#include "TH2.h"
+#include "TObjArray.h"
+#include "TObjString.h"
+#include "TBox.h"
+#include "TGraph.h"
+#include "TMarker.h"
+
+
+
+ClassImp(TKDInterpolator)
+
+/////////////////////////////////////////////////////////////////////
+// Memory setup of protected data memebers
+// fRefPoints : evaluation point of PDF for each terminal node of underlying KD Tree.
+// | 1st terminal node (fNDim point coordinates) | 2nd terminal node (fNDim point coordinates) | ...
+//
+// fRefValues : evaluation value/error of PDF for each terminal node of underlying KD Tree.
+// | 1st terminal node (value) | 2nd terminal node (value) | ... | 1st terminal node (error) | 2nd terminal node (error) | ...
+/////////////////////////////////////////////////////////////////////
+
+//_________________________________________________________________
+TKDInterpolator::TKDInterpolator() : TKDTreeIF()
+ ,fNTNodes(0)
+ ,fRefPoints(0x0)
+ ,fRefValues(0x0)
+ ,fDepth(-1)
+ ,fTmpPoint(0x0)
+ ,fKDhelper(0x0)
+ ,fFitter(0x0)
+{
+}
+
+//_________________________________________________________________
+TKDInterpolator::TKDInterpolator(Int_t npoints, Int_t ndim, UInt_t bsize, Float_t **data) : TKDTreeIF(npoints, ndim, bsize, data)
+ ,fNTNodes(GetNTerminalNodes())
+ ,fRefPoints(0x0)
+ ,fRefValues(0x0)
+ ,fDepth(-1)
+ ,fTmpPoint(0x0)
+ ,fKDhelper(0x0)
+ ,fFitter(0x0)
+{
+ Build();
+}
+
+
+//_________________________________________________________________
+TKDInterpolator::TKDInterpolator(TTree *t, const Char_t *var, const Char_t *cut, UInt_t bsize) : TKDTreeIF()
+ ,fNTNodes(0)
+ ,fRefPoints(0x0)
+ ,fRefValues(0x0)
+ ,fDepth(-1)
+ ,fTmpPoint(0x0)
+ ,fKDhelper(0x0)
+ ,fFitter(0x0)
+{
+// Alocate data from a tree. The variables which have to be analysed are
+// defined in the "var" parameter as a colon separated list. The format should
+// be identical to that used by TTree::Draw().
+//
+//
+
+ fNpoints = t->GetEntriesFast();
+ TObjArray *vars = TString(var).Tokenize(":");
+ fNDim = vars->GetEntriesFast();
+ if(fNDim > 6/*kDimMax*/) Warning("TKDInterpolator(TTree*, UInt_t, const Char_t)", Form("Variable number exceed maximum dimension %d. Results are unpredictable.", 6/*kDimMax*/));
+ fBucketSize = bsize;
+
+ printf("Allocating %d points in %d dimensions.\n", fNpoints, fNDim);
+ Float_t *mem = new Float_t[fNDim*fNpoints];
+ fData = new Float_t*[fNDim];
+ for(int idim=0; idim<fNDim; idim++) fData[idim] = &mem[idim*fNpoints];
+ kDataOwner = kTRUE;
+
+ Double_t *v;
+ for(int idim=0; idim<fNDim; idim++){
+ if(!(t->Draw(((TObjString*)(*vars)[idim])->GetName(), cut, "goff"))){
+ Warning("TKDInterpolator(TTree*, UInt_t, const Char_t)", Form("Can not access data for %s", ((TObjString*)(*vars)[idim])->GetName() ));
+ continue;
+ }
+ v = t->GetV1();
+ for(int ip=0; ip<fNpoints; ip++) fData[idim][ip] = (Float_t)v[ip];
+ }
+ TKDTreeIF::Build();
+ fNTNodes = GetNTerminalNodes();
+ Build();
+}
+
+//_________________________________________________________________
+TKDInterpolator::~TKDInterpolator()
+{
+ if(fFitter) delete fFitter;
+ if(fKDhelper) delete fKDhelper;
+ if(fTmpPoint) delete [] fTmpPoint;
+
+ if(fRefPoints){
+ for(int idim=0; idim<fNDim; idim++) delete [] fRefPoints[idim] ;
+ delete [] fRefPoints;
+ }
+ if(fRefValues) delete [] fRefValues;
+}
+
+//_________________________________________________________________
+void TKDInterpolator::Build()
+{
+ if(!fBoundaries) MakeBoundaries();
+
+ // allocate memory for data
+ fRefValues = new Float_t[fNTNodes];
+ fRefPoints = new Float_t*[fNDim];
+ for(int id=0; id<fNDim; id++){
+ fRefPoints[id] = new Float_t[fNTNodes];
+ for(int in=0; in<fNTNodes; in++) fRefPoints[id][in] = 0.;
+ }
+
+ Float_t *bounds = 0x0;
+ Int_t *indexPoints;
+ for(int inode=0, tnode = fNnodes; inode<fNTNodes-1; inode++, tnode++){
+ fRefValues[inode] = Float_t(fBucketSize)/fNpoints;
+ bounds = GetBoundary(tnode);
+ for(int idim=0; idim<fNDim; idim++) fRefValues[inode] /= (bounds[2*idim+1] - bounds[2*idim]);
+
+ indexPoints = GetPointsIndexes(tnode);
+ // loop points in this terminal node
+ for(int idim=0; idim<fNDim; idim++){
+ for(int ip = 0; ip<fBucketSize; ip++) fRefPoints[idim][inode] += fData[idim][indexPoints[ip]];
+ fRefPoints[idim][inode] /= fBucketSize;
+ }
+ }
+
+ // analyze last (incomplete) terminal node
+ Int_t counts = fNpoints%fBucketSize;
+ counts = counts ? counts : fBucketSize;
+ Int_t inode = fNTNodes - 1, tnode = inode + fNnodes;
+ fRefValues[inode] = Float_t(counts)/fNpoints;
+ bounds = GetBoundary(tnode);
+ for(int idim=0; idim<fNDim; idim++) fRefValues[inode] /= (bounds[2*idim+1] - bounds[2*idim]);
+
+ indexPoints = GetPointsIndexes(tnode);
+ // loop points in this terminal node
+ for(int idim=0; idim<fNDim; idim++){
+ for(int ip = 0; ip<counts; ip++) fRefPoints[idim][inode] += fData[idim][indexPoints[ip]];
+ fRefPoints[idim][inode] /= counts;
+ }
+}
+
+//_________________________________________________________________
+Double_t TKDInterpolator::Eval(Float_t *point)
+{
+
+ // calculate number of parameters in the parabolic expresion
+ Int_t kNN = 1 + fNDim + fNDim*(fNDim+1)/2;
+
+ // prepare workers
+ if(!fTmpPoint) fTmpPoint = new Double_t[fNDim];
+ if(!fKDhelper) fKDhelper = new TKDTreeIF(GetNTerminalNodes(), fNDim, kNN, fRefPoints);
+ if(!fFitter){
+ // generate formula for nD
+ TString formula("1");
+ for(int idim=0; idim<fNDim; idim++){
+ formula += Form("++x[%d]", idim);
+ for(int jdim=idim; jdim<fNDim; jdim++) formula += Form("++x[%d]*x[%d]", idim, jdim);
+ }
+ fFitter = new TLinearFitter(kNN, formula.Data());
+ }
+
+ Int_t kNN_old = 0;
+ Int_t *index;
+ Float_t dist;
+ fFitter->ClearPoints();
+ do{
+ if(!fKDhelper->FindNearestNeighbors(point, kNN, index, dist)){
+ Error("Eval()", Form("Failed retriving %d neighbours for point:", kNN));
+ for(int idim=0; idim<fNDim; idim++) printf("%f ", point[idim]);
+ printf("\n");
+ return -1;
+ }
+ for(int in=kNN_old; in<kNN; in++){
+ for(int idim=0; idim<fNDim; idim++) fTmpPoint[idim] = fRefPoints[idim][index[in]];
+ fFitter->AddPoint(fTmpPoint, TMath::Log(fRefValues[index[in]]), 1.);
+ }
+ kNN_old = kNN;
+ kNN += 4;
+ } while(fFitter->Eval());
+
+ // calculate evaluation
+ TVectorD par(kNN);
+ fFitter->GetParameters(par);
+ Double_t result = par[0];
+ Int_t ipar = 0;
+ for(int idim=0; idim<fNDim; idim++){
+ result += par[++ipar]*point[idim];
+ for(int jdim=idim; jdim<fNDim; jdim++) result += par[++ipar]*point[idim]*point[jdim];
+ }
+ return TMath::Exp(result);
+}
+
+
+//_________________________________________________________________
+void TKDInterpolator::DrawNodes(Int_t depth, Int_t ax1, Int_t ax2)
+{
+// Draw nodes structure projected on plane "ax1:ax2". The parameter
+// "depth" specifies the bucket size per node. If depth == -1 draw only
+// terminal nodes and evaluation points (default -1 i.e. bucket size per node equal bucket size specified by the user)
+
+ if(!fBoundaries) MakeBoundaries();
+
+ // Count nodes in specific view
+ Int_t nnodes = 0;
+ for(int inode = 0; inode <= 2*fNnodes; inode++){
+ if(depth == -1){
+ if(!IsTerminal(inode)) continue;
+ } else if((inode+1) >> depth != 1) continue;
+ nnodes++;
+ }
+
+ //printf("depth %d nodes %d\n", depth, nnodes);
+
+ //TH2 *h2 = new TH2S("hframe", "", 100, fRange[2*ax1], fRange[2*ax1+1], 100, fRange[2*ax2], fRange[2*ax2+1]);
+ TH2 *h2 = new TH2S("hframe", "", 100, 0., 1., 100, 0., 1.);
+ h2->Draw();
+
+ const Float_t border = 0.;//1.E-4;
+ TBox **node_array = new TBox*[nnodes], *node;
+ Float_t *bounds = 0x0;
+ nnodes = 0;
+ for(int inode = 0; inode <= 2*fNnodes; inode++){
+ if(depth == -1){
+ if(!IsTerminal(inode)) continue;
+ } else if((inode+1) >> depth != 1) continue;
+
+ node = node_array[nnodes++];
+ bounds = GetBoundary(inode);
+ node = new TBox(bounds[2*ax1]+border, bounds[2*ax2]+border, bounds[2*ax1+1]-border, bounds[2*ax2+1]-border);
+ node->SetFillStyle(0);
+ node->SetFillColor(51+inode);
+ node->Draw();
+ }
+ if(depth != -1) return;
+
+ // Draw reference points
+ TGraph *ref = new TGraph(GetNTerminalNodes());
+ ref->SetMarkerStyle(2);
+ ref->SetMarkerColor(2);
+ Float_t val, error;
+ for(int inode = 0; inode < GetNTerminalNodes(); inode++) ref->SetPoint(inode, fRefPoints[ax1][inode], fRefPoints[ax2][inode]);
+ ref->Draw("p");
+ return;
+}
+
+//_________________________________________________________________
+void TKDInterpolator::DrawNode(Int_t tnode, Int_t ax1, Int_t ax2)
+{
+// Draw node "node" and the data points within.
+
+ if(tnode < 0 || tnode >= GetNTerminalNodes()){
+ Warning("DrawNode()", Form("Terminal node %d outside defined range.", tnode));
+ return;
+ }
+
+ //TH2 *h2 = new TH2S("hframe", "", 1, fRange[2*ax1], fRange[2*ax1+1], 1, fRange[2*ax2], fRange[2*ax2+1]);
+ TH2 *h2 = new TH2S("hframe", "", 1, 0., 1., 1, 0., 1.);
+ h2->Draw();
+
+ Int_t inode = tnode;
+ tnode += fNnodes;
+ // select zone of interest in the indexes array
+ Int_t *index = GetPointsIndexes(tnode);
+ Int_t nPoints = (tnode == 2*fNnodes) ? fNpoints%fBucketSize : fBucketSize;
+
+ printf("true index %d points %d\n", tnode, nPoints);
+
+ // draw data points
+ TGraph *g = new TGraph(nPoints);
+ g->SetMarkerStyle(3);
+ g->SetMarkerSize(.8);
+ for(int ip = 0; ip<nPoints; ip++) g->SetPoint(ip, fData[ax1][index[ip]], fData[ax2][index[ip]]);
+ g->Draw("p");
+
+ // draw estimation point
+ TMarker *m=new TMarker(fRefPoints[ax1][inode], fRefPoints[ax2][inode], 2);
+ m->SetMarkerColor(2);
+ m->Draw();
+
+ // draw node contour
+ Float_t *bounds = GetBoundary(tnode);
+ TBox *n = new TBox(bounds[2*ax1], bounds[2*ax2], bounds[2*ax1+1], bounds[2*ax2+1]);
+ n->SetFillStyle(0);
+ n->Draw();
+
+ return;
+}
+
--- /dev/null
+#include "TKDTree.h"
+
+#include "TString.h"
+#include <string.h>
+
+#ifndef R__ALPHA
+templateClassImp(TKDTree)
+#endif
+//////////////////////////////////////////////////////////////////////
+// Memory setup of protected data members:
+//
+// kDataOwner; // Toggle ownership of the data
+// fNnodes:
+// size of branch node array, and index of first terminal node
+// fNDim; // number of variables
+// fNpoints; // number of multidimensional points
+// fBucketSize; // number of data points per terminal node
+//
+// fIndPoints : array containing rearanged indexes according to nodes:
+// | point indexes from 1st terminal node (fBucketSize elements) | ... | point indexes from the last terminal node (<= fBucketSize elements)
+//
+// Value **fData;
+// fRange : array containing the boundaries of the domain:
+// | 1st dimension (min + max) | 2nd dimension (min + max) | ...
+// fBoundaries : nodes boundaries
+// | 1st node {1st dim * 2 elements | 2nd dim * 2 elements | ...} | 2nd node {...} | ...
+// the nodes are arranged in the following order :
+// - first fNnodes are primary nodes
+// - after are the terminal nodes
+// fNodes : array of primary nodes
+///////////////////////////////////////////////////////////////////////
+//_________________________________________________________________
+template <typename Index, typename Value>
+TKDTree<Index, Value>::TKDTree() :
+ TObject()
+ ,kDataOwner(kFALSE)
+ ,fNnodes(0)
+ ,fNDim(0)
+ ,fNpoints(0)
+ ,fBucketSize(0)
+ ,fIndPoints(0x0)
+ ,fData(0x0)
+ ,fRange(0x0)
+ ,fBoundaries(0x0)
+ ,fNodes(0x0)
+ ,fkNN(0x0)
+{
+// Default constructor. Nothing is built
+}
+
+
+//_________________________________________________________________
+template <typename Index, typename Value>
+TKDTree<Index, Value>::TKDTree(Index npoints, Index ndim, UInt_t bsize, Value **data) :
+ TObject()
+ ,kDataOwner(kFALSE)
+ ,fNnodes(0)
+ ,fNDim(ndim)
+ ,fNpoints(npoints)
+ ,fBucketSize(bsize)
+ ,fIndPoints(0x0)
+ ,fData(data) //Columnwise!!!!!
+ ,fRange(0x0)
+ ,fBoundaries(0x0)
+ ,fNodes(0x0)
+ ,fkNN(0x0)
+{
+ // Allocate data by hand. See TKDTree(TTree*, const Char_t*) for an automatic method.
+
+ Build();
+}
+
+
+//_________________________________________________________________
+template <typename Index, typename Value>
+TKDTree<Index, Value>::~TKDTree()
+{
+ if (fkNN) delete [] fkNN;
+ if (fNodes) delete [] fNodes;
+ if (fIndPoints) delete [] fIndPoints;
+ if (fRange) delete [] fRange;
+ if (fBoundaries) delete [] fBoundaries;
+ if (kDataOwner && fData){
+ for(int idim=0; idim<fNDim; idim++) delete [] fData[idim];
+ delete [] fData;
+ }
+}
+
+
+//_________________________________________________________________
+template <typename Index, typename Value>
+void TKDTree<Index, Value>::Build(){
+ //
+ // Build binning
+ //
+ // 1. calculate number of nodes
+ // 2. calculate first terminal row
+ // 3. initialize index array
+ // 4. non recursive building of the binary tree
+ //
+ //1.
+ fNnodes = fNpoints/fBucketSize-1;
+ if (fNpoints%fBucketSize) fNnodes++;
+
+ //2.
+ fRowT0=0;
+ for (;(fNnodes+1)>(1<<fRowT0);fRowT0++);
+ fRowT0-=1;
+ // 2 = 2**0 + 1
+ // 3 = 2**1 + 1
+ // 4 = 2**1 + 2
+ // 5 = 2**2 + 1
+ // 6 = 2**2 + 2
+ // 7 = 2**2 + 3
+ // 8 = 2**2 + 4
+
+ //3.
+ // allocate space for boundaries
+ fRange = new Value[2*fNDim];
+ fIndPoints= new Index[fNpoints];
+ for (Index i=0; i<fNpoints; i++) fIndPoints[i] = i;
+ fNodes = new TKDNode[fNnodes];
+ //
+ fCrossNode = (1<<(fRowT0+1))-1;
+ if (fCrossNode<fNnodes) fCrossNode = 2*fCrossNode+1;
+ //
+ // fOffset = (((fNnodes+1)-(1<<fRowT0)))*2;
+ Int_t over = (fNnodes+1)-(1<<fRowT0);
+ Int_t filled = ((1<<fRowT0)-over)*fBucketSize;
+ fOffset = fNpoints-filled;
+ //
+// printf("Row0 %d\n", fRowT0);
+// printf("CrossNode %d\n", fCrossNode);
+// printf("Offset %d\n", fOffset);
+ //
+ //
+ //4.
+ // stack for non recursive build - size 128 bytes enough
+ Int_t rowStack[128];
+ Int_t nodeStack[128];
+ Int_t npointStack[128];
+ Int_t posStack[128];
+ Int_t currentIndex = 0;
+ Int_t iter =0;
+ rowStack[0] = 0;
+ nodeStack[0] = 0;
+ npointStack[0] = fNpoints;
+ posStack[0] = 0;
+ //
+ Int_t nbucketsall =0;
+ while (currentIndex>=0){
+ iter++;
+ //
+ Int_t npoints = npointStack[currentIndex];
+ if (npoints<=fBucketSize) {
+ //printf("terminal node : index %d iter %d\n", currentIndex, iter);
+ currentIndex--;
+ nbucketsall++;
+ continue; // terminal node
+ }
+ Int_t crow = rowStack[currentIndex];
+ Int_t cpos = posStack[currentIndex];
+ Int_t cnode = nodeStack[currentIndex];
+ TKDNode * node = &(fNodes[cnode]);
+ //printf("currentIndex %d npoints %d node %d\n", currentIndex, npoints, cnode);
+ //
+ // divide points
+ Int_t nbuckets0 = npoints/fBucketSize; //current number of buckets
+ if (npoints%fBucketSize) nbuckets0++; //
+ Int_t restRows = fRowT0-rowStack[currentIndex]; // rest of fully occupied node row
+ if (restRows<0) restRows =0;
+ for (;nbuckets0>(2<<restRows); restRows++);
+ Int_t nfull = 1<<restRows;
+ Int_t nrest = nbuckets0-nfull;
+ Int_t nleft =0, nright =0;
+ //
+ if (nrest>(nfull/2)){
+ nleft = nfull*fBucketSize;
+ nright = npoints-nleft;
+ }else{
+ nright = nfull*fBucketSize/2;
+ nleft = npoints-nright;
+ }
+
+ //
+ //find the axis with biggest spread
+ Value maxspread=0;
+ Value tempspread, min, max;
+ Index axspread=0;
+ Value *array;
+ for (Int_t idim=0; idim<fNDim; idim++){
+ array = fData[idim];
+ Spread(npoints, array, fIndPoints+cpos, min, max);
+ tempspread = max - min;
+ if (maxspread < tempspread) {
+ maxspread=tempspread;
+ axspread = idim;
+ }
+ if(cnode==0) {fRange[2*idim] = min; fRange[2*idim+1] = max;}
+ }
+ array = fData[axspread];
+ KOrdStat(npoints, array, nleft, fIndPoints+cpos);
+ node->fAxis = axspread;
+ node->fValue = array[fIndPoints[cpos+nleft]];
+ //printf("Set node %d : ax %d val %f\n", cnode, node->fAxis, node->fValue);
+ //
+ //
+ npointStack[currentIndex] = nleft;
+ rowStack[currentIndex] = crow+1;
+ posStack[currentIndex] = cpos;
+ nodeStack[currentIndex] = cnode*2+1;
+ currentIndex++;
+ npointStack[currentIndex] = nright;
+ rowStack[currentIndex] = crow+1;
+ posStack[currentIndex] = cpos+nleft;
+ nodeStack[currentIndex] = (cnode*2)+2;
+ //
+ if (0){
+ // consistency check
+ Info("Build()", Form("points %d left %d right %d", npoints, nleft, nright));
+ if (nleft<nright) Warning("Build", "Problem Left-Right");
+ if (nleft<0 || nright<0) Warning("Build()", "Problem Negative number");
+ }
+ }
+ OrderIndexes();
+
+ //printf("NBuckets\t%d\n", nbucketsall);
+ //fData = 0;
+}
+
+
+//_________________________________________________________________
+template <typename Index, typename Value>
+Bool_t TKDTree<Index, Value>::FindNearestNeighbors(const Value *point, const Int_t k, Index *&in, Value &d)
+{
+// Find "k" nearest neighbors to "point".
+//
+// Return true in case of success and false in case of failure.
+// The indexes of the nearest k points are stored in the array "in" in
+// increasing order of the distance to "point" and the maxim distance
+// in "d".
+//
+// The array "in" is managed internally by the TKDTree.
+
+ Index inode = FindNode(point);
+ if(inode < fNnodes){
+ Error("FindNearestNeighbors()", "Point outside data range.");
+ return kFALSE;
+ }
+
+ // allocate working memory
+ if(!fkNN) fkNN = new Index[k];
+ if(sizeof(fkNN) < k*sizeof(Index)) fkNN = new(fkNN) Index[k];
+ for(int i=0; i<k; i++) fkNN[i] = -1;
+ Index *fkNN_tmp = new Index[k];
+ Value *fkNN_dist = new Value[k];
+ for(int i=0; i<k; i++) fkNN_dist[i] = 9999.;
+ Value *fkNN_dist_tmp = new Value[k];
+ Value *dist = new Value[fBucketSize];
+ Index *index = new Index[fBucketSize];
+
+ // calculate number of boundaries with the data domain.
+ Index nBounds = 0;
+ if(!fBoundaries) MakeBoundaries();
+ Value *bounds = &fBoundaries[inode*2*fNDim];
+ for(int idim=0; idim<fNDim; idim++){
+ if(bounds[2*idim] == fRange[2*idim]) nBounds++;
+ if(bounds[2*idim+1] == fRange[2*idim+1]) nBounds++;
+ }
+
+ // traverse tree
+ TKDNode *node;
+ Int_t nodeStack[128], nodeIn[128];
+ Index currentIndex = 0;
+ nodeStack[0] = inode;
+ nodeIn[0] = inode;
+ while(currentIndex>=0){
+ Int_t tnode = nodeStack[currentIndex];
+ Int_t entry = nodeIn[currentIndex];
+ currentIndex--;
+
+ // check if node is still eligible
+ node = &fNodes[tnode/2 + (tnode%2) - 1];
+ if((TMath::Abs(point[node->fAxis] - node->fValue) > fkNN_dist[k-1])
+ &&
+ ((point[node->fAxis] > node->fValue && tnode%2) ||
+ (point[node->fAxis] < node->fValue && !tnode%2))) {
+ //printf("\tREMOVE NODE\n");
+
+ // mark bound
+ nBounds++;
+ // end all recursions
+ if(nBounds==2 * fNDim) break;
+ continue;
+ }
+
+ if(IsTerminal(tnode)){
+ // Link data to terminal node
+ Int_t offset = (tnode >= fCrossNode) ? (tnode-fCrossNode)*fBucketSize : fOffset+(tnode-fNnodes)*fBucketSize;
+ Index *indexPoints = &fIndPoints[offset];
+ Int_t nPoints = (tnode == 2*fNnodes) ? fNpoints%fBucketSize : fBucketSize;
+ for(int idx=0; idx<nPoints; idx++){
+ // calculate distance in the L1 metric
+ dist[idx] = 0.;
+ for(int idim=0; idim<fNDim; idim++) dist[idx] += TMath::Abs(point[idim] - fData[idim][indexPoints[idx]]);
+ }
+ // arrange increasingly distances array and update kNN indexes
+ TMath::Sort(nPoints, dist, index, kFALSE);
+ for(int ibrowse=0; ibrowse<nPoints; ibrowse++){
+ // exit if the current distance calculated in this node is
+ // larger than the largest distance stored in the kNN array
+ if(dist[index[ibrowse]] >= fkNN_dist[k-1]) break;
+ for(int iNN=0; iNN<k; iNN++){
+ if(dist[index[ibrowse]] < fkNN_dist[iNN]){
+ //insert neighbor. In principle this is only one call to STL vector. Maybe we can change to it ?!
+ //printf("\t\tinsert data %d @ %d distance %f\n", indexPoints[index[ibrowse]], iNN, dist[index[ibrowse]]);
+
+ memcpy(fkNN_tmp, &fkNN[iNN], (k-iNN-1)*sizeof(Index));
+ fkNN[iNN] = indexPoints[index[ibrowse]];
+ memcpy(&fkNN[iNN+1], fkNN_tmp, (k-iNN-1)*sizeof(Index));
+
+ memcpy(fkNN_dist_tmp, &fkNN_dist[iNN], (k-iNN-1)*sizeof(Value));
+ fkNN_dist[iNN] = dist[index[ibrowse]];
+ memcpy(&fkNN_dist[iNN+1], fkNN_dist_tmp, (k-iNN-1)*sizeof(Value));
+ break;
+ }
+ }
+ }
+ }
+
+ // register parent
+ Int_t parent_node = tnode/2 + (tnode%2) - 1;
+ if(parent_node >= 0 && entry != parent_node){
+ // check if parent node is eligible at all
+ node = &fNodes[parent_node];
+ if(TMath::Abs(point[node->fAxis] - node->fValue) > fkNN_dist[k-1]){
+ // mark bound
+ nBounds++;
+ // end all recursions
+ if(nBounds==2 * fNDim) break;
+ }
+ currentIndex++;
+ nodeStack[currentIndex]=tnode/2 + (tnode%2) - 1;
+ nodeIn[currentIndex]=tnode;
+ //printf("\tregister %d\n", nodeStack[currentIndex]);
+ }
+
+ // register children nodes
+ Int_t child_node;
+ Bool_t kAllow[] = {kTRUE, kTRUE};
+ node = &fNodes[tnode];
+ if(TMath::Abs(point[node->fAxis] - node->fValue) > fkNN_dist[k-1]){
+ if(point[node->fAxis] > node->fValue) kAllow[0] = kFALSE;
+ else kAllow[1] = kFALSE;
+ }
+ for(int ic=1; ic<=2; ic++){
+ if(!kAllow[ic-1]) continue;
+ child_node = (tnode*2)+ic;
+ if(child_node < fNnodes + GetNTerminalNodes() && entry != child_node){
+ currentIndex++;
+ nodeStack[currentIndex] = child_node;
+ nodeIn[currentIndex]=tnode;
+ //printf("\tregister %d\n", nodeStack[currentIndex]);
+ }
+ }
+ }
+ // save results
+ in = fkNN;
+ d = fkNN_dist[k-1];
+
+ delete [] index;
+ delete [] dist;
+ delete [] fkNN_tmp;
+ delete [] fkNN_dist;
+ delete [] fkNN_dist_tmp;
+
+ return kTRUE;
+}
+
+
+
+//_________________________________________________________________
+template <typename Index, typename Value>
+Index TKDTree<Index, Value>::FindNode(const Value * point){
+ //
+ // find the terminal node to which point belongs
+
+ Index stackNode[128], inode;
+ Int_t currentIndex =0;
+ stackNode[0] = 0;
+ //
+ while (currentIndex>=0){
+ inode = stackNode[currentIndex];
+ currentIndex--;
+ if (IsTerminal(inode)) return inode;
+
+ TKDNode & node = fNodes[inode];
+ if (point[node.fAxis]<=node.fValue){
+ currentIndex++;
+ stackNode[currentIndex]=(inode*2)+1;
+ }
+ if (point[node.fAxis]>=node.fValue){
+ currentIndex++;
+ stackNode[currentIndex]=(inode*2)+2;
+ }
+ }
+}
+
+
+//_________________________________________________________________
+template <typename Index, typename Value>
+void TKDTree<Index, Value>::FindPoint(Value * point, Index &index, Int_t &iter){
+ //
+ // find the index of point
+ // works only if we keep fData pointers
+
+ Int_t stackNode[128];
+ Int_t currentIndex =0;
+ stackNode[0] = 0;
+ iter =0;
+ //
+ while (currentIndex>=0){
+ iter++;
+ Int_t inode = stackNode[currentIndex];
+ currentIndex--;
+ if (IsTerminal(inode)){
+ // investigate terminal node
+ Int_t indexIP = (inode >= fCrossNode) ? (inode-fCrossNode)*fBucketSize : (inode-fNnodes)*fBucketSize+fOffset;
+ printf("terminal %d indexP %d\n", inode, indexIP);
+ for (Int_t ibucket=0;ibucket<fBucketSize;ibucket++){
+ Bool_t isOK = kTRUE;
+ indexIP+=ibucket;
+ printf("ibucket %d index %d\n", ibucket, indexIP);
+ if (indexIP>=fNpoints) continue;
+ Int_t index0 = fIndPoints[indexIP];
+ for (Int_t idim=0;idim<fNDim;idim++) if (fData[idim][index0]!=point[idim]) isOK = kFALSE;
+ if (isOK) index = index0;
+ }
+ continue;
+ }
+
+ TKDNode & node = fNodes[inode];
+ if (point[node.fAxis]<=node.fValue){
+ currentIndex++;
+ stackNode[currentIndex]=(inode*2)+1;
+ }
+ if (point[node.fAxis]>=node.fValue){
+ currentIndex++;
+ stackNode[currentIndex]=(inode*2)+2;
+ }
+ }
+ //
+ // printf("Iter\t%d\n",iter);
+}
+
+//_________________________________________________________________
+template <typename Index, typename Value>
+void TKDTree<Index, Value>::FindInRangeA(Value * point, Value * delta, Index *res , Index &npoints, Index & iter, Int_t bnode)
+{
+ //
+ // Find all points in the range specified by (point +- range)
+ // res - Resulting indexes are stored in res array
+ // npoints - Number of selected indexes in range
+ // NOTICE:
+ // For some cases it is better to don't keep data - because of memory consumption
+ // If the data are not kept - only boundary conditions are investigated
+ // some of the data can be outside of the range
+ // What is guranteed in this mode: All of the points in the range are selected + some fraction of others (but close)
+
+ Index stackNode[128];
+ iter=0;
+ Index currentIndex = 0;
+ stackNode[currentIndex] = bnode;
+ while (currentIndex>=0){
+ iter++;
+ Int_t inode = stackNode[currentIndex];
+ //
+ currentIndex--;
+ if (!IsTerminal(inode)){
+ // not terminal
+ TKDNode * node = &(fNodes[inode]);
+ if (point[node->fAxis] - delta[node->fAxis] < node->fValue) {
+ currentIndex++;
+ stackNode[currentIndex]= (inode*2)+1;
+ }
+ if (point[node->fAxis] + delta[node->fAxis] >= node->fValue){
+ currentIndex++;
+ stackNode[currentIndex]= (inode*2)+2;
+ }
+ }else{
+ Int_t indexIP =
+ (inode >= fCrossNode) ? (inode-fCrossNode)*fBucketSize : (inode-fNnodes)*fBucketSize+fOffset;
+ for (Int_t ibucket=0;ibucket<fBucketSize;ibucket++){
+ if (indexIP+ibucket>=fNpoints) break;
+ res[npoints] = fIndPoints[indexIP+ibucket];
+ npoints++;
+ }
+ }
+ }
+ if (fData){
+ //
+ // compress rest if data still accesible
+ //
+ Index npoints2 = npoints;
+ npoints=0;
+ for (Index i=0; i<npoints2;i++){
+ Bool_t isOK = kTRUE;
+ for (Index idim = 0; idim< fNDim; idim++){
+ if (TMath::Abs(fData[idim][res[i]]- point[idim])>delta[idim])
+ isOK = kFALSE;
+ }
+ if (isOK){
+ res[npoints] = res[i];
+ npoints++;
+ }
+ }
+ }
+}
+
+
+//_________________________________________________________________
+template <typename Index, typename Value>
+void TKDTree<Index, Value>::FindInRangeB(Value * point, Value * delta, Index *res , Index &npoints,Index & iter, Int_t bnode)
+{
+ //
+ Long64_t goldStatus = (1<<(2*fNDim))-1; // gold status
+ Index stackNode[128];
+ Long64_t stackStatus[128];
+ iter=0;
+ Index currentIndex = 0;
+ stackNode[currentIndex] = bnode;
+ stackStatus[currentIndex] = 0;
+ while (currentIndex>=0){
+ Int_t inode = stackNode[currentIndex];
+ Long64_t status = stackStatus[currentIndex];
+ currentIndex--;
+ iter++;
+ if (IsTerminal(inode)){
+ Int_t indexIP =
+ (inode >= fCrossNode) ? (inode-fCrossNode)*fBucketSize : (inode-fNnodes)*fBucketSize+fOffset;
+ for (Int_t ibucket=0;ibucket<fBucketSize;ibucket++){
+ if (indexIP+ibucket>=fNpoints) break;
+ res[npoints] = fIndPoints[indexIP+ibucket];
+ npoints++;
+ }
+ continue;
+ }
+ // not terminal
+ if (status == goldStatus){
+ Int_t ileft = inode;
+ Int_t iright = inode;
+ for (;ileft<fNnodes; ileft = (ileft<<1)+1);
+ for (;iright<fNnodes; iright = (iright<<1)+2);
+ Int_t indexL =
+ (ileft >= fCrossNode) ? (ileft-fCrossNode)*fBucketSize : (ileft-fNnodes)*fBucketSize+fOffset;
+ Int_t indexR =
+ (iright >= fCrossNode) ? (iright-fCrossNode)*fBucketSize : (iright-fNnodes)*fBucketSize+fOffset;
+ if (indexL<=indexR){
+ Int_t endpoint = indexR+fBucketSize;
+ if (endpoint>fNpoints) endpoint=fNpoints;
+ for (Int_t ipoint=indexL;ipoint<endpoint;ipoint++){
+ res[npoints] = fIndPoints[ipoint];
+ npoints++;
+ }
+ continue;
+ }
+ }
+ //
+ TKDNode * node = &(fNodes[inode]);
+ if (point[node->fAxis] - delta[node->fAxis] < node->fValue) {
+ currentIndex++;
+ stackNode[currentIndex]= (inode<<1)+1;
+ if (point[node->fAxis] + delta[node->fAxis] > node->fValue)
+ stackStatus[currentIndex]= status | (1<<(2*node->fAxis));
+ }
+ if (point[node->fAxis] + delta[node->fAxis] >= node->fValue){
+ currentIndex++;
+ stackNode[currentIndex]= (inode<<1)+2;
+ if (point[node->fAxis] - delta[node->fAxis]<node->fValue)
+ stackStatus[currentIndex]= status | (1<<(2*node->fAxis+1));
+ }
+ }
+ if (fData){
+ // compress rest
+ Index npoints2 = npoints;
+ npoints=0;
+ for (Index i=0; i<npoints2;i++){
+ Bool_t isOK = kTRUE;
+ for (Index idim = 0; idim< fNDim; idim++){
+ if (TMath::Abs(fData[idim][res[i]]- point[idim])>delta[idim])
+ isOK = kFALSE;
+ }
+ if (isOK){
+ res[npoints] = res[i];
+ npoints++;
+ }
+ }
+ }
+}
+
+
+
+//_________________________________________________________________
+template <typename Index, typename Value>
+void TKDTree<Index, Value>::SetData(Index npoints, Index ndim, UInt_t bsize, Value **data)
+{
+// TO DO
+//
+// Check reconstruction/reallocation of memory of data. Maybe it is not
+// necessary to delete and realocate space but only to use the same space
+
+ Clear();
+
+ //Columnwise!!!!
+ fData = data;
+ fNpoints = npoints;
+ fNDim = ndim;
+ fBucketSize = bsize;
+
+ Build();
+}
+
+
+
+//_________________________________________________________________
+template <typename Index, typename Value>
+void TKDTree<Index, Value>::Spread(Index ntotal, Value *a, Index *index, Value &min, Value &max)
+{
+ //Value min, max;
+ Index i;
+ min = a[index[0]];
+ max = a[index[0]];
+ for (i=0; i<ntotal; i++){
+ if (a[index[i]]<min) min = a[index[i]];
+ if (a[index[i]]>max) max = a[index[i]];
+ }
+}
+
+
+//_________________________________________________________________
+template <typename Index, typename Value>
+Value TKDTree<Index, Value>::KOrdStat(Index ntotal, Value *a, Index k, Index *index)
+{
+ //
+ //copy of the TMath::KOrdStat because I need an Index work array
+
+ Index i, ir, j, l, mid;
+ Index arr;
+ Index temp;
+
+ Index rk = k;
+ l=0;
+ ir = ntotal-1;
+ for(;;) {
+ if (ir<=l+1) { //active partition contains 1 or 2 elements
+ if (ir == l+1 && a[index[ir]]<a[index[l]])
+ {temp = index[l]; index[l]=index[ir]; index[ir]=temp;}
+ Value tmp = a[index[rk]];
+ return tmp;
+ } else {
+ mid = (l+ir) >> 1; //choose median of left, center and right
+ {temp = index[mid]; index[mid]=index[l+1]; index[l+1]=temp;}//elements as partitioning element arr.
+ if (a[index[l]]>a[index[ir]]) //also rearrange so that a[l]<=a[l+1]
+ {temp = index[l]; index[l]=index[ir]; index[ir]=temp;}
+
+ if (a[index[l+1]]>a[index[ir]])
+ {temp=index[l+1]; index[l+1]=index[ir]; index[ir]=temp;}
+
+ if (a[index[l]]>a[index[l+1]])
+ {temp = index[l]; index[l]=index[l+1]; index[l+1]=temp;}
+
+ i=l+1; //initialize pointers for partitioning
+ j=ir;
+ arr = index[l+1];
+ for (;;) {
+ do i++; while (a[index[i]]<a[arr]);
+ do j--; while (a[index[j]]>a[arr]);
+ if (j<i) break; //pointers crossed, partitioning complete
+ {temp=index[i]; index[i]=index[j]; index[j]=temp;}
+ }
+ index[l+1]=index[j];
+ index[j]=arr;
+ if (j>=rk) ir = j-1; //keep active the partition that
+ if (j<=rk) l=i; //contains the k_th element
+ }
+ }
+}
+
+//_________________________________________________________________
+template <typename Index, typename Value>
+void TKDTree<Index, Value>::MakeBoundaries(Value *range)
+{
+// Build boundaries for each node.
+
+
+ if(range) memcpy(fRange, range, 2*fNDim*sizeof(Value));
+ // total number of nodes including terminal nodes
+ Int_t totNodes = fNnodes + GetNTerminalNodes();
+ fBoundaries = new Value[totNodes*2*fNDim];
+ printf("Allocate %d nodes\n", totNodes);
+ // loop
+ Int_t child_index;
+ for(int inode=fNnodes-1; inode>=0; inode--){
+ //printf("\tAllocate node %d\n", inode);
+ memcpy(&fBoundaries[inode*2*fNDim], fRange, 2*fNDim*sizeof(Value));
+
+ // check left child node
+ child_index = 2*inode+1;
+ if(IsTerminal(child_index)) CookBoundariesTerminal(inode, kTRUE);
+ for(int idim=0; idim<fNDim; idim++) fBoundaries[2*fNDim*inode+2*idim] = fBoundaries[2*fNDim*child_index+2*idim];
+
+ // check right child node
+ child_index = 2*inode+2;
+ if(IsTerminal(child_index)) CookBoundariesTerminal(inode, kFALSE);
+ for(int idim=0; idim<fNDim; idim++) fBoundaries[2*fNDim*inode+2*idim+1] = fBoundaries[2*fNDim*child_index+2*idim+1];
+ }
+}
+
+//_________________________________________________________________
+template <typename Index, typename Value>
+void TKDTree<Index, Value>::CookBoundariesTerminal(Int_t parent_node, Bool_t LEFT)
+{
+ // define index of this terminal node
+ Int_t index = 2 * parent_node + (LEFT ? 1 : 2);
+
+ // build and initialize boundaries for this node
+ //printf("\tAllocate terminal node %d\n", index);
+ memcpy(&fBoundaries[2*fNDim*index], fRange, 2*fNDim*sizeof(Value));
+ Bool_t flag[256]; // cope with up to 128 dimensions
+ memset(flag, kFALSE, 2*fNDim);
+ Int_t nvals = 0;
+
+ // recurse parent nodes
+ TKDNode *node = 0x0;
+ while(parent_node >= 0 && nvals < 2 * fNDim){
+ node = &(fNodes[parent_node]);
+ if(LEFT){
+ if(!flag[2*node->fAxis+1]) {
+ fBoundaries[2*fNDim*index+2*node->fAxis+1] = node->fValue;
+ flag[2*node->fAxis+1] = kTRUE;
+ nvals++;
+ }
+ } else {
+ if(!flag[2*node->fAxis]) {
+ fBoundaries[2*fNDim*index+2*node->fAxis] = node->fValue;
+ flag[2*node->fAxis] = kTRUE;
+ nvals++;
+ }
+ }
+ LEFT = parent_node%2;
+ parent_node = parent_node/2 + (parent_node%2) - 1;
+ }
+}
+
+//_________________________________________________________________
+template <typename Index, typename Value>
+void TKDTree<Index, Value>::OrderIndexes()
+{
+// Order array of point's indexes in increasing order of terminal nodes
+// indexes such that first bucket points correspond to first terminal
+// node (index fNnodes) and so on.
+
+ printf("fRowT0 %d fCrossNode %d fOffset %d\n", fRowT0, fCrossNode, fOffset);
+
+}
+
+template class TKDTree<Int_t, Float_t>;
+template class TKDTree<Int_t, Double_t>;
+
git://git.uio.no / u / mrichter / AliRoot.git / commitdiff