class CElement2{
public:
	CVertex2 **vertex;
	CVertex2 **vertexList;
	int vertexSize;
	Double density;
	int type;


	int sn22( int i, int j ){ return i+j*2; }	
	int sn33( int i, int j ){ return i+j*3; }	
	void constructVertexList();
	void reconstruct(){ this->constructVertexList(); this->setR(); }
	void reconstruct( Double centerX, Double centerY, Double angle, Double scale ){ this->constructVertexList(); this->transform( centerX, centerY, angle, scale ); this->setR(); }

	CElement2( int x, int y, Double centerX, Double centerY, Double angle, Double scale );
	CElement2( int x, int y, Double centerX, Double centerY, Double angle, Double scale, CVoxel2 &voxel );
	CElement2( CVertex2 &vertex0, CVertex2 &vertex1, CVertex2 &vertex2, CVertex2 &vertex3 );
	CElement2( CVertex2 &vertex0, CVertex2 &vertex1, CVertex2 &vertex2, CVertex2 &vertex3, bool xm1, bool xm2, bool ym1, bool ym2 );
	CElement2(){ }
	~CElement2(){ for(int i=0; i<type; i++ ) if(vertex[i]) delete vertex[i]; delete vertexList; delete vertex; }
	void deleteVertex( int i ){ delete this->vertex[i]; this->vertex[i] = NULL; }
	void constructMiddlePoints();
	void print(){ for(int i=0;i<type;i++) if(vertex[i]){ printf("%d ",i); vertex[i]->position.print(); } printf("\n"); }
	void scale( Double x, Double y ){ for( int i=0; i<vertexSize; i++ ) vertexList[i]->scale( x, y ); }
	void translate( Double x, Double y ){ for( int i=0; i<vertexSize; i++ ) vertexList[i]->translate( x, y ); }//only for position
	void rotate( Double angle );
	void transform( Double centerX, Double centerY, Double angle, Double scale ){ this->scale( scale, scale ); this->rotate( angle ); this->translate( centerX, centerY ); }
	void setR();
	//for renewal
	void setAcceleration( Double x, Double y ){ for( int i=0; i<vertexSize; i++ ) if( vertexList[i] ) vertexList[i]->acceleration.set( x, y ); }
	void addAcceleration( Double x, Double y ){ for( int i=0; i<vertexSize; i++ ) if( vertexList[i] ) vertexList[i]->acceleration.add( x, y ); }
	void addElasticForce( Double youngsModulus, Double dampingCoefficient );
	void setElasticForce( Double youngsModulus, Double dampingCoefficient ){ this->setAcceleration( 0.0, 0.0 ); this->addElasticForce( youngsModulus, dampingCoefficient ); }
	void renewVelocity( Double timeStep ){ for( int i=0; i<vertexSize; i++ ) if( vertexList[i] ) vertexList[i]->renewVelocity( timeStep ); }
	void renewPosition( Double timeStep ){ for( int i=0; i<vertexSize; i++ ) if( vertexList[i] ) vertexList[i]->renewPosition( timeStep ); }
	void rebound( CObstacle2 *obstacle[], Double restitutionCoefficient, Double frictionCoefficient ){ for( int i=0; i<vertexSize; i++ ) if( vertexList[i] ) vertexList[i]->rebound( obstacle, restitutionCoefficient, frictionCoefficient ); }
	//for drawing
	void draw( unsigned char red, unsigned char green, unsigned char blue );
	void drawPoints( unsigned char red, unsigned char green, unsigned char blue ){ for( int i=0; i<=vertexSize; i++ ) og.DrawCircle( vertexList[i]->position.x,  vertexList[i]->position.y, 0.3, red, green, blue ); }
};

class CElement{
public:
	CVertex **vertex;
	CVertex **vertexList;
	int vertexSize;
	Double density;
	int type;


	int sn222( int i, int j, int k ){ return i+(j+k*2)*2; }
	int sn333( int i, int j, int k ){ return i+(j+k*3)*3; }	
	void constructVertexList();
	void reconstruct(){ this->constructVertexList(); this->setR(); }
	void reconstruct( Double centerX, Double centerY, Double centerZ, Double angle, Double scale ){ this->constructVertexList(); this->transform( centerX, centerY, centerZ, angle, scale ); this->setR(); }

