Index: issm/trunk-jpl/src/c/bamg/BamgQuadtree.cpp
===================================================================
--- issm/trunk-jpl/src/c/bamg/BamgQuadtree.cpp	(revision 18064)
+++ issm/trunk-jpl/src/c/bamg/BamgQuadtree.cpp	(revision 21623)
@@ -7,46 +7,5 @@
 
 namespace bamg {
-
-	/*MACROS {{{*/
-	/* 
-	 * 
-	 *    J    j
-	 *    ^    ^
-	 *    |    | +--------+--------+
-	 *    |    | |        |        |
-	 * 1X |    | |   2    |   3    |
-	 *    |    | |        |        |
-	 *    |    | +--------+--------+
-	 *    |    | |        |        |
-	 * 0X |    | |   0    |   1    |
-	 *    |    | |        |        |
-	 *    |    | +--------+--------+
-	 *    |    +-----------------------> i
-	 *    |         
-	 *    |----------------------------> I
-	 *              X0        X1  
-	 *
-	 * box 0 -> I=0 J=0 IJ=00  = 0
-	 * box 1 -> I=1 J=0 IJ=01  = 1
-	 * box 2 -> I=0 J=1 IJ=10  = 2
-	 * box 3 -> I=1 J=1 IJ=11  = 3
-	 */
-#define INTER_SEG(a,b,x,y) (((y) > (a)) && ((x) <(b)))
 #define ABS(i) ((i)<0 ?-(i) :(i))
-#define MAX1(i,j) ((i)>(j) ?(i) :(j))
-#define NORM(i1,j1,i2,j2) MAX1(ABS((i1)-(j1)),ABS((i2)-(j2)))
-
-	//IJ(i,j,l) returns the box number of i and j with respect to l
-	//if !j&l and !i&l -> 0 (box zero: lower left )
-	//if !j&l and  i&l -> 1 (box one:  lower right)
-	//if  j&l and !i&l -> 2 (box two:  upper left )
-	//if  j&l and  i&l -> 3 (box three:upper right)
-#define IJ(i,j,l)  ((j&l) ? ((i&l) ? 3:2 ) :((i&l) ? 1:0 ))
-
-	//I_IJ(k,l) returns l if first  bit of k is 1, else 0
-#define I_IJ(k,l)  ((k&1) ? l:0)
-	//J_IJ(k,l) returns l if second bit of k is 1, else 0
-#define J_IJ(k,l)  ((k&2) ? l:0)
-	/*}}}*/
 	/*DOCUMENTATION What is a BamgQuadtree? {{{
 	 * A Quadtree is a very simple way to group vertices according
@@ -100,34 +59,41 @@
 	BamgQuadtree::BamgQuadtree(){/*{{{*/
 
-		/*Number of boxes and vertices*/
-		NbBamgQuadtreeBox=0;
-		NbVertices=0;
+		/*Initialize fields*/
+		this->MaxDepth      = 30;
+		this->MaxISize      = 1073741824; //2^30
+		this->MaxICoord     = 1073741823; //2^30 - 1
+		this->NbQuadtreeBox = 0;
+		this->NbVertices    = 0;
 
 		/*Create container*/
-		boxcontainer=new DataSet();
+		this->boxcontainer=new DataSet();
 
 		/*Create Root, pointer toward the main box*/
-		root=NewBamgQuadtreeBox();
-
-		}
-	/*}}}*/
+		this->root=NewBamgQuadtreeBox();
+
+	} /*}}}*/
 	BamgQuadtree::BamgQuadtree(Mesh * t,long nbv){ /*{{{*/
 
-		/*Number of boxes and vertices*/
-		NbBamgQuadtreeBox=0;
-		NbVertices=0;
+		/*Initialize fields*/
+		this->MaxDepth      = 30;
+		this->MaxISize      = 1073741824; //2^30
+		this->MaxICoord     = 1073741823; //2^30 - 1
+		this->NbQuadtreeBox = 0;
+		this->NbVertices    = 0;
 
 		/*Create container*/
-		boxcontainer=new DataSet();
+		this->boxcontainer=new DataSet();
 
 		/*Create Root, pointer toward the main box*/
-		root=NewBamgQuadtreeBox();
+		this->root=NewBamgQuadtreeBox();
 
 		/*Check Sizes*/
-		_assert_(MaxISize>MaxICoor);
+		_assert_(this->MaxISize>this->MaxICoord);
 
 		/*Add all vertices of the mesh*/
-		if (nbv==-1) nbv=t->nbv;
-		for (int i=0;i<nbv;i++) Add(t->vertices[i]);
+		if(nbv==-1) nbv=t->nbv;
+		for(int i=0;i<nbv;i++){
+			this->Add(t->vertices[i]);
+		}
 
 	}
