#include "BSH.h"
#include <queue>
#include <list>
#include "ClothSim.h"


void BSH::BuidBSH( objLoader *model, int level, Cloth cloth )
{
	this->model=model;
	this->level=level;

	Sphere BS;
	//Point *P = new Point[model->vertexCount];
      Point *P = new Point[cloth.particleActiveState.size()];
///////////////////////////////////////////////////////////////////////////////////////////////////////
	int k,l=0;
	for (k=0;k<cloth.particleActiveState.size();k=k+276)
	  {  
      	for (;l<69+k;l=l+4)
		{	obj_vector* v=model->vertexList[l];
			P[l].x=v->e[0];
		    P[l].y=v->e[1];
		    P[l].z=v->e[2];		
		}
		l=l-3;		
        l=l+207;
	 }
///////////////////////////////////////////////////////////////////////////////////////////////////////


	/*for (int i=0;i<model->vertexCount;i++)
	{
		obj_vector* v=model->vertexList[i];

		P[i].x=v->e[0];
		P[i].y=v->e[1];
		P[i].z=v->e[2];
	}*/

	BS = MinSphere(P, model->vertexCount);

	root->BS->center=BS.center;
	root->BS->radius=BS.radius;
	root->pere=NULL;
	FaceList *faces=new FaceList;
    //cout<<model->faceCount<<endl;
	for(int i=0; i<model->faceCount; i++)
	{
		obj_face *o = model->faceList[i];
    
		
		obj_vector* v[3];

		for (int j=0;j<3;j++)
		{//cout<< o->vertex_index[j]<<endl;
			v[j]=model->vertexList[o->vertex_index[j]];//v[] contient les indices des vertexes de la face.			
		}
		/*for(int k=0;k<3;i++)
			for(int j=0;j<3;j++)
			{cout<< v[j]->e[0]<<endl;
		    cout<< v[j]->e[1]<<endl;
			cout<< v[j]->e[2]<<endl;}*/
             
		vector3 sum(0,0,0);

		for(int j=0; j<3; j++)
		{
			sum.x+=v[j]->e[0];
			sum.y+=v[j]->e[1];
			sum.z+=v[j]->e[2];
		}

		sum.x/=3;
		sum.y/=3;
		sum.z/=3;

		Face tri;
		tri.center=sum;
		tri.index=i;

		faces->push_back(tri);
	}

	if (level<=1)
	{
		faces->clear();
		delete faces;
		return;
	}

	RecurseBuild(root,faces,2);

	faces->clear();
	delete faces;
}

BSH::BSH()
{
	root=new BSN;
	root->BS=new Sphere();
	output=new BSHList;
	leaffaces= new LeafIndFaces;
}

