class CVoxelBody2{
public:
	int xSize;
	int ySize;
	CElement2 **element;
	int elementSize;
	CJoint2 **joint;
	int jointSize;

	int sn( int x, int y ){ return x+xSize*y; }
	int jsn( int x, int y ){ return x+(xSize+1)*y; }
	int sn22( int i, int j ){ return i+j*2; }	
	// for construction
	CVoxelBody2(){ }
	CVoxelBody2( CVoxel2 &voxel, Double centerX, Double centerY, Double angle, Double scale = 1.0 ){ this->construct( voxel, centerX, centerY, angle, scale ); }
	~CVoxelBody2(){ for( int i=0; i<elementSize; i++ ) delete element[i]; delete element; for( int i=0; i<jointSize; i++ ) delete joint[i]; delete joint; }
	void construct( CVoxel2 &voxel, Double centerX, Double centerY, Double angle, Double scale = 1.0 );
	void constructElement( int elementSize ){ element = new CElement2 *[this->elementSize=elementSize]; for( int i=0; i<elementSize; i++ ) element[i] = NULL; }
	void constructJoint(){ joint = new CJoint2 *[jointSize=(xSize+1)*(ySize+1)]; for( int i=0; i<jointSize; i++ ) joint[i] = NULL; }
	void registerJoint( CVertex2 *vertex, int x, int y, int i );
	void registerJoint( CElement2 *element, int x, int y );
	void registerJoint();
	// for renewal
	void setAcceleration( Double x, Double y ){ for( int i=0; i<elementSize; i++ ) if( element[i] ) element[i]->setAcceleration( x, y ); }//0.03
	void addAcceleration( Double x, Double y ){ for( int i=0; i<elementSize; i++ ) if( element[i] ) element[i]->addAcceleration( x, y ); }//0.03
	void setElasticForce( Double youngsModulus, Double dampingCoefficient, int firstElement=0 ){ for( int i=firstElement; i<elementSize; i++ ) if( element[i] ) element[i]->setElasticForce( youngsModulus, dampingCoefficient ); }//0.96
	void adjustAcceleration(){ for( int i=0; i<jointSize; i++ ) if( joint[i] ) joint[i]->adjustAcceleration(); }//0.18
	void disconnect( Double threshold ){ for( int i=0; i<jointSize; i++ ) if( joint[i] ) joint[i]->disconnect( threshold ); }
	void renewVelocity( Double timeStep ){ for( int i=0; i<elementSize; i++ ) if( element[i] ) element[i]->renewVelocity( timeStep ); }
	void renewPosition( Double timeStep ){ for( int i=0; i<elementSize; i++ ) if( element[i] ) element[i]->renewPosition( timeStep ); }
	virtual void addGravityAndRenew( Double gravityConstant, Double timeStep ){ this->addAcceleration(0,-gravityConstant); this->renewVelocity(timeStep); this->renewPosition(timeStep); }
	virtual ogVector2 rebound( CObstacle2 *obstacle[], Double restitutionCoefficient, Double frictionCoefficient, Double timeStep, Double springConstant ){ for( int i=0; i<elementSize; i++ ) if( element[i] ) element[i]->rebound( obstacle, restitutionCoefficient, frictionCoefficient ); ogVector2 v(0.0,0.0); return v; }
	virtual void renew( Double gravityConstant, Double youngsModulus, Double dampingCoefficient, Double timeStep, Double threshold = -1.0 );
	// for drawing
	void draw( unsigned char red, unsigned char green, unsigned char blue ){ for( long i=0; i<elementSize; i++ ) if( element[i] ) element[i]->draw(red, green, blue); }
	void drawPoints( unsigned char red, unsigned char green, unsigned char blue ){ for( long i=0; i<elementSize; i++ ) if( element[i] ) element[i]->drawPoints(red, green, blue); }
	void draw( unsigned char red, unsigned char green, unsigned char blue, unsigned char pointRed, unsigned char pointGreen, unsigned char pointBlue  ){ this->draw( red, green, blue ); this->drawPoints( pointRed, pointGreen, pointBlue); }
};

class CVoxelBody{
public:
	int xSize;
	int ySize;
	int zSize;
	CElement **element;
	int elementSize;
	CJoint **joint;
	int jointSize;