	CElement( int x, int y, int z, Double centerX, Double centerY, Double centerZ, Double angle, Double scale );
	CElement( int x, int y, int z, Double centerX, Double centerY, Double centerZ, Double angle, Double scale, CVoxel &voxel );
	CElement( CVertex &vertex0, CVertex &vertex1, CVertex &vertex2, CVertex &vertex3, CVertex &vertex4, CVertex &vertex5, CVertex &vertex6, CVertex &vertex7 );
	CElement( CVertex &vertex0, CVertex &vertex1, CVertex &vertex2, CVertex &vertex3, CVertex &vertex4, CVertex &vertex5, CVertex &vertex6, CVertex &vertex7,
				   bool mx, bool mX, bool my, bool mY, bool mz, bool mZ, bool mxy, bool mXy, bool mxY, bool mXY, bool myz, bool mYz, bool myZ, bool mYZ, bool mzx, bool mZx, bool mzX, bool mZX );
	CElement(){ }
	~CElement(){ for(int i=0; i<type; i++ ) if(vertex[i]) delete vertex[i]; delete vertexList; delete vertex; }
	void deleteVertex( int i ){ delete this->vertex[i]; this->vertex[i] = NULL; }
	void constructMiddlePoint(int x1, int y1, int z1, int x2, int y2, int z2);
	void constructMiddlePoint(int x1, int y1, int z1, int x2, int y2, int z2, int x3, int y3, int z3, int x4, int y4, int z4);
	void constructMiddlePoints();
	void print(){ for(int i=0;i<type;i++) if(vertex[i]){ printf("%d ",i); vertex[i]->position.print(); } printf("vertexSize=%d\n",vertexSize); }
	void scale( Double x, Double y, Double z ){ for( int i=0; i<vertexSize; i++ ) vertexList[i]->scale( x, y, z ); }
	void translate( Double x, Double y, Double z ){ for( int i=0; i<vertexSize; i++ ) vertexList[i]->translate( x, y, z ); }//only for position
	void rotate( Double angle );
	void transform( Double centerX, Double centerY, Double centerZ, Double angle, Double scale ){ this->scale( scale, scale, scale ); this->rotate( angle ); this->translate( centerX, centerY, centerZ ); }
	void setR();
	//for renewal
	void setAcceleration( Double x, Double y, Double z ){ for( int i=0; i<vertexSize; i++ ) if( vertexList[i] ) vertexList[i]->acceleration.set( x, y, z ); }
	void addAcceleration( Double x, Double y, Double z ){ for( int i=0; i<vertexSize; i++ ) if( vertexList[i] ) vertexList[i]->acceleration.add( x, y, z ); }
	void addElasticForce( Double youngsModulus, Double dampingCoefficient );
	void setElasticForce( Double youngsModulus, Double dampingCoefficient ){ this->setAcceleration( 0.0, 0.0, 0.0 ); this->addElasticForce( youngsModulus, dampingCoefficient ); }
	void renewR();
	void renewVelocity( Double timeStep ){ for( int i=0; i<vertexSize; i++ ) if( vertexList[i] ) vertexList[i]->renewVelocity( timeStep ); }
	void renewPosition( Double timeStep ){ for( int i=0; i<vertexSize; i++ ) if( vertexList[i] ) vertexList[i]->renewPosition( timeStep ); }
	void rebound( CObstacle *obstacle[], Double restitutionCoefficient, Double frictionCoefficient ){ for( int i=0; i<vertexSize; i++ ) if( vertexList[i] ) vertexList[i]->rebound( obstacle, restitutionCoefficient, frictionCoefficient ); }
	//for drawing
	void draw( unsigned char red, unsigned char green, unsigned char blue );
	void drawPoints( unsigned char red, unsigned char green, unsigned char blue ){ }
};

void CElement2::constructVertexList(){
	if( type == 4 ){
		vertexSize = 4;
		vertexList[0] = vertex[0];
		vertexList[1] = vertex[1];
		vertexList[3] = vertex[2];
		vertexList[2] = vertex[3];
	}else{
		vertexSize = 0;
		if( vertex[0] ) vertexList[vertexSize++] = vertex[0];
		else if(vertex[6]) vertexList[vertexSize++] = vertex[4];
		if( vertex[1] ) vertexList[vertexSize++] = vertex[1];
		if( vertex[2] ) vertexList[vertexSize++] = vertex[2];
		else if(vertex[0]) vertexList[vertexSize++] = vertex[4];
		if( vertex[5] ) vertexList[vertexSize++] = vertex[5];
		if( vertex[8] ) vertexList[vertexSize++] = vertex[8];
		else if(vertex[2]) vertexList[vertexSize++] = vertex[4];
		if( vertex[7] ) vertexList[vertexSize++] = vertex[7];
		if( vertex[6] ) vertexList[vertexSize++] = vertex[6];
		else if(vertex[8]) vertexList[vertexSize++] = vertex[4];
		if( vertex[3] ) vertexList[vertexSize++] = vertex[3];
	}
}
void CElement::constructVertexList(){
	vertexSize = 0;
	for(int i=0; i<type; i++ ) if( vertex[i] ) vertexList[vertexSize++] = vertex[i];
}