void BSH::RecurseBuild( BSN *current, FaceList *faces, int l )
{

	float minX, maxX, minY, maxY, minZ, maxZ;
	float sum[3];

	sum[0]=sum[1]=sum[2]=0;

	minX=minY=minZ=1e20;
	maxX=maxY=maxZ=-minX;

	for (int i=0;i<faces->size();i++)
	{
		Face tri=faces->at(i);

		sum[0]+=tri.center.x;
		sum[1]+=tri.center.y;
		sum[2]+=tri.center.z;

		if (tri.center.x<minX)
		{
			minX=tri.center.x;
		}

		if (tri.center.x>maxX)
		{
			maxX=tri.center.x;
		}

		if (tri.center.y<minY)
		{
			minY=tri.center.y;
		}

		if (tri.center.y>maxY)
		{
			maxY=tri.center.y;
		}

		if (tri.center.z<minZ)
		{
			minZ=tri.center.z;
		}

		if (tri.center.z>maxZ)
		{
			maxZ=tri.center.z;
		}
	}

	float span[3];

	span[0]=maxX-minX;
	span[1]=maxY-minY;
	span[2]=maxZ-minZ;

	int axis;

	if (span[0]>span[1])
	{
		if (span[0]>span[2])
		{
			axis=0;
		}else{
			axis=2;
		}
	}else{
		if (span[1]>span[2])
		{
			axis=1;
		}else{
			axis=2;
		}
	}

	sum[axis]/=faces->size();

	FaceList *leftFace=new FaceList;
	FaceList *rightFace=new FaceList;

	for (int i=0;i<faces->size();i++)
	{
		Face tri=faces->at(i);

		bool left=false;

		switch (axis)
		{
		case 0:
			if (sum[axis]<tri.center.x)
			{
				left=true;
			}
			break;

		case 1:
			if (sum[axis]<tri.center.y)
			{
				left=true;
			}
			break;

		case 2:
			if (sum[axis]<tri.center.z)
			{
				left=true;
			}
			break;
		}

		if (left)
		{
			leftFace->push_back(tri);
		}else{
			rightFace->push_back(tri);
		}
	}

	/************************************************************************/
	/* left                                                                 */
	/************************************************************************/

	Sphere lBS;
	Point *lP = new Point[leftFace->size()*3];

	for (int i=0;i<leftFace->size();i++)
	{
		obj_face *o = model->faceList[leftFace->at(i).index];

		obj_vector* v[3];

		for (int j=0;j<3;j++)
		{
			v[j]=model->vertexList[o->vertex_index[j]];
		}

		for (int j=0;j<3;j++)
		{
			lP[i*3+j].x=v[j]->e[0];
			lP[i*3+j].y=v[j]->e[1];
			lP[i*3+j].z=v[j]->e[2];
		}
	}

	lBS = MinSphere(lP, leftFace->size()*3);

	current->left=new BSN;
	current->left->BS=new Sphere();
	current->left->pere=current;
	current->left->BS->center=lBS.center;
	current->left->BS->radius=lBS.radius;

	if (l==level)
	{
		current->left->left=NULL; 
		current->left->right=NULL;
		current->left->mesh=new TriList;

		for (int i=0;i<leftFace->size();i++)
		{
			int ind=leftFace->at(i).index;
			current->left->mesh->push_back(ind);
		}
	}else{
		RecurseBuild(current->left,leftFace,l+1);
	}

	leftFace->clear();

	delete [] lP;
	delete leftFace;

	/************************************************************************/
	/* right                                                                */
	/************************************************************************/

	Sphere rBS;
	Point *rP = new Point[rightFace->size()*3];

	for (int i=0;i<rightFace->size();i++)
	{
		obj_face *o = model->faceList[rightFace->at(i).index];

		obj_vector* v[3];

		for (int j=0;j<3;j++)
		{
			v[j]=model->vertexList[o->vertex_index[j]];
		}

		for (int j=0;j<3;j++)
		{
			rP[i*3+j].x=v[j]->e[0];
			rP[i*3+j].y=v[j]->e[1];
			rP[i*3+j].z=v[j]->e[2];
		}
	}

	rBS = MinSphere(rP, rightFace->size()*3);

	current->right=new BSN;
	current->right->BS=new Sphere();
	current->right->pere=current;
	current->right->BS->center=rBS.center;
	current->right->BS->radius=rBS.radius;

	if (l==level)
	{
		current->right->left=NULL;
		current->right->right=NULL;
		current->right->mesh=new TriList;

		for (int i=0;i<rightFace->size();i++)
		{
			int ind=rightFace->at(i).index;
			current->right->mesh->push_back(ind);
		}
	}else{
		RecurseBuild(current->right,rightFace,l+1);
	}

	rightFace->clear();

	delete [] rP;
	delete rightFace;


}

void BSH::SearchBSH( int l )
{
	if (l>level)
	{
		return;
	}

	detail=l;
	output->clear();
	RecurseSearch(root,1);
}

void BSH::RecurseSearch( BSN *current, int l )
{
	if (detail==l)
	{
		output->push_back(current);
	}else{
		RecurseSearch(current->left,l+1);
		RecurseSearch(current->right,l+1);
	}
}

Vec3 BSH:: minus(const Vec3& v1, const vector3& v2) {
    Vec3 r;
	r.f[0] = v1.f[0] - v2.x;
    r.f[1] = v1.f[1] - v2.y;
    r.f[2] = v1.f[2] - v2.z;
    return r;
}

double BSH:: dotProduct(const Vec3& v1, const Vec3& v2) {
    return v1.f[0] * v2.f[0] + v1.f[1] * v2.f[1] + v1.f[2] * v2.f[2];
}

int BSH:: distanceSquared(const vector3& v1, const Vec3& v2) {
    Vec3 delta = minus(v2, v1);
    return dotProduct(delta, delta);
}

bool BSH:: doesItCollide(Sphere *BS, const Vec3 center,const float radius){
	int rSquared = BS->radius + radius;
    rSquared *= rSquared;
	return distanceSquared(BS->center, center) < rSquared;
}