@@ -140,5 +106,5 @@
 
 	/*Methods*/
-	void  BamgQuadtree::Add(BamgVertex &w){/*{{{*/
+	void          BamgQuadtree::Add(BamgVertex &w){/*{{{*/
 		/*Original code from Frederic Hecht <hecht@ann.jussieu.fr> (BAMG v1.01, BamgQuadtree.cpp/Add)*/
 		BamgQuadtreeBox** pb=NULL;
@@ -164,5 +130,5 @@
 
 			//Get next subbox according to the bit value (level)
-			pb = &b->b[IJ(i,j,level)];
+			pb = &b->box[BoxNumber(i,j,level)];
 		}
 
@@ -195,5 +161,5 @@
 
 			/*Initialize the 4 pointers toward the 4 subboxes*/
-			b->b[0]=b->b[1]=b->b[2]=b->b[3]=NULL;
+			b->box[0]=b->box[1]=b->box[2]=b->box[3]=NULL;
 
 			/*level = 0010000 -> 0001000*/
@@ -205,8 +171,8 @@
 				int          ij;
 				/*bb is the new "sub"box of b where v4[k] is located*/
-				BamgQuadtreeBox *bb = b->b[ij=IJ(v4[k]->i.x,v4[k]->i.y,level)];
+				BamgQuadtreeBox *bb = b->box[ij=BoxNumber(v4[k]->i.x,v4[k]->i.y,level)];
 
 				// alloc the BamgQuadtreeBox
-				if (!bb) bb=b->b[ij]=NewBamgQuadtreeBox(); 
+				if (!bb) bb=b->box[ij]=NewBamgQuadtreeBox(); 
 
 				/*Copy the current vertex*/
@@ -215,5 +181,5 @@
 
 			/*Get the subbox where w (i,j) is located*/
-			pb = &b->b[IJ(i,j,level)];
+			pb = &b->box[BoxNumber(i,j,level)];
 		}
 
@@ -228,48 +194,79 @@
 	}
 	/*}}}*/