CElement2::CElement2( int x, int y, Double centerX, Double centerY, Double angle, Double scale ){
	type = 4;
	vertex = new CVertex2 *[4];
	for( int j=0; j<2; j++ ){
	for( int i=0; i<2; i++ ){
		vertex[sn22(i,j)] = new CVertex2( x+Double(i), y+Double(j) );
	}}
	vertexList = new CVertex2 *[4];
	this->reconstruct( centerX, centerY, angle, scale );
}
CElement::CElement( int x, int y, int z, Double centerX, Double centerY, Double centerZ, Double angle, Double scale ){
	type = 8;
	vertex = new CVertex *[8];
	for( int k=0; k<2; k++ ){
	for( int j=0; j<2; j++ ){
	for( int i=0; i<2; i++ ){
		vertex[sn222(i,j,k)] = new CVertex( x+Double(i), y+Double(j), z+Double(k) );
	}}}
	vertexList = new CVertex *[8];
	this->reconstruct( centerX, centerY, centerZ, angle, scale );
}

CElement2::CElement2( int x, int y, Double centerX, Double centerY, Double angle, Double scale, CVoxel2 &voxel ){
	type = 9;
	vertex = new CVertex2 *[9];
	vertex[0] = voxel.density(x  , y  ) ? new CVertex2( Double(x  ), Double(y  ) ) : NULL;
	vertex[2] = voxel.density(x+1, y  ) ? new CVertex2( Double(x+2), Double(y  ) ) : NULL;
	vertex[6] = voxel.density(x  , y+1) ? new CVertex2( Double(x  ), Double(y+2) ) : NULL;
	vertex[8] = voxel.density(x+1, y+1) ? new CVertex2( Double(x+2), Double(y+2) ) : NULL;
	vertex[4] = (voxel.density(x, y)+voxel.density(x+1, y)+voxel.density(x, y+1)+voxel.density(x+1, y+1)) < 4 ? new CVertex2( Double(x+1), Double(y+1) ) : NULL;
	vertex[1] = voxel.density(x, y  )+voxel.density(x+1, y  )==1 || voxel.ifSurface(x,y  ,'y') || voxel.ifSurface(x+1,y  ,'y') ? new CVertex2( Double(x+1), Double(y  ) ) : NULL;
	vertex[7] = voxel.density(x, y+1)+voxel.density(x+1, y+1)==1 || voxel.ifSurface(x,y+1,'Y') || voxel.ifSurface(x+1,y+1,'Y') ? new CVertex2( Double(x+1), Double(y+2) ) : NULL;
	vertex[3] = voxel.density(x  , y)+voxel.density(x  , y+1)==1 || voxel.ifSurface(x  ,y,'x') || voxel.ifSurface(x  ,y+1,'x') ? new CVertex2( Double(x  ), Double(y+1) ) : NULL;
	vertex[5] = voxel.density(x+1, y)+voxel.density(x+1, y+1)==1 || voxel.ifSurface(x+1,y,'X') || voxel.ifSurface(x+1,y+1,'X') ? new CVertex2( Double(x+2), Double(y+1) ) : NULL;

	vertexList = new CVertex2 *[9];
	this->reconstruct( centerX, centerY, angle, scale );
}
CElement::CElement( int x, int y, int z, Double centerX, Double centerY, Double centerZ, Double angle, Double scale, CVoxel &voxel ){
	type = 27;
	vertex = new CVertex *[27];
	int corner = 0;
	for(int k=0; k<2; k++){
	for(int j=0; j<2; j++){
	for(int i=0; i<2; i++){
		vertex[sn333(i*2,j*2,k*2)]  = voxel.density(x+i, y+j,z+k) ? new CVertex( Double(x+i*2), Double(y+j*2), Double(z+k*2) ) : NULL;
		if( voxel.density(x+i, y+j,z+k) ) corner++;
	}}}

	vertex[13] = corner > 0 && corner < 8 ? new CVertex( Double(x+1), Double(y+1), Double(z+1) ) : NULL;

	for(int i=0; i<4; i++){
		int a=i%2;
		int b=i/2;
		int a2=a*2;
		int b2=b*2;
		vertex[sn333(1,a2,b2)]  = voxel.density(x,y+a,z+b)+voxel.density(x+1,y+a,z+b)==1 || voxel.ifSurface(x,y+a,z+b,'y'+('Y'-'y')*a) || voxel.ifSurface(x,y+a,z+b,'z'+('Z'-'z')*b) || voxel.ifSurface(x+1,y+a,z+b,'y'+('Y'-'y')*a) || voxel.ifSurface(x+1,y+a,z+b,'z'+('Z'-'z')*b) ? new CVertex( Double(x+1), Double(y+a2), Double(z+b2) ) : NULL;
		vertex[sn333(b2,1,a2)]  = voxel.density(x+b,y,z+a)+voxel.density(x+b,y+1,z+a)==1 || voxel.ifSurface(x+b,y,z+a,'z'+('Z'-'z')*a) || voxel.ifSurface(x+b,y,z+a,'x'+('X'-'x')*b) || voxel.ifSurface(x+b,y+1,z+a,'z'+('Z'-'z')*a) || voxel.ifSurface(x+b,y+1,z+a,'x'+('X'-'x')*b) ? new CVertex( Double(x+b2), Double(y+1), Double(z+a2) ) : NULL;
		vertex[sn333(a2,b2,1)]  = voxel.density(x+a,y+b,z)+voxel.density(x+a,y+b,z+1)==1 || voxel.ifSurface(x+a,y+b,z,'x'+('X'-'x')*a) || voxel.ifSurface(x+a,y+b,z,'y'+('Y'-'y')*b) || voxel.ifSurface(x+a,y+b,z+1,'x'+('X'-'x')*a) || voxel.ifSurface(x+a,y+b,z+1,'y'+('Y'-'y')*b) ? new CVertex( Double(x+a2), Double(y+b2), Double(z+1) ) : NULL;
	}
	for(int j=0;j<2;j++){
		vertex[sn333(1,1,j*2)]  = (corner=voxel.density(x,y,z+j)+voxel.density(x+1,y,z+j)+voxel.density(x,y+1,z+j)+voxel.density(x+1,y+1,z+j)) > 0 && corner < 4 ? new CVertex( Double(x+1), Double(y+1), Double(z+j) ) : NULL;
		vertex[sn333(j*2,1,1)]  = (corner=voxel.density(x+j,y,z)+voxel.density(x+j,y+1,z)+voxel.density(x+j,y,z+1)+voxel.density(x+j,y+1,z+1)) > 0 && corner < 4 ? new CVertex( Double(x+j), Double(y+1), Double(z+1) ) : NULL;
		vertex[sn333(1,j*2,1)]  = (corner=voxel.density(x,y+j,z)+voxel.density(x,y+j,z+1)+voxel.density(x+1,y+j,z)+voxel.density(x+1,y+j,z+1)) > 0 && corner < 4 ? new CVertex( Double(x+1), Double(y+j), Double(z+1) ) : NULL;
	}
	vertexList = new CVertex *[27];
	this->reconstruct( centerX, centerY, centerZ, angle, scale );
}