	int sn( int x, int y, int z ){ return x+xSize*(y+ySize*z); }
	int jsn( int x, int y, int z ){ return x+(xSize+1)*(y+(ySize+1)*z); }
	int sn222( int i, int j, int k ){ return i+(j+k*2)*2; }	
	// for construction
	CVoxelBody(){ }
	CVoxelBody( CVoxel &voxel, Double centerX, Double centerY, Double centerZ, Double angle, Double scale = 1.0 ){ this->construct( voxel, centerX, centerY, centerZ, angle, scale ); }
	~CVoxelBody(){ for( int i=0; i<elementSize; i++ ) delete element[i]; delete element; for( int i=0; i<jointSize; i++ ) delete joint[i]; delete joint; }
	void construct( CVoxel &voxel, Double centerX, Double centerY, Double centerZ, Double angle, Double scale = 1.0 );
	void constructElement( int elementSize ){ element = new CElement *[this->elementSize=elementSize]; for( int i=0; i<elementSize; i++ ) element[i] = NULL; }
	void constructJoint(){ joint = new CJoint *[jointSize=(xSize+1)*(ySize+1)*(zSize+1)]; for( int i=0; i<jointSize; i++ ) joint[i] = NULL; }
	void registerJoint( CVertex *vertex, int x, int y, int z, int i );
	void registerJoint( CElement *element, int x, int y, int z );
	void registerJoint();
	// for renewal
	void setAcceleration( Double x, Double y, Double z ){ for( int i=0; i<elementSize; i++ ) if( element[i] ) element[i]->setAcceleration( x, y, z ); }
	void addAcceleration( Double x, Double y, Double z ){ for( int i=0; i<elementSize; i++ ) if( element[i] ) element[i]->addAcceleration( x, y, z ); }
	void setElasticForce( Double youngsModulus, Double dampingCoefficient, int firstElement=0 ){ for( int i=firstElement; i<elementSize; i++ ) if( element[i] ) element[i]->setElasticForce( youngsModulus, dampingCoefficient ); }
	virtual void adjustAcceleration(){ for( int i=0; i<jointSize; i++ ) if( joint[i] ) joint[i]->adjustAcceleration(); }
	void disconnect( Double threshold ){ for( int i=0; i<jointSize; i++ ) if( joint[i] ) joint[i]->disconnect( threshold ); }
	void renewVelocity( Double timeStep ){ for( int i=0; i<elementSize; i++ ) if( element[i] ) element[i]->renewVelocity( timeStep ); }
	void renewPosition( Double timeStep ){ for( int i=0; i<elementSize; i++ ) if( element[i] ) element[i]->renewPosition( timeStep ); }
	virtual void addGravityAndRenew( Double gravityConstant, Double timeStep ){ this->addAcceleration(0,0,-gravityConstant); this->renewVelocity(timeStep); this->renewPosition(timeStep); }
	virtual void rebound( CObstacle *obstacle[], Double restitutionCoefficient, Double frictionCoefficient, Double timeStep, Double springConstant ){ for( int i=0; i<elementSize; i++ ) if( element[i] ) element[i]->rebound( obstacle, restitutionCoefficient, frictionCoefficient ); }
	virtual void renew( Double gravityConstant, Double youngsModulus, Double dampingCoefficient, Double timeStep, Double threshold = -1.0 );
	// for drawing
	void draw( unsigned char red, unsigned char green, unsigned char blue ){ for( long i=0; i<elementSize; i++ ) if( element[i] ) element[i]->draw(red, green, blue); }
};

void CVoxelBody2::construct( CVoxel2 &voxel, Double centerX, Double centerY, Double angle, Double scale ){
	this->constructElement( (xSize = voxel.xSize)*(ySize = voxel.ySize) );

	int elementCount = 0;
	for( int y=0; y<ySize; y++ ){
	for( int x=0; x<xSize; x++ ){
		if( voxel.density( x, y ) ){
			element[sn(x,y)] = new CElement2( x, y, centerX, centerY, angle, scale );
			elementCount++;
		}
	}}
	printf( "elementCount = %d\n", elementCount );

	this->constructJoint();
	this->registerJoint();
}
void CVoxelBody::construct( CVoxel &voxel, Double centerX, Double centerY, Double centerZ, Double angle, Double scale ){
	this->constructElement( (xSize = voxel.xSize)*(ySize = voxel.ySize)*(zSize = voxel.zSize) );

	int elementCount = 0;
	for( int z=0; z<zSize; z++ ){
	for( int y=0; y<ySize; y++ ){
	for( int x=0; x<xSize; x++ ){
		if( voxel.density( x, y, z ) ){
			element[sn(x,y,z)] = new CElement( x, y, z, centerX, centerY, centerZ, angle, scale );
			elementCount++;
		}
	}}}
	printf( "elementCount = %d\n", elementCount );

	this->constructJoint();
	this->registerJoint();
}

void CVoxelBody2::registerJoint( CVertex2 *vertex, int x, int y, int i ){
		if( joint[jsn(x,y)] == NULL ) joint[jsn(x,y)] = new CJoint2();
		joint[jsn(x,y)]->vertex[i] = vertex;
}
void CVoxelBody::registerJoint( CVertex *vertex, int x, int y, int z, int i ){
		if( joint[jsn(x,y,z)] == NULL ) joint[jsn(x,y,z)] = new CJoint();
		joint[jsn(x,y,z)]->vertex[i] = vertex;
}

void CVoxelBody2::registerJoint( CElement2 *element, int x, int y ){
	for( int j=0; j<2; j++ ){
	for( int i=0; i<2; i++ ){
		this->registerJoint( element->vertex[sn22(i,j)], x+i, y+j, 3-sn22(i,j) );
	}}
}
void CVoxelBody::registerJoint( CElement *element, int x, int y, int z ){
	for( int k=0; k<2; k++ ){
	for( int j=0; j<2; j++ ){
	for( int i=0; i<2; i++ ){
		this->registerJoint( element->vertex[sn222(i,j,k)], x+i, y+j, z+k, 7-sn222(i,j,k) );
	}}}
}