-	BamgVertex*  BamgQuadtree::NearestVertex(Icoor1 i,Icoor1 j) {/*{{{*/
-		/*Original code from Frederic Hecht <hecht@ann.jussieu.fr> (BAMG v1.01, BamgQuadtree.cpp/NearestVertex)*/
-
-		/*Intermediaries*/
-		BamgQuadtreeBox *pb[MaxDepth];
-		int          pi[MaxDepth];
-		Icoor1       ii[MaxDepth];
-		Icoor1       jj[MaxDepth];
-		int          level;
-		long         n0;
-		BamgQuadtreeBox *b;
-		long         h0;
-		long         h = MaxISize;
-		long         hb= MaxISize;
-		Icoor1       i0=0,j0=0;
+	int           BamgQuadtree::BoxNumber(int i,int j,int l){/*{{{*/
+		/* 
+		 * 
+		 *    J    j
+		 *    ^    ^
+		 *    |    | +--------+--------+
+		 *    |    | |        |        |
+		 * 1X |    | |   2    |   3    |
+		 *    |    | |        |        |
+		 *    |    | +--------+--------+
+		 *    |    | |        |        |
+		 * 0X |    | |   0    |   1    |
+		 *    |    | |        |        |
+		 *    |    | +--------+--------+
+		 *    |    +-----------------------> i
+		 *    |         
+		 *    |----------------------------> I
+		 *              X0        X1  
+		 *
+		 * box 0 -> I=0 J=0 BoxNumber=00  = 0
+		 * box 1 -> I=1 J=0 BoxNumber=01  = 1
+		 * box 2 -> I=0 J=1 BoxNumber=10  = 2
+		 * box 3 -> I=1 J=1 BoxNumber=11  = 3
+		 */
+		//BoxNumber(i,j,l) returns the box number of i and j with respect to l
+		//if !j&l and !i&l -> 0 (box zero: lower left )
+		//if !j&l and  i&l -> 1 (box one:  lower right)
+		//if  j&l and !i&l -> 2 (box two:  upper left )
+		//if  j&l and  i&l -> 3 (box three:upper right)
+		return ((j&l) ? ((i&l) ? 3:2 ) :((i&l) ? 1:0 ));
+	}/*}}}*/
+	bool          BamgQuadtree::Intersection(int a,int b,int x,int y){/*{{{*/
+		/*is [x y] in [a b]*/
+		return ((y) > (a)) && ((x) <(b));
+	}/*}}}*/
+	BamgVertex*   BamgQuadtree::NearestVertex(int xi,int yi) {/*{{{*/
 
 		/*initial output as NULL (no vertex found)*/
 		BamgVertex*  nearest_v=NULL;
 
-		/*Project w coordinates (i,j) onto [0,MaxISize-1] x [0,MaxISize-1] -> (iplus,jplus)*/
-		Icoor1 iplus( i<MaxISize ? (i<0?0:i) : MaxISize-1);
-		Icoor1 jplus( j<MaxISize ? (j<0?0:j) : MaxISize-1);
-
-		/*Get initial Quadtree box (largest)*/
-		b = root;
-
 		/*if the tree is empty, return NULL pointer*/
-		if (!root->nbitems) return nearest_v; 
+		if(!this->root->nbitems) return nearest_v; 
+
+		/*Project coordinates (xi,yi) onto [0,MaxICoord-1] x [0,MaxICoord-1]*/
+		int xi2 = xi;
+		int yi2 = yi;
+		if(xi<0)        xi2 = 0;
+		if(xi>MaxISize) xi2 = MaxICoord;
+		if(yi<0)        yi2 = 0;
+		if(yi>MaxISize) yi2 = MaxICoord;
+
+		/*Get inital box (the largest)*/
+		BamgQuadtreeBox* b = this->root;
+
+		/*Initialize level and sizes for largest box*/
+		int levelbin = (1L<<this->MaxDepth);// = 2^30
+		int        h = this->MaxISize;
+		int       hb = this->MaxISize;
+		int       i0 = 0;
+		int       j0 = 0;
 
 		/*else, find the smallest non-empty BamgQuadtreeBox containing  the point (i,j)*/
-		while((n0=b->nbitems)<0){
-
-			Icoor1       hb2 = hb >> 1;             //size of the current box
-			int          k   = IJ(iplus,jplus,hb2); //box number (0,1,2 or 3)
-			BamgQuadtreeBox *b0  = b->b[k];             //pointer toward current box
-
-			/* break if NULL box or empty (Keep previous box b)*/
-			if (( b0 == NULL) || (b0->nbitems == 0)) break;
+		while(b->nbitems<0){
+
+			int hb2 = hb >> 1;                  //size of the current box
+			int k   = BoxNumber(xi2,yi2,hb2);   //box number (0,1,2 or 3)
+			BamgQuadtreeBox* b0  = b->box[k];   //pointer toward current box
+
+			/* break if box is empty (Keep previous box b)*/
+			if((b0==NULL) || (b0->nbitems==0)) break;
 
 			/*Get next Quadtree box*/
-			b=b0;	
-			i0 += I_IJ(k,hb2); // i orign of BamgQuadtreeBox (macro)
-			j0 += J_IJ(k,hb2); // j orign of BamgQuadtreeBox 
-			hb = hb2;          // size of the box (in Int)
+			b  = b0;
+			hb = hb2;
+			this->SubBoxCoords(&i0,&j0,k,hb2);
 		}
 
@@ -280,18 +277,18 @@
 		 * - i0: x coordinate of the lower left corner
 		 * - j0: y coordinate of the lower left corner*/
+		int n0 = b->nbitems;
 
 		/* if the current subbox is holding vertices, we are almost done*/
-		if (n0>0){  
-			//loop over the vertices of the box and find the closest vertex
+		if(n0>0){  
+			/*loop over the vertices of the box and find the closest vertex*/
 			for(int k=0;k<n0;k++){
-
-				/*get integer coordinates of current vertex*/
-				I2 i2=b->v[k]->i;
-
-				/*Compute norm with w*/
-				h0=NORM(iplus,i2.x,jplus,i2.y);
+				int xiv = b->v[k]->i.x;
+				int yiv = b->v[k]->i.y;
+
+
+				int h0 = Norm(xi2,xiv,yi2,yiv);
 
 				/*is it smaller than previous value*/
-				if (h0<h){
+				if(h0<h){
 					h = h0;
 					nearest_v = b->v[k];
@@ -306,4 +303,8 @@
 
 		/*Initialize search variables*/
+		BamgQuadtreeBox **pb = xNew<BamgQuadtreeBox*>(this->MaxDepth);
+		int* pi = xNew<int>(this->MaxDepth);
+		int* ii = xNew<int>(this->MaxDepth);
+		int* jj = xNew<int>(this->MaxDepth);
 		pb[0]=b;                             //pointer toward the box b
 		pi[0]=b->nbitems>0?(int)b->nbitems:4;//number of boxes in b
@@ -315,12 +316,11 @@
 
 		/*Main loop*/
-		level=0;
-		do {
-
+		int level=0;
+		do{
 			/*get current box*/
-			b= pb[level];
+			b = pb[level];
 
 			/*Loop over the items in current box (if not empty!)*/
-			while (pi[level]){
+			while(pi[level]){
 
 				/*We are looping now over the items of b. k is the current index (in [0 3])*/
@@ -330,6 +330,5 @@
 				/*if the current subbox is holding vertices (b->nbitems<0 is subboxes)*/
 				if (b->nbitems>0){
-					I2 i2=b->v[k]->i;
-					h0 = NORM(iplus,i2.x,jplus,i2.y);
+					int h0 = Norm(xi2,b->v[k]->i.x,yi2,b->v[k]->i.y);
 					if (h0<h){
 						h=h0;
@@ -345,24 +344,25 @@
 
 					/*if the next box exists:*/
-					if((b=b->b[k])){
+					if((b=b->box[k])){
 
 						/*Get size (hb) and coordinates of the current sub-box lowest left corner*/
 						hb>>=1;
-						Icoor1 iii = ii[level]+I_IJ(k,hb);
-						Icoor1 jjj = jj[level]+J_IJ(k,hb);
-
-						/*if the current point (iplus,jplus) is in b (modulo h), this box is good:
+						int iii = ii[level];
+						int jjj = jj[level];
+						this->SubBoxCoords(&iii,&jjj,k,hb);
+
+						/*if the current point (xi2,yi2) is in b (modulo h), this box is good:
 						 * it is holding vertices that are close to w */
-						if (INTER_SEG(iii,iii+hb,iplus-h,iplus+h) && INTER_SEG(jjj,jjj+hb,jplus-h,jplus+h)){
+						if(this->Intersection(iii,iii+hb,xi2-h,xi2+h) && this->Intersection(jjj,jjj+hb,yi2-h,yi2+h)){
 							level++;
-							pb[level]= b;
-							pi[level]= b->nbitems>0 ?(int)  b->nbitems : 4  ;
-							ii[level]= iii;
-							jj[level]= jjj;
+							pb[level] = b;
+							pi[level] = b->nbitems>0 ? b->nbitems:4  ;
+							ii[level] = iii;
+							jj[level] = jjj;
 						}
 
-						//else go backwards
+						/*else go backwards*/
 						else{
-							//shifted righ by one bit: hb=001000000 -> 01000000
+							/*shifted righ by one bit: hb=001000000 -> 01000000*/
 							b=b0;
 							hb<<=1;
@@ -379,82 +379,107 @@
 			 * in case there is a vertex closest to w that has not yet been tested*/
 			hb <<= 1;
-		} while (level--);
-
-		/*return nearest_v, nearest vertex*/
+		}while(level--);
+
+		/*return nearest vertex pointer*/
+		xDelete<BamgQuadtreeBox*>(pb);
+		xDelete<int>(pi);
+		xDelete<int>(ii);
+		xDelete<int>(jj);
 		return nearest_v;
-
 	}
 	/*}}}*/
-	BamgQuadtree::BamgQuadtreeBox* BamgQuadtree::NewBamgQuadtreeBox(void){/*{{{*/
-
-		/*Output*/
-		BamgQuadtreeBox* newbox=NULL;
-
-		/*Create and initialize a new box*/
-		newbox=new BamgQuadtreeBox;
-		newbox->nbitems=0;
-		newbox->b[0]=NULL;
-		newbox->b[1]=NULL;
-		newbox->b[2]=NULL;
-		newbox->b[3]=NULL;
-
-		/*Add root to the container*/
-		boxcontainer->AddObject(newbox);
-
-		/*Increase counter*/
-		NbBamgQuadtreeBox++;
-
-		/*currentbox now points toward next quadtree box*/
-		return newbox;
+	int           BamgQuadtree::Norm(int xi1,int xi2,int yi1,int yi2){/*{{{*/
+
+		int deltax = xi2 - xi1;
+		int deltay = yi2 - yi1;
+
+		if(deltax<0) deltax = -deltax;
+		if(deltay<0) deltay = -deltay;
+
+		if(deltax> deltay){
+			return deltax;
+		}
+		else{
+			return deltay;
+		}
 	}/*}}}*/
-	BamgVertex*   BamgQuadtree::ToClose(BamgVertex & v,double seuil,Icoor1 hx,Icoor1 hy){/*{{{*/
-		/*Original code from Frederic Hecht <hecht@ann.jussieu.fr> (BAMG v1.01, BamgQuadtree.cpp/ToClose)*/
-
-		const Icoor1 i=v.i.x;
-		const Icoor1 j=v.i.y;
-		const R2 X(v.r);
-		const Metric  Mx(v.m);
-
-		BamgQuadtreeBox * pb[ MaxDepth ];
-		int  pi[ MaxDepth  ];
-		Icoor1 ii[  MaxDepth ], jj [ MaxDepth];
+	void          BamgQuadtree::SubBoxCoords(int* pi,int*pj,int boxnumber,int length){/*{{{*/
+		/* 
+		 *         j (first bit)
+		 *         ^
+		 *         | +--------+--------+
+		 *         | |        |        |
+		 *   1X    | |   2    |   3    |
+		 *         | |        |        |
+		 *         | +--------+--------+
+		 *         | |        |        |
+		 *   0X    | |   0    |   1    |
+		 *         | |        |        |
+		 *         | +--------+--------+
+		 *         +-----------------------> i (second bit)
+		 *               X0       X1
+		 */
+
+		/*Add sub-box coordinate to i and j*/
+		//_assert_(!(*pi & length));
+		//_assert_(!(*pj & length));
+
+		/*length if first  bit of boxnumber is 1, elengthse 0*/
+		*pi += ((boxnumber & 1) ? length:0);
+		/*length if second bit of boxnumber is 1, elengthse 0*/
+		*pj += ((boxnumber & 2) ? length:0);
+
+	}/*}}}*/
+	BamgVertex*   BamgQuadtree::TooClose(BamgVertex* v,double threshold,int hx,int hy){/*{{{*/
+
+		const int i=v->i.x;
+		const int j=v->i.y;
+		const double Xx = v->r.x;
+		const double Xy = v->r.y;
+		Metric* Mx = new Metric(v->m);
+
+		BamgQuadtreeBox *pb[MaxDepth];
+		int  pi[MaxDepth];
+		int  ii[MaxDepth], jj[MaxDepth];
 		int l=0; // level
-		BamgQuadtreeBox * b;
-		Icoor1 hb =  MaxISize;
-		Icoor1 i0=0,j0=0;
-
-		//  BamgVertex *vn=0;
-
-		if (!root->nbitems)
-		 return 0; // empty tree 
-
-		// general case -----
+		BamgQuadtreeBox* b;
+		int hb =  MaxISize;
+		int i0=0,j0=0;
+
+		// BamgVertex *vn=0;
+
+		if(!root->nbitems) return 0; // empty tree 
+
+		// general case
 		pb[0]=root;
-		pi[0]=root->nbitems>0 ?(int)  root->nbitems : 4  ;
+		pi[0]=root->nbitems>0 ?(int)root->nbitems:4;
 		ii[0]=i0;
 		jj[0]=j0;
-		do {    
+		do{
 			b= pb[l];
-			while (pi[l]--){ 	      
+			while(pi[l]--){ 	      
 				int k = pi[l];
 
-				if (b->nbitems>0){ // BamgVertex BamgQuadtreeBox none empty
-					I2 i2 =  b->v[k]->i;
-					if ( ABS(i-i2.x) <hx && ABS(j-i2.y) <hy )
-					  {
-						R2 XY(X,b->v[k]->r);
-						if(LengthInterpole(Mx(XY), b->v[k]->m(XY)) < seuil){
+				if(b->nbitems>0){ // BamgVertex BamgQuadtreeBox none empty
+					int i2x = b->v[k]->i.x;
+					int i2y = b->v[k]->i.y;
+					if (ABS(i-i2x)<hx && ABS(j-i2y) <hy ){
+						double XYx = b->v[k]->r.x - Xx;
+						double XYy = b->v[k]->r.y - Xy;
+						if(LengthInterpole(Mx->Length(XYx,XYy),b->v[k]->m.Length(XYx,XYy)) < threshold){
+							delete Mx;
 							return b->v[k]; 
 						}
-					  }
+					}
 				}
 				else{ // Pointer BamgQuadtreeBox 
 					BamgQuadtreeBox *b0=b;
-					if ((b=b->b[k])){
+					if ((b=b->box[k])){
 						hb >>=1 ; // div by 2
-						long iii = ii[l]+I_IJ(k,hb);
-						long jjj = jj[l]+J_IJ(k,hb);
-
-						if  (INTER_SEG(iii,iii+hb,i-hx,i+hx) && INTER_SEG(jjj,jjj+hb,j-hy,j+hy)){
+						int iii = ii[l];
+						int jjj = jj[l];
+						this->SubBoxCoords(&iii,&jjj,k,hb);
+
+						if(this->Intersection(iii,iii+hb,i-hx,i+hx) && this->Intersection(jjj,jjj+hb,j-hy,j+hy)){
 							pb[++l]=  b;
 							pi[l]= b->nbitems>0 ?(int)  b->nbitems : 4  ;
@@ -474,8 +499,30 @@
 			}
 			hb <<= 1; // mul by 2 
-		} while (l--);
-
+		}while(l--);
+
+		delete Mx;
 		return 0;
 	}
 	/*}}}*/
+
+	BamgQuadtree::BamgQuadtreeBox* BamgQuadtree::NewBamgQuadtreeBox(void){/*{{{*/
+
+		/*Output*/
+		BamgQuadtreeBox* newbox=NULL;
+
+		/*Create and initialize a new box*/
+		newbox=new BamgQuadtreeBox;
+		newbox->nbitems=0;
+		newbox->box[0]=NULL;
+		newbox->box[1]=NULL;
+		newbox->box[2]=NULL;
+		newbox->box[3]=NULL;
+
+		/*Add root to the container*/
+		boxcontainer->AddObject(newbox);
+		this->NbQuadtreeBox++;
+
+		/*currentbox now points toward next quadtree box*/
+		return newbox;
+	}/*}}}*/
 }