CElement2::CElement2( CVertex2 &vertex0, CVertex2 &vertex1, CVertex2 &vertex2, CVertex2 &vertex3 ){
	type = 4;
	vertex = new CVertex2 *[4];
	vertex[0] = new CVertex2( vertex0 );
	vertex[1] = new CVertex2( vertex1 );
	vertex[2] = new CVertex2( vertex2 );
	vertex[3] = new CVertex2( vertex3 );
	vertexList = new CVertex2 *[4];
	this->reconstruct();
}
CElement::CElement( CVertex &vertex0, CVertex &vertex1, CVertex &vertex2, CVertex &vertex3, CVertex &vertex4, CVertex &vertex5, CVertex &vertex6, CVertex &vertex7 ){
	type = 8;
	vertex = new CVertex *[8];
	if(&vertex0!=NULL) vertex[0] = new CVertex( vertex0 ); else vertex[0]=NULL;
	if(&vertex1!=NULL) vertex[1] = new CVertex( vertex1 ); else vertex[1]=NULL;
	if(&vertex2!=NULL) vertex[2] = new CVertex( vertex2 ); else vertex[2]=NULL;
	if(&vertex3!=NULL) vertex[3] = new CVertex( vertex3 ); else vertex[3]=NULL;
	if(&vertex4!=NULL) vertex[4] = new CVertex( vertex4 ); else vertex[4]=NULL;
	if(&vertex5!=NULL) vertex[5] = new CVertex( vertex5 ); else vertex[5]=NULL;
	if(&vertex6!=NULL) vertex[6] = new CVertex( vertex6 ); else vertex[6]=NULL;
	if(&vertex7!=NULL) vertex[7] = new CVertex( vertex7 ); else vertex[7]=NULL;
	vertexList = new CVertex *[8];
	this->reconstruct();
}