void CVoxelBody2::registerJoint(){
	for( int y=0; y<ySize; y++ ){
	for( int x=0; x<xSize; x++ ){
		if( element[sn(x,y)] ) this->registerJoint( element[sn(x,y)], x, y );
	}}
}
void CVoxelBody::registerJoint(){
	for( int z=0; z<zSize; z++ ){
	for( int y=0; y<ySize; y++ ){
	for( int x=0; x<xSize; x++ ){
		if( element[sn(x,y,z)] ) this->registerJoint( element[sn(x,y,z)], x, y, z );
	}}}
}

void CVoxelBody2::renew( Double gravityConstant, Double youngsModulus, Double dampingCoefficient, Double timeStep, Double threshold ){
	this->setElasticForce( youngsModulus, dampingCoefficient );
	if( threshold > 0.0 ) this->disconnect( threshold );
	this->adjustAcceleration();
	this->addGravityAndRenew(gravityConstant, timeStep);
}
void CVoxelBody::renew( Double gravityConstant, Double youngsModulus, Double dampingCoefficient, Double timeStep, Double threshold ){
	this->setElasticForce( youngsModulus, dampingCoefficient );
	if( threshold > 0.0 ) this->disconnect( threshold );
	this->adjustAcceleration();
	this->addGravityAndRenew(gravityConstant, timeStep);
}
//-------------------------------------------------------------------------------------------------------------------

class CVoxelBodyF2: public CVoxelBody2{// Accelerated version derived from the original version for the voxel-based elastic model
public:                                // Some member functions are replaced into the accelerated ones.
	// for renewal
	void setElasticForce( Double youngsModulus, Double dampingCoefficient, int firstElement=0 );//0.055
	void adjustAcceleration();//0.18->0.016
	void addGravityAndRenew( Double gravityConstant, Double timeStep );//0.01
	ogVector2 rebound( CObstacle2 *obstacle[], Double restitutionCoefficient, Double frictionCoefficient, Double timeStep, Double springConstant );
};
class CVoxelBodyF: public CVoxelBody{// Accelerated version derived from the original version for the voxel-based elastic model
public:                                // Some member functions are replaced into the accelerated ones.
	// for renewal
	void setElasticForce( Double youngsModulus, Double dampingCoefficient, int firstElement=0 );
	void adjustAcceleration();
	void addGravityAndRenew( Double gravityConstant, Double timeStep );
	void rebound( CObstacle *obstacle[], Double restitutionCoefficient, Double frictionCoefficient, Double timeStep, Double springConstant );
};