void BSH:: traverse(Cloth cloth, BSN *current,const Vec3 center,const float radius,int Reslevel,float stepsize){
       queue <BSN*> file;
	 	   int i=-1; int line,col,col1;
		   file.push(current);
		   if(!doesItCollide(current->BS, center, radius))
			   return;
		   while(!file.empty())
	        {
				BSN *node=file.front(); 
				i++;
				file.pop();				
				if(doesItCollide(node->BS, center, radius))
				{
				   if(node->left==NULL&&node->right==NULL)
				   {		
					   leaffaces->push_back(node->mesh);                    
				   }
				   else
					 {  
                      if(node->left!=NULL)					 
					  file.push(node->left);
					  if(node->right!=NULL)
					  file.push(node->right);
					 }  	
			   }
				else
				{
					if(i%2!=0) node->pere->left=NULL;
					else       node->pere->right=NULL;
				}
	        }   
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    TriList *Refine=new TriList;
	TriList *Refine1=new TriList;
	for(int i=0;i<leaffaces->size();i++)
	{
		for(int j=0;j<leaffaces->at(i)->size(); j++){  
			
						 Refine->push_back(cloth.Indexface[leaffaces->at(i)->at(j)]->vertex_index[0]);
						 Refine->push_back(cloth.Indexface[leaffaces->at(i)->at(j)]->vertex_index[1]);
						 Refine->push_back(cloth.Indexface[leaffaces->at(i)->at(j)]->vertex_index[2]);						
	  }
	}
    //here we have to build a function that delete multiple occurence of the same particle
	if(!Refine->empty())
	{
     Refine1->push_back(Refine->at(0));
      int ind; vector<int>::iterator it;
		vector<int>::iterator it1;
		for(it1=Refine->begin();it1<Refine->end();it1++)
		{
			ind=*it1;
			for(it=Refine1->begin();it<Refine1->end()&&(*it)!=ind;it++);		
				if(it>=Refine1->end())
					Refine1->push_back(ind);							
		}		
		//for(int i=0;i<Refine1->size();i++)
          //Refine1->at(i)=Refine1->at(i)*4;
		
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		//Activate the neighboring of particles on collision
		                int l=0;
                       for(int i=0;i<Refine1->size();i++){	
						   //vector<Particle> *RefinedParticles; 						   
						for(l=0;l<cloth.t->size()&& cloth.t->at(l).index!=Refine1->at(i);l++);  
						  if(l<cloth.t->size())
						  { line=col=col1=0;							  
							int j= cloth.t->at(l).x;
							int k= cloth.t->at(l).y;   
                            
							    if(k==0&&j!=0&&j!=(cloth.num_particles_height* (Reslevel+1)-Reslevel)-1)
								{ line=5; col=9; col1=9;   
								  for(int m=0;m<k+5;m++)
							       for(int n=j-4;n<j+5;n++)  
									{//cloth.particleActiveState[cloth.MatResol[m][n]].state=1;
								     cloth.RefinedParticles.push_back(cloth.particleActiveState.at(cloth.MatResol[m][n]));
							        }                   
							     }
								else 
								if(k==(cloth.num_particles_height* (Reslevel+1)-Reslevel)-1&&j!=0&&j!=(cloth.num_particles_height* (Reslevel+1)-Reslevel)-1)
								{ line=5; col=9; col1=9;
								  for(int m=k-4;m<=k;m++)
							       for(int n=j-4;n<j+5;n++)  
									{//cloth.particleActiveState[cloth.MatResol[m][n]].state=1;
								     cloth.RefinedParticles.push_back(cloth.particleActiveState.at(cloth.MatResol[m][n]));
							        }    								  
								}
								else 
								if(j==0&&k!=0&&k!=(cloth.num_particles_height* (Reslevel+1)-Reslevel)-1)
								{ line=9; col=5; col1=5; 
								  for(int m=k-4;m<k+5;m++)
							       for(int n=0;n<j+5;n++)  
									{//cloth.particleActiveState[cloth.MatResol[m][n]].state=1;
								     cloth.RefinedParticles.push_back(cloth.particleActiveState.at(cloth.MatResol[m][n]));
							        }                   
							    }
							    else 
								if(j==(cloth.num_particles_height* (Reslevel+1)-Reslevel)-1&&k!=0&&k!=(cloth.num_particles_height* (Reslevel+1)-Reslevel)-1)
								{  line=9; col=5; col1=5; 
								  for(int m=k-4;m<k+5;m++)
							       for(int n=j-4;n<=j;n++)  
									{//cloth.particleActiveState[cloth.MatResol[m][n]].state=1;
									 
								     cloth.RefinedParticles.push_back(cloth.particleActiveState.at(cloth.MatResol[m][n]));
							        }    								  
								}
								else 
								if(k==0&&j==0)
								{  line=5; col=5; col1=5;
								  for(int m=0;m<k+5;m++)
							       for(int n=0;n<j+5;n++)  
									{//cloth.particleActiveState[cloth.MatResol[m][n]].state=1;
								     cloth.RefinedParticles.push_back(cloth.particleActiveState.at(cloth.MatResol[m][n]));
							        }
								 }
								else 
								if((k==(cloth.num_particles_height* (Reslevel+1)-Reslevel)-1)&&(j==(cloth.num_particles_height* (Reslevel+1)-Reslevel)-1))
								{ line=5; col=5; col1=5; 
								  for(int m=k-4;m<=k;m++)
							       for(int n=j-4;n<=j;n++)  
									{//cloth.particleActiveState[cloth.MatResol[m][n]].state=1;
								     cloth.RefinedParticles.push_back(cloth.particleActiveState.at(cloth.MatResol[m][n]));
							        }    								  
								}
								else
								if(k==0&&j==(cloth.num_particles_height* (Reslevel+1)-Reslevel)-1)
								{ line=5; col=5; col1=5; 
								  for(int m=0;m<k+5;m++)
							       for(int n=j-4;n<=j;n++)  
									{//cloth.particleActiveState[cloth.MatResol[m][n]].state=1;
								     cloth.RefinedParticles.push_back(cloth.particleActiveState.at(cloth.MatResol[m][n]));
							        }                   
							    }
								else 
								if(k==(cloth.num_particles_height* (Reslevel+1)-Reslevel)-1&&j==0)
								{ line=5; col=5; col1=5; 
								  for(int m=k-4;m<=k;m++)
							       for(int n=0;n<j+5;n++)  
									{//cloth.particleActiveState[cloth.MatResol[m][n]].state=1;
								     cloth.RefinedParticles.push_back(cloth.particleActiveState.at(cloth.MatResol[m][n]));
							        }    								  
								}
								else{
									line=9; col=9; col1=9; 
                                 for(int m=k-4;m<k+5;m++)
							      for(int n=j-4;n<j+5;n++)                               
								  {//cloth.particleActiveState[cloth.MatResol[m][n]].state=1;
								    cloth.RefinedParticles.push_back(cloth.particleActiveState.at(cloth.MatResol[m][n]));
								  }	}


								           //connecting neighbors, drawing,,,,etc
	//connectParticles(col,line,col1,cloth);  
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	cloth.constraints1.erase(cloth.constraints1.begin(),cloth.constraints1.end());
	  for(int x=0; x<col; x++)
		{ 
			for(int y=0; y<line; y++)
			{ 
				Particle *p1,*p2,*p3,*p4; 
				/*p1=cloth.getParticle1(x+1,y,col1); p1=&cloth.particleActiveState[p1->particleIndex];cout<<"y1 "<<endl;
				p2=cloth.getParticle1(x,y,col1);   p2=&cloth.particleActiveState[p2->particleIndex];cout<<"y2 "<<endl;
				p3=cloth.getParticle1(x,y+1,col1); p3=&cloth.particleActiveState[p3->particleIndex];cout<<"y3 "<<endl;
				p4=cloth.getParticle1(x+1,y+1,col1); p4=&cloth.particleActiveState[p4->particleIndex];cout<<"y4 "<<endl;*/

				if (x<col-1) {p1=cloth.getParticle1(x+1,y,col1); //p1=&cloth.particleActiveState[p1->particleIndex]; 
				              p2=cloth.getParticle1(x,y,col1);   //p2=&cloth.particleActiveState[p2->particleIndex];
				              cloth.makeConstraint1(p2,p1); }
				if (y<line-1) {p2=cloth.getParticle1(x,y,col1);   //p2=&cloth.particleActiveState[p2->particleIndex];
				               p3=cloth.getParticle1(x,y+1,col1); //p3=&cloth.particleActiveState[p3->particleIndex];
				               cloth.makeConstraint1(p2,p3);}
				if (x<col-1 && y<line-1) {p2=cloth.getParticle1(x,y,col1);   //p2=&cloth.particleActiveState[p2->particleIndex];
				                          p4=cloth.getParticle1(x+1,y+1,col1); //p4=&cloth.particleActiveState[p4->particleIndex];
				                          cloth.makeConstraint1(p2,p4);}
				if (x<col-1 && y<line-1) {p1=cloth.getParticle1(x+1,y,col1); //p1=&cloth.particleActiveState[p1->particleIndex];
				                          p3=cloth.getParticle1(x,y+1,col1); //p3=&cloth.particleActiveState[p3->particleIndex];
					                      cloth.makeConstraint1(p1,p3);}
			}
		}
	// Connecting secondary neighbors with constraints (distance 2 and sqrt(4) in the grid)
		for(int x=0; x<col; x++)
		{
			for(int y=0; y<line; y++)
			{
				Particle *p2,*p5,*p6,*p7; 
				
				/*p2=cloth.getParticle1(x,y,col1);   p2=&cloth.particleActiveState[p2->particleIndex];				
				p5=cloth.getParticle1(x+2,y,col1); p5=&cloth.particleActiveState[p5->particleIndex];
				p6=cloth.getParticle1(x,y+2,col1); p6=&cloth.particleActiveState[p6->particleIndex];
				p7=cloth.getParticle1(x+2,y+2,col1); p7=&cloth.particleActiveState[p7->particleIndex];*/

				

				if (x<col-2) {p2=cloth.getParticle1(x,y,col1);   //p2=&cloth.particleActiveState[p2->particleIndex];
				              p5=cloth.getParticle1(x+2,y,col1); //p5=&cloth.particleActiveState[p5->particleIndex];
				              cloth.makeConstraint1(p2,p5);}
				if (y<line-2) {p2=cloth.getParticle1(x,y,col1);   //p2=&cloth.particleActiveState[p2->particleIndex];
				               p6=cloth.getParticle1(x,y+2,col1); //p6=&cloth.particleActiveState[p6->particleIndex];
					           cloth.makeConstraint1(p2,p6);}
				if (x<col-2 && y<line-2) {p2=cloth.getParticle1(x,y,col1);   //p2=&cloth.particleActiveState[p2->particleIndex];
				                          p7=cloth.getParticle1(x+2,y+2,col1); //p7=&cloth.particleActiveState[p7->particleIndex];
				                          cloth.makeConstraint1(p2,p7);}
				if (x<col-2 && y<line-2) {p5=cloth.getParticle1(x+2,y,col1); //p5=&cloth.particleActiveState[p5->particleIndex];
				                          p6=cloth.getParticle1(x,y+2,col1); //p6=&cloth.particleActiveState[p6->particleIndex];
				                          cloth.makeConstraint1(p5,p6);}
			}
		}  
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	//cout<<"taille constr after connect= "<<cloth.constraints1.size()<<endl;
    cloth.addForce1(Vec3(0,-0.2,0)*stepsize/3); // add gravity each frame, pointing down
    cloth.windForce1(Vec3(0.5,0,0.2)*stepsize/3,col,line,col1); // generate some wind each frame
	cloth.timeStep1(); // calculate the particle positions of the next frame     
    //vector<Constraint>::iterator it; cout<<"taille= "<<cloth.constraints.size()<<endl;	
	//it=cloth.constraints.begin(); cout<<constraintSize<<endl; cout<<"first= "<<cloth.constraints.size()-constraintSize<<endl;
	//it=it+(cloth.constraints.size()-constraintSize);	
	//cloth.constraints.erase(it,cloth.constraints.end()); cout<<"after erase= "<<cloth.constraints.size()<<endl;
    cloth.ballCollision1(center,radius);
   cloth.drawShaded1(col,line,col1);
						  cloth.RefinedParticles.erase(cloth.RefinedParticles.begin(),cloth.RefinedParticles.end());
					         } 	
						}
	}
	delete Refine;
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
delete Refine1;
	}

	/*void BSH:: connectParticles(int col, int line,int col1, Cloth cloth){
           // Connecting immediate neighbor particles with constraints (distance 1 and sqrt(2) in the grid)
		 cloth.constraints1.erase(cloth.constraints1.begin(),cloth.constraints1.end());
		 cout<<"taille constr= "<<cloth.constraints1.size()<<endl;
		//constraintSize=0; 
        for(int x=0; x<col; x++)
		{ 
			for(int y=0; y<line; y++)
			{ 
				Particle *p1,*p2,*p3,*p4; 
				/*p1=cloth.getParticle1(x+1,y,col1); p1=&cloth.particleActiveState[p1->particleIndex];cout<<"y1 "<<endl;
				p2=cloth.getParticle1(x,y,col1);   p2=&cloth.particleActiveState[p2->particleIndex];cout<<"y2 "<<endl;
				p3=cloth.getParticle1(x,y+1,col1); p3=&cloth.particleActiveState[p3->particleIndex];cout<<"y3 "<<endl;
				p4=cloth.getParticle1(x+1,y+1,col1); p4=&cloth.particleActiveState[p4->particleIndex];cout<<"y4 "<<endl;*/

				/*if (x<col-1) {p1=cloth.getParticle1(x+1,y,col1); //p1=&cloth.particleActiveState[p1->particleIndex]; 
				              p2=cloth.getParticle1(x,y,col1);   //p2=&cloth.particleActiveState[p2->particleIndex];
				              cloth.makeConstraint1(p2,p1); }
				if (y<line-1) {p2=cloth.getParticle1(x,y,col1);   //p2=&cloth.particleActiveState[p2->particleIndex];
				               p3=cloth.getParticle1(x,y+1,col1); //p3=&cloth.particleActiveState[p3->particleIndex];
				               cloth.makeConstraint1(p2,p3);}
				if (x<col-1 && y<line-1) {p2=cloth.getParticle1(x,y,col1);   //p2=&cloth.particleActiveState[p2->particleIndex];
				                          p4=cloth.getParticle1(x+1,y+1,col1); //p4=&cloth.particleActiveState[p4->particleIndex];
				                          cloth.makeConstraint1(p2,p4);}
				if (x<col-1 && y<line-1) {p1=cloth.getParticle1(x+1,y,col1); //p1=&cloth.particleActiveState[p1->particleIndex];
				                          p3=cloth.getParticle1(x,y+1,col1); //p3=&cloth.particleActiveState[p3->particleIndex];
					                      cloth.makeConstraint1(p1,p3);}
			}
		}
	// Connecting secondary neighbors with constraints (distance 2 and sqrt(4) in the grid)
		for(int x=0; x<col; x++)
		{
			for(int y=0; y<line; y++)
			{
				Particle *p2,*p5,*p6,*p7; 
				
				/*p2=cloth.getParticle1(x,y,col1);   p2=&cloth.particleActiveState[p2->particleIndex];				
				p5=cloth.getParticle1(x+2,y,col1); p5=&cloth.particleActiveState[p5->particleIndex];
				p6=cloth.getParticle1(x,y+2,col1); p6=&cloth.particleActiveState[p6->particleIndex];
				p7=cloth.getParticle1(x+2,y+2,col1); p7=&cloth.particleActiveState[p7->particleIndex];*/

				

				/*if (x<col-2) {p2=cloth.getParticle1(x,y,col1);   //p2=&cloth.particleActiveState[p2->particleIndex];
				              p5=cloth.getParticle1(x+2,y,col1); //p5=&cloth.particleActiveState[p5->particleIndex];
				              cloth.makeConstraint1(p2,p5);}
				if (y<line-2) {p2=cloth.getParticle1(x,y,col1);   //p2=&cloth.particleActiveState[p2->particleIndex];
				               p6=cloth.getParticle1(x,y+2,col1); //p6=&cloth.particleActiveState[p6->particleIndex];
					           cloth.makeConstraint1(p2,p6);}
				if (x<col-2 && y<line-2) {p2=cloth.getParticle1(x,y,col1);   //p2=&cloth.particleActiveState[p2->particleIndex];
				                          p7=cloth.getParticle1(x+2,y+2,col1); //p7=&cloth.particleActiveState[p7->particleIndex];
				                          cloth.makeConstraint1(p2,p7);}
				if (x<col-2 && y<line-2) {p5=cloth.getParticle1(x+2,y,col1); //p5=&cloth.particleActiveState[p5->particleIndex];
				                          p6=cloth.getParticle1(x,y+2,col1); //p6=&cloth.particleActiveState[p6->particleIndex];
				                          cloth.makeConstraint1(p5,p6);}
			}
		}  
		cout<<"taille constr fin= "<<cloth.constraints1.size()<<endl;
	}*/