CElement2::CElement2( CVertex2 &vertex0, CVertex2 &vertex1, CVertex2 &vertex2, CVertex2 &vertex3, bool xm1, bool xm2, bool ym1, bool ym2 ){
	type = 9;
	vertex = new CVertex2 *[9];
	vertex[0] = new CVertex2( vertex0 );
	vertex[2] = new CVertex2( vertex1 );
	vertex[6] = new CVertex2( vertex2 );
	vertex[8] = new CVertex2( vertex3 );
	vertex[1] = ym1 ? new CVertex2( vertex0, vertex1 ) : NULL;
	vertex[7] = ym2 ? new CVertex2( vertex2, vertex3 ) : NULL;
	vertex[3] = xm1 ? new CVertex2( vertex0, vertex2 ) : NULL;
	vertex[5] = xm2 ? new CVertex2( vertex1, vertex3 ) : NULL;
	vertex[4] = NULL;
	vertexList = new CVertex2 *[9];
	this->reconstruct();
}
CElement::CElement( CVertex &vertex0, CVertex &vertex1, CVertex &vertex2, CVertex &vertex3, CVertex &vertex4, CVertex &vertex5, CVertex &vertex6, CVertex &vertex7,
				   bool mx, bool mX, bool my, bool mY, bool mz, bool mZ, bool mxy, bool mXy, bool mxY, bool mXY, bool myz, bool mYz, bool myZ, bool mYZ, bool mzx, bool mZx, bool mzX, bool mZX ){
	type = 27;
	vertex = new CVertex *[27];
	vertex[0]  = new CVertex( vertex0 );
	vertex[2]  = new CVertex( vertex1 );
	vertex[6]  = new CVertex( vertex2 );
	vertex[8]  = new CVertex( vertex3 );
	vertex[18] = new CVertex( vertex4 );
	vertex[20] = new CVertex( vertex5 );
	vertex[24] = new CVertex( vertex6 );
	vertex[26] = new CVertex( vertex7 );

	vertex[12] = mx ? new CVertex( vertex0, vertex2, vertex4, vertex6 ) : NULL;
	vertex[14] = mX ? new CVertex( vertex1, vertex3, vertex5, vertex7 ) : NULL;
	vertex[10] = my ? new CVertex( vertex0, vertex1, vertex4, vertex5 ) : NULL;
	vertex[16] = mY ? new CVertex( vertex2, vertex3, vertex6, vertex7 ) : NULL;
	vertex[4]  = mz ? new CVertex( vertex0, vertex1, vertex2, vertex3 ) : NULL;
	vertex[22] = mZ ? new CVertex( vertex4, vertex5, vertex6, vertex7 ) : NULL;

	vertex[9]  = mxy ? new CVertex( vertex0, vertex4 ) : NULL;
	vertex[11] = mXy ? new CVertex( vertex1, vertex5 ) : NULL;
	vertex[15] = mxY ? new CVertex( vertex2, vertex6 ) : NULL;
	vertex[17] = mXY ? new CVertex( vertex3, vertex7 ) : NULL;

	vertex[1]  = myz ? new CVertex( vertex0, vertex1 ) : NULL;
	vertex[7]  = mYz ? new CVertex( vertex2, vertex3 ) : NULL;
	vertex[19] = myZ ? new CVertex( vertex4, vertex5 ) : NULL;
	vertex[25] = mYZ ? new CVertex( vertex6, vertex7 ) : NULL;

	vertex[3]  = mzx ? new CVertex( vertex0, vertex2 ) : NULL;
	vertex[21] = mZx ? new CVertex( vertex4, vertex6 ) : NULL;
	vertex[5]  = mzX ? new CVertex( vertex1, vertex3 ) : NULL;
	vertex[23] = mZX ? new CVertex( vertex5, vertex7 ) : NULL;

	vertex[13] = NULL;
	vertexList = new CVertex *[27];
	this->reconstruct();
}