void CVoxelBodyF2::setElasticForce( Double youngsModulus, Double dampingCoefficient, int firstElement ){//0.055
	Double rVertexSize;
	Double rx[16], ry[16], Rox[16], Roy[16];
	for( int j=firstElement; j<elementSize; j++ ){
		if( element[j] ){
			int vertexSize = element[j]->vertexSize;
			CVertex2 *v = element[j]->vertexList[0];
			Double cx = v->position.x;
			Double cy = v->position.y;
			Double cvx = v->velocity.x;
			Double cvy = v->velocity.y;
			int i;
			for( i=1; i<vertexSize; i++ ){
				v = element[j]->vertexList[i];
				cx += v->position.x;
				cy += v->position.y;
				cvx += v->velocity.x;
				cvy += v->velocity.y;
			}
			rVertexSize = vertexSize < 9 ?
				vertexSize < 5 ? vertexSize<3?(vertexSize==1?1.0:0.5):(vertexSize==3?0.333333333:0.25) : vertexSize<7?(vertexSize==5?0.2:0.1666666666):(vertexSize==7?0.142857142857:0.125)
				: vertexSize < 13 ? vertexSize<11?(vertexSize==9?0.111111111:0.1):(vertexSize==11?0.090909090909:0.0833333333333) : vertexSize<15?(vertexSize==13?0.076923076923:0.0714285714285):(vertexSize==15?0.066666666666:0.0625);
//				cx /= vertexSize;
//				cy /= vertexSize;
//				cvx /= vertexSize;
//				cvy /= vertexSize;
			cx *= rVertexSize;
			cy *= rVertexSize;
			cvx *= rVertexSize;
			cvy *= rVertexSize;
			for( i=0; i<vertexSize; i++ ){
				v = element[j]->vertexList[i];
				rx[i] = v->position.x - cx;
				ry[i] = v->position.y - cy;
				Rox[i] = v->R.x;
				Roy[i] = v->R.y;
			}
			Double cs = Rox[0] * rx[0] + Roy[0] * ry[0];
			Double sn = Rox[0] * ry[0] - Roy[0] * rx[0];
			for( i=1; i<vertexSize; i++ ){
				cs += Rox[i] * rx[i] + Roy[i] * ry[i];
				sn += Rox[i] * ry[i] - Roy[i] * rx[i];
			}
			Double l = sqrt( cs*cs + sn*sn );
			if( l < 0.000001 ){
				printf("Error in renewR()!\n");
				return;
			}
			l = 1.0/l;
			cs *= l;
			sn *= l;
			for( i=0; i<vertexSize; i++ ){
				v = element[j]->vertexList[i];
				v->R.x = cs * Rox[i] - sn *Roy[i];
				v->R.y = sn * Rox[i] + cs *Roy[i];
			}

			Double springConstant = youngsModulus*element[j]->density;
			for( i=0; i<vertexSize; i++ ){
				v = element[j]->vertexList[i];
				v->acceleration.x = -springConstant*(rx[i]-v->R.x) -dampingCoefficient*(v->velocity.x-cvx);
				v->acceleration.y = -springConstant*(ry[i]-v->R.y) -dampingCoefficient*(v->velocity.y-cvy);
			}
		}
	}
}
void CVoxelBodyF::setElasticForce( Double youngsModulus, Double dampingCoefficient, int firstElement ){
	Double rx[32], ry[32], rz[32], Rox[32], Roy[32], Roz[32];
	CVertex *v;
	for( int j=firstElement; j<elementSize; j++ ){
		if( element[j] ){
			int vertexSize = element[j]->vertexSize;
			Double rVertexSize = 1.0/vertexSize;
			v = element[j]->vertexList[0];
			Double cx = v->position.x;
			Double cy = v->position.y;
			Double cz = v->position.z;
			Double cvx = v->velocity.x;
			Double cvy = v->velocity.y;
			Double cvz = v->velocity.z;
			int i;
			for( i=1; i<vertexSize; i++ ){
				v = element[j]->vertexList[i];
				cx += v->position.x;
				cy += v->position.y;
				cz += v->position.z;
				cvx += v->velocity.x;
				cvy += v->velocity.y;
				cvz += v->velocity.z;
			}
			cx *= rVertexSize;
			cy *= rVertexSize;
			cz *= rVertexSize;
			cvx *= rVertexSize;
			cvy *= rVertexSize;
			cvz *= rVertexSize;
			for( i=0; i<vertexSize; i++ ){
				v = element[j]->vertexList[i];
				rx[i] = v->position.x - cx;
				ry[i] = v->position.y - cy;
				rz[i] = v->position.z - cz;
			}


			for( int round=0; round<2; round++ ){
				for( i=0; i<vertexSize; i++ ){
					v = element[j]->vertexList[i];
					Rox[i] = v->R.x;
					Roy[i] = v->R.y;
					Roz[i] = v->R.z;
				}
				//calclate u, theta
				Double Sxx = Rox[0] * rx[0];
				Double Syy = Roy[0] * ry[0];
				Double Szz = Roz[0] * rz[0];
				Double Syz = Roy[0] * rz[0];
				Double Szy = Roz[0] * ry[0];
				Double Szx = Roz[0] * rx[0];
				Double Sxz = Rox[0] * rz[0];
				Double Sxy = Rox[0] * ry[0];
				Double Syx = Roy[0] * rx[0];
				for( i=1; i<vertexSize; i++ ){
					Sxx += Rox[i] * rx[i];
					Syy += Roy[i] * ry[i];
					Szz += Roz[i] * rz[i];
					Syz += Roy[i] * rz[i];
					Szy += Roz[i] * ry[i];
					Szx += Roz[i] * rx[i];
					Sxz += Rox[i] * rz[i];
					Sxy += Rox[i] * ry[i];
					Syx += Roy[i] * rx[i];
				}
				Double Sxxyy = Sxx + Syy;
				Double Syyzz = Syy + Szz;
				Double Szzxx = Szz + Sxx;
				Double m00 = Sxxyy*Szzxx-Syz*Szy;
				Double m01 = Sxxyy*Sxy  +Sxz*Szy;
				Double m02 = Szzxx*Sxz  +Sxy*Syz;
				Double m10 = Sxxyy*Syx  +Syz*Szx;
				Double m11 = Syyzz*Sxxyy-Szx*Sxz;
				Double m12 = Syyzz*Syz  +Syx*Sxz;
				Double m20 = Szzxx*Szx  +Szy*Syx;
				Double m21 = Syyzz*Szy  +Szx*Sxy;
				Double m22 = Szzxx*Syyzz-Sxy*Syx;
				Double ex = Syz - Szy;
				Double ey = Szx - Sxz;
				Double ez = Sxy - Syx;
				Double ux = m00*ex + m01*ey + m02*ez;
				Double uy = m10*ex + m11*ey + m12*ez;
				Double uz = m20*ex + m21*ey + m22*ez;
				Double xx, yy, zz, l, l2;
				l = sqrt( l2 = (xx=ux*ux) + (yy=uy*uy) + (zz=uz*uz) );
				if( l < 0.1 ) break;

				Double xy = ux*uy;
				Double yz = uy*uz;
				Double zx = uz*ux;
				Double a, b, c;
				if( fabs(ux) > fabs(uy) && fabs(ux) > fabs(uz) ){
					//a = xy*Sxz-zx*Sxy + yz*(Szz-Syy) + (yy-l2)*Syz - (zz-l2)*Szy;
					b = (uy*Sxy + uz*Sxz - ux*Syyzz) * l;
					c = ex * l2;
				}else if( fabs(uy) > fabs(uz) ){
					//a = yz*Syx-xy*Syz + zx*(Sxx-Szz) + (zz-l2)*Szx - (xx-l2)*Sxz;
					b = (uz*Syz + ux*Syx - uy*Szzxx) * l;
					c = ey * l2;
				}else{
					//a = zx*Szy-yz*Szx + xy*(Syy-Sxx) + (xx-l2)*Sxy - (yy-l2)*Syx;
					b = (ux*Szx + uy*Szy - uz*Sxxyy) * l;
					c = ez * 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 = 1.0/a*a+b*b;
				sn *= d;
				cs *= d;
			***** Better sn,cs *******/
				cs = 1.0-cs;
				m00 = (xx/l2-1.0)*cs + 1.0;
				m11 = (yy/l2-1.0)*cs + 1.0;
				m22 = (zz/l2-1.0)*cs + 1.0;
				cs /= l2;
				sn /= l;
				xy *= cs;	yz *= cs;	zx *= cs;
				ux *= sn;	uy *= sn;	uz *= sn;
				m01 = xy - uz;	m12 = yz - ux;	m20 = zx - uy;
				m10 = xy + uz;	m21 = yz + ux;	m02 = zx + uy;
			/*	u.print();
				Double uAbs = u.abs();
				if( uAbs < 0.1 ) return;
				u /= uAbs;

				matrix.assign( Syyzz, -Sxy,  -Sxz,
						-Syx,  Szzxx, -Syz,
						-Szx,  -Szy,  Sxxyy );
				CVector a = matrix * u;
				Double theta = fabs(a.x) > fabs(a.z) && fabs(a.x) > fabs(a.z) ? ext.x/a.x : fabs(a.y) && fabs(a.z) ? ext.y/a.y : ext.z/a.z;
				u *= theta;

				matrix.assign( 1.0, -u.z, u.y, u.z, 1.0, -u.x, -u.y, u.x, 1.0 );*/
				for( i=0; i<vertexSize; i++ ){
					v = element[j]->vertexList[i];
					v->R.x = m00*Rox[i] + m01*Roy[i] + m02*Roz[i];
					v->R.y = m10*Rox[i] + m11*Roy[i] + m12*Roz[i];
					v->R.z = m20*Rox[i] + m21*Roy[i] + m22*Roz[i];
				}
			}

			Double springConstant = youngsModulus*element[j]->density;
			for( i=0; i<vertexSize; i++ ){
				v = element[j]->vertexList[i];
				v->acceleration.x = -springConstant*(rx[i]-v->R.x) -dampingCoefficient*(v->velocity.x-cvx);
				v->acceleration.y = -springConstant*(ry[i]-v->R.y) -dampingCoefficient*(v->velocity.y-cvy);
				v->acceleration.z = -springConstant*(rz[i]-v->R.z) -dampingCoefficient*(v->velocity.z-cvz);
			}
		}
	}
}

void CVoxelBodyF2::adjustAcceleration(){//0.18->0.016
	for( int i=0; i<jointSize; i++ ){
		if( joint[i] && !joint[i]->disconnected ){
			Double x=0.0;
			Double y=0.0;
			int count = 0;
			int j;
			for( j=0; j<4; j++ ){
				if( CVertex2 *v=joint[i]->vertex[j] ){
					x += v->acceleration.x;
					y += v->acceleration.y;
					count++;
				}
			}
			Double rCount = count<3?(count==1?1.0:0.5):(count==3?0.333333333:0.25);
//				x /= count;
//				y /= count;
			x *= rCount;// no accelerated
			y *= rCount;
			for( j=0; j<4; j++ ) if( CVertex2 *v=joint[i]->vertex[j] ){ v->acceleration.x = x; v->acceleration.y = y; }
		}
	}
}
void CVoxelBodyF::adjustAcceleration(){
	for( int i=0; i<jointSize; i++ ){
		if( joint[i] && !joint[i]->disconnected ){
			Double x=0.0;
			Double y=0.0;
			Double z=0.0;
			int count = 0;
			int j;
			for( j=0; j<8; j++ ){
				if( CVertex *v=joint[i]->vertex[j] ){
					x += v->acceleration.x;
					y += v->acceleration.y;
					z += v->acceleration.z;
					count++;
				}
			}
			Double rCount = count<5?(count<3?(count==1?1.0:0.5):(count==3?0.333333333:0.25)) : (count<7?(count==5?0.2:1.6666666667):(count==7?0.142857143:0.125));
//				x /= count;
//				y /= count;
			x *= rCount;// no accelerated
			y *= rCount;
			z *= rCount;
			for( j=0; j<8; j++ ) if( CVertex *v=joint[i]->vertex[j] ){ v->acceleration.x = x; v->acceleration.y = y; v->acceleration.z = z; }
		}
	}
}

void CVoxelBodyF2::addGravityAndRenew( Double gravityConstant, Double timeStep ){//0.01
//		aligned float a[4];
//		aligned float b[4];
//		aligned float t[4] = { timeStep, timeStep, timeStep, timeStep };
	for( int i=0; i<elementSize; i++ ){
		if( element[i] ){
			for( int j=0; j<element[i]->vertexSize; j++ ){
				if( CVertex2 *v=element[i]->vertexList[j] ){
					v->acceleration.y -= gravityConstant;
					v->velocity.x += timeStep * v->acceleration.x;
					v->velocity.y += timeStep * v->acceleration.y;
					v->position.x += timeStep * v->velocity.x;
					v->position.y += timeStep * v->velocity.y;
/*						a[0] = v->acceleration.x;
					a[1] = v->acceleration.y-gravityConstant;
					b[0] =a[2] = v->velocity.x;
					b[1] = a[3] = v->velocity.y;
					b[2] = v->position.x;
					b[3] = v->position.y;
					__asm {
						movaps xmm0, [a]
						movaps xmm1, [b]
						mulps xmm0, [t]
						addps xmm1, xmm0
						movaps [a], xmm1
 					}
					v->velocity.x = a[0];
					v->velocity.y = a[1];
					v->position.x = a[2];
					v->position.y = a[3];*/
				}
			}
		}
	}
/*		aligned float t[4] = { timeStep, timeStep, timeStep, timeStep };
	aligned float g[4] = { -gravityConstant, -gravityConstant, -gravityConstant, -gravityConstant };
	aligned float aax[4];
	aligned float ay[4];
	aligned float vx[4];
	aligned float vy[4];
	aligned float px[4];
	aligned float py[4];
	CVertex2 *v[4];
	int k=0;
	for( int i=0; i<elementSize; i++ ){
		if( element[i] ){
			for( int j=0; j<element[i]->vertexSize; j++ ){
				if( v[k]=element[i]->vertexList[j] ){
					aax[k] = v[k]->acceleration.x;
					ay[k] = v[k]->acceleration.y;
					vx[k] = v[k]->velocity.x;
					vy[k] = v[k]->velocity.y;
					k++;
					if(k==4){
						__asm {
							movaps xmm0, [aax]
							movaps xmm1, [ay]
							movaps xmm2, [vx]
							movaps xmm3, [vy]
							addps xmm1, [g]
							mulps xmm0, [t]
							addps xmm2, xmm0
							movaps [vx], xmm2
							mulps xmm1, [t]
							addps xmm3, xmm1
							movaps [vy], xmm3
 							mulps xmm2, [t]
							addps xmm4, xmm2
							movaps [px], xmm4
							mulps xmm3, [t]
							addps xmm5, xmm3
							movaps [py], xmm5
 						}
						v[0]->velocity.x = vx[0];
						v[0]->velocity.y = vy[0];
						v[1]->velocity.x = vx[1];
						v[1]->velocity.y = vy[1];
						v[2]->velocity.x = vx[2];
						v[2]->velocity.y = vy[2];
						v[3]->velocity.x = vx[3];
						v[3]->velocity.y = vy[3];
						v[0]->position.x = px[0];
						v[0]->position.y = py[0];
						v[1]->position.x = px[1];
						v[1]->position.y = py[1];
						v[2]->position.x = px[2];
						v[2]->position.y = py[2];
						v[3]->position.x = px[3];
						v[3]->position.y = py[3];
						k = 0;
					}
				}
			}
		}
	}
	if(k>0){
		__asm {
			movaps xmm0, [aax]
			movaps xmm1, [ay]
			movaps xmm2, [vx]
			movaps xmm3, [vy]
			addps xmm1, [g]
			mulps xmm0, [t]
			addps xmm2, xmm0
			movaps [vx], xmm2
			mulps xmm1, [t]
			addps xmm3, xmm1
			movaps [vy], xmm3
 			mulps xmm2, [t]
			addps xmm4, xmm2
			movaps [px], xmm4
			mulps xmm3, [t]
			addps xmm5, xmm3
			movaps [py], xmm5
 		}
		for(int l=0; l<k; l++){
						v[l]->velocity.x = vx[l];
						v[l]->velocity.y = vy[l];
						v[l]->position.x = px[l];
						v[l]->position.y = py[l];
		}
	}*/
}
void CVoxelBodyF::addGravityAndRenew( Double gravityConstant, Double timeStep ){
	for( int i=0; i<elementSize; i++ ){
		if( element[i] ){
			for( int j=0; j<element[i]->vertexSize; j++ ){
				if( CVertex *v=element[i]->vertexList[j] ){
					v->acceleration.z -= gravityConstant;
					v->velocity.x += timeStep * v->acceleration.x;
					v->velocity.y += timeStep * v->acceleration.y;
					v->velocity.z += timeStep * v->acceleration.z;
					v->position.x += timeStep * v->velocity.x;
					v->position.y += timeStep * v->velocity.y;
					v->position.z += timeStep * v->velocity.z;
				}
			}
		}
	}
}

ogVector2 CVoxelBodyF2::rebound( CObstacle2 *obstacle[], Double restitutionCoefficient, Double frictionCoefficient, Double timeStep, Double springConstant ){
	CRigidRoom2 *room;
	double k = frictionCoefficient * (1.0+restitutionCoefficient);
	double st = springConstant*timeStep;
	ogVector2 force(0.0,0.0);
	for( int i=0; i<elementSize; i++ ){
		if( element[i] ){
			for( int j=0; j<element[i]->vertexSize; j++ ){
				if( CVertex2 *v=element[i]->vertexList[j] ){
					int h=0;
					while(obstacle[h]){
						if(obstacle[h]->type == 'r'){
							room = (CRigidRoom2 *)obstacle[h];
							if(v->position.y<room->min.y && v->velocity.y<0.0){
								double dv = -k * v->velocity.y;//velocity.y<0.0
								if(v->velocity.x > dv ) v->velocity.x -= dv;
								else if(v->velocity.x < -dv ) v->velocity.x += dv;
								else v->velocity.x = 0.0;
									v->velocity.y *= -restitutionCoefficient;
								v->position.y = room->min.y + (room->min.y-v->position.y)*restitutionCoefficient;
								//v->position.y = room->min.y;
							}
							if(v->position.y>room->max.y && v->velocity.y>0.0){
								double dv = k * v->velocity.y;
								if(v->velocity.x > dv ) v->velocity.x -= dv;
								else if(v->velocity.x < -dv ) v->velocity.x += dv;
								else v->velocity.x = 0.0;
									v->velocity.y *= -restitutionCoefficient;
								v->position.y = room->max.y + (room->max.y-v->position.y)*restitutionCoefficient;
								//v->position.y = room->max.y;
							}
							if(v->position.x<room->min.x && v->velocity.x<0.0){
								double dv = -k * v->velocity.x;//velocity.x<0.0
								if(v->velocity.y > dv ) v->velocity.y -= dv;
								else if(v->velocity.y < -dv ) v->velocity.y += dv;
								else v->velocity.y = 0.0;
									v->velocity.x *= -restitutionCoefficient;
								v->position.x = room->min.x + (room->min.x-v->position.x)*restitutionCoefficient;
								//v->position.x = room->min.x;
							}
							if(v->position.x>room->max.x && v->velocity.x>0.0){
								double dv = k * v->velocity.x;
								if(v->velocity.y > dv ) v->velocity.y -= dv;
								else if(v->velocity.y < -dv ) v->velocity.y += dv;
								else v->velocity.y = 0.0;
									v->velocity.x *= -restitutionCoefficient;
								v->position.x = room->max.x + (room->max.x-v->position.x)*restitutionCoefficient;
								//v->position.x = room->max.x;
							}
						}else if(obstacle[h]->type == 'c'){
							CRigidCylinder2 *c = (CRigidCylinder2 *)obstacle[h];
							double nx = v->position.x - c->center.x;
							double ny = v->position.y - c->center.y;
							double nabs2 = nx*nx + ny*ny;
							double vXn = v->velocity.x*nx+v->velocity.y*ny;
							if( nabs2 < c->radiusXradius && vXn<0.0 ){
								double nabs = sqrt(nabs2);
								double rNabs = 1.0/nabs;
								/*************consraint method************/
								double k1 = vXn/nabs2;
								double VYx = k1*nx;
								double VYy = k1*ny;
								double VXx = v->velocity.x - VYx;
								double VXy = v->velocity.y - VYy;
								double dv = k * sqrt(VYx*VYx+VYy*VYy);
								double VXabs = sqrt(VXx*VXx+VXy*VXy);
								double vox = v->velocity.x;
								double voy = v->velocity.y;
								if(VXabs > dv ){
									k1 = 1.0 - dv/VXabs;
									v->velocity.x = VXx * k1;
									v->velocity.y = VXy * k1;
								}else{
									v->velocity.x = 0.0;
									v->velocity.y = 0.0;
								}
								v->velocity.x -= restitutionCoefficient * VYx;
								v->velocity.y -= restitutionCoefficient * VYy;

								k1 = (c->radius-nabs)*(1.0+restitutionCoefficient)*rNabs;
								v->position.x += k1 * nx;
								v->position.y += k1 * ny;
								if(h==0){
									force.x += vox - v->velocity.x;
									force.y += voy - v->velocity.y;
								}
								/*************consraint method************/
								/*************virtual spring method************
								double k1 = rNabs*c->radius;
								double fx = nx - nx*k1;
								double fy = ny - ny*k1;
								v->velocity.x -= fx * st;
								v->velocity.y -= fy * st;
								v->position.x += v->velocity.x * timeStep;
								v->position.y += v->velocity.y * timeStep;
								if(h==0){
									force.x += fx;
									force.y += fy;
								}
								*************virtual spring method************/
							}
						}
						h++;
					}
				}
			}
		}
	}
	return force/timeStep;
}

void CVoxelBodyF::rebound( CObstacle *obstacle[], Double restitutionCoefficient, Double frictionCoefficient, Double timeStep, Double springConstant ){
	CRigidRoom *room;
	double k = frictionCoefficient * (1.0+restitutionCoefficient);
	for( int i=0; i<elementSize; i++ ){
		if( element[i] ){
			for( int j=0; j<element[i]->vertexSize; j++ ){
				if( CVertex *v=element[i]->vertexList[j] ){
					int h=0;
					while(obstacle[h]){
						if(obstacle[h]->type == 'r'){
							room = (CRigidRoom *)obstacle[h];
							if(v->position.y<room->min.y && v->velocity.y<0.0){
								double dv = -k * v->velocity.y;//velocity.y<0.0
								if(v->velocity.x > dv ) v->velocity.x -= dv;
								else if(v->velocity.x < -dv ) v->velocity.x += dv;
								else v->velocity.x = 0.0;
									v->velocity.y *= -restitutionCoefficient;
								v->position.y = room->min.y + (room->min.y-v->position.y)*restitutionCoefficient;
								//v->position.y = room->min.y;
							}
							if(v->position.y>room->max.y && v->velocity.y>0.0){
								double dv = k * v->velocity.y;
								if(v->velocity.x > dv ) v->velocity.x -= dv;
								else if(v->velocity.x < -dv ) v->velocity.x += dv;
								else v->velocity.x = 0.0;
									v->velocity.y *= -restitutionCoefficient;
								v->position.y = room->max.y + (room->max.y-v->position.y)*restitutionCoefficient;
								//v->position.y = room->max.y;
							}
							if(v->position.x<room->min.x && v->velocity.x<0.0){
								double dv = -k * v->velocity.x;//velocity.x<0.0
								if(v->velocity.y > dv ) v->velocity.y -= dv;
								else if(v->velocity.y < -dv ) v->velocity.y += dv;
								else v->velocity.y = 0.0;
									v->velocity.x *= -restitutionCoefficient;
								v->position.x = room->min.x + (room->min.x-v->position.x)*restitutionCoefficient;
								//v->position.x = room->min.x;
							}
							if(v->position.x>room->max.x && v->velocity.x>0.0){
								double dv = k * v->velocity.x;
								if(v->velocity.y > dv ) v->velocity.y -= dv;
								else if(v->velocity.y < -dv ) v->velocity.y += dv;
								else v->velocity.y = 0.0;
									v->velocity.x *= -restitutionCoefficient;
								v->position.x = room->max.x + (room->max.x-v->position.x)*restitutionCoefficient;
								//v->position.x = room->max.x;
							}
							if(v->position.z<room->min.z && v->velocity.z<0.0){
								double dv = -k * v->velocity.z;//velocity.z<0.0
								if(v->velocity.y > dv ) v->velocity.y -= dv;
								else if(v->velocity.y < -dv ) v->velocity.y += dv;
								else v->velocity.y = 0.0;
									v->velocity.z *= -restitutionCoefficient;
								v->position.z = room->min.z + (room->min.z-v->position.z)*restitutionCoefficient;
								//v->position.z = room->min.z;
							}
							if(v->position.z>room->max.z && v->velocity.z>0.0){
								/*double dv = k * v->velocity.x;
								if(v->velocity.y > dv ) v->velocity.y -= dv;
								else if(v->velocity.y < -dv ) v->velocity.y += dv;
								else v->velocity.y = 0.0;
									v->velocity.x *= -restitutionCoefficient;
								v->position.x = room->max.x + (room->max.x-v->position.x)*restitutionCoefficient;*/
								v->position.z = room->max.z;
							}
						}else if(obstacle[h]->type == 'c'){
							CRigidCylinder *c = (CRigidCylinder *)obstacle[h];
							double nx = v->position.x - c->center.x;
							double ny = v->position.z - c->center.z;
							double nabs2 = nx*nx + ny*ny;
							double vXn = v->velocity.x*nx+v->velocity.z*ny;
							if( nabs2 < c->radiusXradius && vXn<0.0 ){
								double k1 = vXn/nabs2;
								double VYx = k1*nx;
								double VYy = k1*ny;
								double VXx = v->velocity.x - VYx;
								double VXy = v->velocity.z - VYy;
								double dv = k * sqrt(VYx*VYx+VYy*VYy);
								double VXabs = sqrt(VXx*VXx+VXy*VXy);
								double nabs = sqrt(nabs2);
								if(VXabs > dv ){
									v->velocity.x = VXx * (1.0 - dv/VXabs );
									v->velocity.z = VXy * (1.0 - dv/VXabs );
								}else{
									v->velocity.x = 0.0;
									v->velocity.z = 0.0;
								}
								v->velocity.x -= restitutionCoefficient * VYx;
								v->velocity.z -= restitutionCoefficient * VYy;
								v->position.x += (c->radius-nabs)*(1.0+restitutionCoefficient) * nx/nabs;
								v->position.z += (c->radius-nabs)*(1.0+restitutionCoefficient) * ny/nabs;
							}
						}
						h++;
					}
				}
			}
		}
	}
}