void CElement2::constructMiddlePoints(){
	if( vertex[0] && vertex[2] && vertex[1]==NULL ) vertex[1] = new CVertex2( *vertex[0], *vertex[2] );
	if( vertex[6] && vertex[8] && vertex[7]==NULL ) vertex[7] = new CVertex2( *vertex[6], *vertex[8] );
	if( vertex[0] && vertex[6] && vertex[3]==NULL ) vertex[3] = new CVertex2( *vertex[0], *vertex[6] );
	if( vertex[2] && vertex[8] && vertex[5]==NULL ) vertex[5] = new CVertex2( *vertex[2], *vertex[8] );
	if( vertex[4]==NULL ) vertex[4] = new CVertex2( *vertex[1], *vertex[3], *vertex[5], *vertex[7] );
}
void CElement::constructMiddlePoint(int x1, int y1, int z1, int x2, int y2, int z2){
	int x3 = (x1+x2)/2;
	int y3 = (y1+y2)/2;
	int z3 = (z1+z2)/2;
	if( vertex[sn333(x1,y1,z1)] && vertex[sn333(x2,y2,z2)] && vertex[sn333(x3,y3,z3)]==NULL ) vertex[sn333(x3,y3,z3)] = new CVertex( *vertex[sn333(x1,y1,z1)], *vertex[sn333(x2,y2,z2)] );
}
void CElement::constructMiddlePoint(int x1, int y1, int z1, int x2, int y2, int z2, int x3, int y3, int z3, int x4, int y4, int z4){
	int x5 = (x1+x2+x3+x4)/4;
	int y5 = (y1+y2+y3+y4)/4;
	int z5 = (z1+z2+z3+z4)/4;
	if( vertex[sn333(x1,y1,z1)] && vertex[sn333(x2,y2,z2)] && vertex[sn333(x3,y3,z3)] && vertex[sn333(x4,y4,z4)] && vertex[sn333(x5,y5,z5)]==NULL ) vertex[sn333(x5,y5,z5)] = new CVertex( *vertex[sn333(x1,y1,z1)], *vertex[sn333(x2,y2,z2)], *vertex[sn333(x3,y3,z3)], *vertex[sn333(x4,y4,z4)] );
}
void CElement::constructMiddlePoints(){
	for(int i=0; i<4; i++){
		int a=i%2*2;
		int b=i/2*2;
		this->constructMiddlePoint(0,a,b, 2,a,b);
		this->constructMiddlePoint(b,0,a, b,2,a);
		this->constructMiddlePoint(a,b,0, a,b,2);
	}
	for(int j=0;j<3;j+=2){
		this->constructMiddlePoint(0,0,j, 2,0,j, 0,2,j, 2,2,j);
		this->constructMiddlePoint(j,0,0, j,2,0, j,0,2, j,2,2);
		this->constructMiddlePoint(0,j,0, 0,j,2, 2,j,0, 2,j,2);
	}
	if( vertex[sn333(1,1,1)]==NULL ) vertex[sn333(1,1,1)] = new CVertex( *vertex[sn333(1,1,0)], *vertex[sn333(1,1,2)], *vertex[sn333(1,0,1)], *vertex[sn333(1,2,1)], *vertex[sn333(0,1,1)], *vertex[sn333(2,1,1)] );
}

void CElement2::rotate( Double angle ){
	Double sn = sin( rad(angle) );
	Double cs = cos( rad(angle) );
	ogMatrix2 matrix( cs, -sn, sn, cs );
	for( int i=0; i<vertexSize; i++ ) vertexList[i]->rotate( matrix );
}
void CElement::rotate( Double angle ){
	Double sn = sin( rad(angle) );
	Double cs = cos( rad(angle) );
	ogMatrix3 matrix( cs, 0.0, -sn, 0.0, 1.0, 0.0, sn, 0.0, cs );
	for( int i=0; i<vertexSize; i++ ) vertexList[i]->rotate( matrix );
}

void CElement2::setR(){
	ogVector2 center = vertexList[0]->positionOrigin;
	int i;
	for( i=1; i<vertexSize; i++ ) center += vertexList[i]->positionOrigin;
	center.multiply( 1.0 / vertexSize );
	double averageR = 0.0;
	for( i=0; i<vertexSize; i++ ){
		ogVector2 v;
		vertexList[i]->setR( v = vertexList[i]->positionOrigin - center );
		averageR += (vertexList[i]->positionOrigin - center).abs();
	}
	averageR /= vertexSize;
	density = 1.0/averageR/averageR;
}
void CElement::setR(){
	ogVector center = vertexList[0]->positionOrigin;
	int i;
	for( i=1; i<vertexSize; i++ ) center += vertexList[i]->positionOrigin;
	center.multiply( 1.0 / vertexSize );
	double averageR = 0.0;
	for( i=0; i<vertexSize; i++ ){
		ogVector v;
		vertexList[i]->setR( v = vertexList[i]->positionOrigin - center );
		averageR += (vertexList[i]->positionOrigin - center).abs();
	}
	averageR /= vertexSize;
	density = 1.0/averageR/averageR;
}

void CElement2::addElasticForce( Double youngsModulus, Double dampingCoefficient ){
	ogVector2 center = vertexList[0]->position;
	ogVector2 centerVelocity = vertexList[0]->velocity;
	int i;
	for( i=1; i<vertexSize; i++ ){
		center.add( vertexList[i]->position );
		centerVelocity.add( vertexList[i]->velocity );
	}
	center.multiply( 1.0 / vertexSize );
	centerVelocity.multiply( 1.0 / vertexSize );
	for( i=0; i<vertexSize; i++ ) vertexList[i]->r.subtract( vertexList[i]->position, center );

	ogVector2 r[32], Ro[32];
	for( i=0; i<vertexSize; i++ ){
		Ro[i] = vertexList[i]->R;
		r[i] = vertexList[i]->r;
	}
	Double cs = Ro[0].x * r[0].x + Ro[0].y * r[0].y;
	Double sn = Ro[0].x * r[0].y - Ro[0].y * r[0].x;
	for( i=1; i<vertexSize; i++ ){
		cs += Ro[i].x * r[i].x + Ro[i].y * r[i].y;
		sn += Ro[i].x * r[i].y - Ro[i].y * r[i].x;
	}
	Double l = sqrt( cs*cs + sn*sn );
	if( l < 0.000001 ){
		printf("Error in renewR()!\n");
		return;
	}
	cs /= l;
	sn /= l;
	ogMatrix2 matrix( cs, -sn, sn, cs );
	for( i=0; i<vertexSize; i++ ){
		vertexList[i]->R = matrix * Ro[i];
	}

	Double squreErrors = 0.0;
	for( i=0; i<vertexSize; i++ ) squreErrors += vertexList[i]->addElasticForce( youngsModulus*density, dampingCoefficient, centerVelocity );
}
void CElement::addElasticForce( Double youngsModulus, Double dampingCoefficient ){
	ogVector center = vertexList[0]->position;
	ogVector centerVelocity = vertexList[0]->velocity;
	int i;
	for( i=1; i<vertexSize; i++ ){
		center.add( vertexList[i]->position );
		centerVelocity.add( vertexList[i]->velocity );
	}
	center.multiply( 1.0 / vertexSize );
	centerVelocity.multiply( 1.0 / vertexSize );
	for( i=0; i<vertexSize; i++ ) vertexList[i]->r.subtract( vertexList[i]->position, center );


	for( i=0; i<2; i++ ) this->renewR();

	Double squreErrors = 0.0;
	for( i=0; i<vertexSize; i++ ) squreErrors += vertexList[i]->addElasticForce( youngsModulus*density, dampingCoefficient, centerVelocity );
}

void CElement::renewR(){
	ogVector r[32], Ro[32];
	int i;
	for( i=0; i<vertexSize; i++ ){
		Ro[i] = vertexList[i]->R;
		r[i] = vertexList[i]->r;
	}
	//calclate u, theta
	Double Sxx = Ro[0].x * r[0].x;
	Double Syy = Ro[0].y * r[0].y;
	Double Szz = Ro[0].z * r[0].z;
	Double Syz = Ro[0].y * r[0].z;
	Double Szy = Ro[0].z * r[0].y;
	Double Szx = Ro[0].z * r[0].x;
	Double Sxz = Ro[0].x * r[0].z;
	Double Sxy = Ro[0].x * r[0].y;
	Double Syx = Ro[0].y * r[0].x;
	for( i=1; i<vertexSize; i++ ){
		Sxx += Ro[i].x * r[i].x;
		Syy += Ro[i].y * r[i].y;
		Szz += Ro[i].z * r[i].z;
		Syz += Ro[i].y * r[i].z;
		Szy += Ro[i].z * r[i].y;
		Szx += Ro[i].z * r[i].x;
		Sxz += Ro[i].x * r[i].z;
		Sxy += Ro[i].x * r[i].y;
		Syx += Ro[i].y * r[i].x;
	}
	Double Sxxyy = Sxx + Syy;
	Double Syyzz = Syy + Szz;
	Double Szzxx = Szz + Sxx;
	ogMatrix3 matrix( Sxxyy*Szzxx-Syz*Szy, Sxxyy*Sxy  +Sxz*Szy, Szzxx*Sxz  +Sxy*Syz,
				Sxxyy*Syx  +Syz*Szx, Syyzz*Sxxyy-Szx*Sxz, Syyzz*Syz  +Syx*Sxz,
				Szzxx*Szx  +Szy*Syx, Syyzz*Szy  +Szx*Sxy, Szzxx*Syyzz-Sxy*Syx );
	ogVector ext( Syz - Szy, Szx - Sxz, Sxy - Syx );
	ogVector u = matrix * ext;
	Double xx, yy, zz, l, l2;
	l = sqrt( l2 = (xx=u.x*u.x) + (yy=u.y*u.y) + (zz=u.z*u.z) );
	if( l < 0.1 ) return;
/***** Better x,y,z *******/

	Double xy = u.x*u.y;
	Double yz = u.y*u.z;
	Double zx = u.z*u.x;
	Double a, b, c;
	if( fabs(u.x) > fabs(u.y) && fabs(u.x) > fabs(u.z) ){
		a = xy*Sxz-zx*Sxy + yz*(Szz-Syy) + (yy-l2)*Syz - (zz-l2)*Szy;
		b = (u.y*Sxy + u.z*Sxz - u.x*Syyzz) * l;
		c = ext.x * l2;
	}else if( fabs(u.y) > fabs(u.z) ){
		a = yz*Syx-xy*Syz + zx*(Sxx-Szz) + (zz-l2)*Szx - (xx-l2)*Sxz;
		b = (u.z*Syz + u.x*Syx - u.y*Szzxx) * l;
		c = ext.y * l2;
	}else{
		a = zx*Szy-yz*Szx + xy*(Syy-Sxx) + (xx-l2)*Sxy - (yy-l2)*Syx;
		b = (u.x*Szx + u.y*Szy - u.z*Sxxyy) * l;
		c = ext.z * l2;
	}
	Double sn, cs;
/***** Simple sn,cs *******
	sn = sin(-c/b);
	cs = sqrt(1.0-sn*sn);
***** Simple sn,cs *******/
/***** Better sn,cs *******/
	Double d = sqrt(b*b-2*a*c-c*c);
	if( b > 0.0 ){
		sn = -b*(a+c)+a*d;
		cs =  a*(a+c)+b*d;
	}else{
		sn = -b*(a+c)-a*d;
		cs =  a*(a+c)-b*d;
	}
	d = a*a+b*b;
	sn /= d;
	cs /= d;
/***** Better sn,cs *******/
	cs = 1.0-cs;
	matrix.e[0] = (xx/l2-1.0)*cs + 1.0;
	matrix.e[4] = (yy/l2-1.0)*cs + 1.0;
	matrix.e[8] = (zz/l2-1.0)*cs + 1.0;
	cs /= l2;
	sn /= l;
	xy *= cs;	yz *= cs;	zx *= cs;
	u.x *= sn;	u.y *= sn;	u.z *= sn;
	matrix.e[1] = xy - u.z;	matrix.e[5] = yz - u.x;	matrix.e[6]= zx - u.y;
	matrix.e[3] = xy + u.z;	matrix.e[7] = yz + u.x;	matrix.e[2] = zx + u.y;
	for( i=0; i<vertexSize; i++ ){
		vertexList[i]->R = matrix * Ro[i];
	}
}

void CElement2::draw( unsigned char red, unsigned char green, unsigned char blue ){
	ogVFxyzc vertex[10];
	for( int i=0; i<=vertexSize; i++ ){
		vertex[i].set( vertexList[i%vertexSize]->position.x,  vertexList[i%vertexSize]->position.y );
//		vertex[i].setColor(red, green, blue );
		type == 4 ? vertex[i].setColor(255,255,255 ) : vertex[i].setColor(255, 0, 0 );
	}
	ogDrawLineStrip( vertex, vertexSize+1 );
}
void CElement::draw( unsigned char red, unsigned char green, unsigned char blue ){
	if(type != 8 ) return;

	ogVector c(0,0,0);
	int i;
	for( i=0; i<8; i++ ){
			c += vertex[i]->position;
	}
	c *= 0.125;
	ogVFxyznc v[36];
	ogVector n;
	v[0].set( vertex[0]->position );
	v[1].set( vertex[1]->position );
	v[2].set( vertex[2]->position );
	v[3].set( vertex[3]->position );
	v[4].set( vertex[6]->position );
	v[5].set( vertex[7]->position );
	v[6].set( vertex[4]->position );
	v[7].set( vertex[5]->position );
	v[8].set( vertex[3]->position );
	v[9].set( vertex[7]->position );
	v[10].set( vertex[1]->position );
	v[11].set( vertex[5]->position );
	v[12].set( vertex[0]->position );
	v[13].set( vertex[4]->position );
	v[14].set( vertex[2]->position );
	v[15].set( vertex[6]->position );
	n = vertex[0]->position - c; n.normalize(); v[0].setNormal( n );
	n = vertex[1]->position - c; n.normalize(); v[1].setNormal( n );
	n = vertex[2]->position - c; n.normalize(); v[2].setNormal( n );
	n = vertex[3]->position - c; n.normalize(); v[3].setNormal( n );
	n = vertex[6]->position - c; n.normalize(); v[4].setNormal( n );
	n = vertex[7]->position - c; n.normalize(); v[5].setNormal( n );
	n = vertex[4]->position - c; n.normalize(); v[6].setNormal( n );
	n = vertex[5]->position - c; n.normalize(); v[7].setNormal( n );
	n = vertex[3]->position - c; n.normalize(); v[8].setNormal( n );
	n = vertex[7]->position - c; n.normalize(); v[9].setNormal( n );
	n = vertex[1]->position - c; n.normalize(); v[10].setNormal( n );
	n = vertex[5]->position - c; n.normalize(); v[11].setNormal( n );
	n = vertex[0]->position - c; n.normalize(); v[12].setNormal( n );
	n = vertex[4]->position - c; n.normalize(); v[13].setNormal( n );
	n = vertex[2]->position - c; n.normalize(); v[14].setNormal( n );
	n = vertex[6]->position - c; n.normalize(); v[15].setNormal( n );
	for(i=0;i<16;i++) v[i].setColor(red,green,blue);
}