physics.cpp

一个3D的保龄球的源代码

				}
			}
		}			
	return checked;
} 


int Physics::dCollideSpheres(Vector p1, PhType r1, Vector p2, PhType r2)
{
	Vector p = p1-p2;
	PhType l = p.Magnitude();
	if(l>r1+r2)
		return 0;
	return 1;
}

int Physics::dCollideCylinderSphere(int id1, int id2, pCollision collisionData,int &count)
{
	Vector z(Bodies[id1].R.e13>>MULT_BTW_TRI_PHT,Bodies[id1].R.e23>>MULT_BTW_TRI_PHT,Bodies[id1].R.e33>>MULT_BTW_TRI_PHT);
	Vector p = Bodies[id1].vPosition - Bodies[id2].vPosition;
	PhType lenH = z*p;
	Vector hR = z^p;
	PhType lenR = hR.Magnitude();
	Vector pt1,pt2,vel1,vel2;
	lenH = (lenH>=0 ? lenH : -lenH)-HEIGHT_TENPIN/2;
	lenR = (lenR>=0 ? lenR : -lenR)-RADIUS_TENPIN_DOWN;
	if(lenH>RADIUS_BALL||lenR>RADIUS_BALL)
		return NO_CONTACT;
	if(lenH>0&&lenR>0)
	{
		if(lenH*lenH+lenR*lenR > RADIUS_BALL*RADIUS_BALL)
			return NO_CONTACT;
	}
	collisionData->body1 = id1;
	collisionData->body2 = id2;
	p.Normalize();
	collisionData->vCollisionNormal = p;
	collisionData->vCollisionPoint = Bodies[id2].vPosition+(p*RADIUS_BALL);
	pt1 = collisionData->vCollisionPoint - Bodies[id1].vPosition;
	pt2 = collisionData->vCollisionPoint - Bodies[id2].vPosition;
	vel1 = Bodies[id1].vVelocity + (Bodies[id1].vAngularVelocityGlobal^pt1);
	vel2 = Bodies[id2].vVelocity + (Bodies[id2].vAngularVelocityGlobal^pt2);
	collisionData->vRelativeVelocity = vel1-vel2;
	collisionData->vCollisionTangent = ((collisionData->vCollisionNormal)^(collisionData->vRelativeVelocity))^(collisionData->vCollisionNormal);
	collisionData->vCollisionTangent.Normalize();
	collisionData++;
	count++;
	NumCollisions++;
	return HAVE_COLLISION;
}

int Physics::dCollideCylinders(int id1,int id2, pCollision collisionData,int &count)
{
	PhType d=0,d1,d2,l;
	Vector z1(Bodies[id1].R.e13>>MULT_BTW_TRI_PHT,Bodies[id1].R.e23>>MULT_BTW_TRI_PHT,Bodies[id1].R.e33>>MULT_BTW_TRI_PHT);
	Vector z2(Bodies[id2].R.e13>>MULT_BTW_TRI_PHT,Bodies[id2].R.e23>>MULT_BTW_TRI_PHT,Bodies[id2].R.e33>>MULT_BTW_TRI_PHT);
	Vector p[2];
	Vector tempV,p12,pp,pp01,pp12,temP;
	Vector pt1,pt2,vel1,vel2;
	p12 = Bodies[id1].vPosition-Bodies[id2].vPosition;
	int b = calcPlumbAmong2L(Bodies[id1].vPosition,z1,Bodies[id2].vPosition,z2,p);
	if(!b)
	{
        tempV = p12^z1;
		d = tempV.Magnitude();
        l = p12*z1;
		if(d<(90+90) && l<90 && l>(-HEIGHT_TENPIN)){//if(d<(RADIUS_TENPIN_DOWN+RADIUS_TENPIN_DOWN) && l<HEIGHT_TENPIN && l>(-HEIGHT_TENPIN)){
			collisionData->body1 = id1;
			collisionData->body2 = id2;
			p12.Normalize();
			collisionData->vCollisionNormal=tempV^z2;
			collisionData->vCollisionNormal.Normalize();
			temP = Bodies[id2].vPosition+(z2*(l/2));
			collisionData->vCollisionPoint = temP+(collisionData->vCollisionNormal*RADIUS_TENPIN_DOWN);
			pt1 = collisionData->vCollisionPoint - Bodies[id1].vPosition;
			pt2 = collisionData->vCollisionPoint - Bodies[id2].vPosition;
			vel1 = Bodies[id1].vVelocity + (Bodies[id1].vAngularVelocityGlobal^pt1);
			vel2 = Bodies[id2].vVelocity + (Bodies[id2].vAngularVelocityGlobal^pt2);
			collisionData->vRelativeVelocity = vel1-vel2;
			collisionData->vCollisionTangent = ((collisionData->vCollisionNormal)^(collisionData->vRelativeVelocity))^(collisionData->vCollisionNormal);
			collisionData->vCollisionTangent.Normalize();
			collisionData++;
			NumCollisions++;
			count++;
			return HAVE_COLLISION;
		}
	}
	else{
		pp = p[0]-p[1];
		pp01 = p[0]-Bodies[id1].vPosition;
		pp12 = p[1]-Bodies[id2].vPosition;
		d = pp.Magnitude();
		d1 = pp01.Magnitude();
		d2 = pp12.Magnitude();	
		if(d<(90+90) && d1<(HEIGHT_TENPIN/2+90) && d2<(HEIGHT_TENPIN/2+90)){//if(d<(RADIUS_TENPIN_DOWN+RADIUS_TENPIN_DOWN) && d1<(HEIGHT_TENPIN/2+RADIUS_TENPIN_DOWN) && d2<(HEIGHT_TENPIN/2+RADIUS_TENPIN_DOWN)){
			collisionData->body1 = id1;
			collisionData->body2 = id2;
			p12.Normalize();
			collisionData->vCollisionNormal=pp;
			collisionData->vCollisionNormal.Normalize();
			collisionData->vCollisionPoint = (p[0]+p[1])/(2<<PHT_EXT);
			pt1 = collisionData->vCollisionPoint - Bodies[id1].vPosition;
			pt2 = collisionData->vCollisionPoint - Bodies[id2].vPosition;
			vel1 = Bodies[id1].vVelocity + (Bodies[id1].vAngularVelocityGlobal^pt1);
			vel2 = Bodies[id2].vVelocity + (Bodies[id2].vAngularVelocityGlobal^pt2);
			collisionData->vRelativeVelocity = vel1-vel2;
			collisionData->vCollisionTangent = (collisionData->vCollisionNormal^collisionData->vRelativeVelocity)^collisionData->vCollisionNormal;
			collisionData->vCollisionTangent.Normalize();
			collisionData++;
			NumCollisions++;
			count++;
			return HAVE_COLLISION;
		}	
	}
	return NO_CONTACT;
}

int Physics::dCollideCylinderPlane(int id, pCollision collisionData)
{
	Vector z(Bodies[id].R.e13>>MULT_BTW_TRI_PHT,Bodies[id].R.e23>>MULT_BTW_TRI_PHT,Bodies[id].R.e33>>MULT_BTW_TRI_PHT);
	Vector z0(0,0,(1<<PHT_EXT));
	Vector c1 = Bodies[id].vPosition+(z*(HEIGHT_TENPIN/2));
	Vector c2 = Bodies[id].vPosition-(z*(HEIGHT_TENPIN/2));
	Vector n = (z^z0)^z;
	Vector a[4];
	Vector pt1;
	int i;
//	pCollision tempC = collisionData;
	a[0] = c1+(n*(RADIUS_TENPIN*2));
	a[1] = c1-(n*(RADIUS_TENPIN*2));
	a[2] = c2+(n*(RADIUS_TENPIN*2));
	a[3] = c2-(n*(RADIUS_TENPIN*2));
	if(a[0].z<=0 || a[1].z<=0 || a[2].z<=0 || a[3].z<=0)
	{
		for(i=0;i<4;i++)
		{
			if(a[i].z<=0)
			{
				collisionData->body1 = id;
				collisionData->body2 = -1;
				collisionData->vCollisionNormal.x=0;
				collisionData->vCollisionNormal.y=0;
				collisionData->vCollisionNormal.z=(1<<PHT_EXT);
				collisionData->vCollisionPoint = a[i];
				pt1=collisionData->vCollisionPoint-Bodies[id].vPosition;
				collisionData->vRelativeAcceleration = 
				Bodies[id].vAcceleration + (Bodies[id].vAngularVelocity^(Bodies[id].vAngularVelocity^pt1))+(Bodies[id].vAngularAcceleration ^ pt1);
					
				collisionData->vRelativeVelocity = Bodies[id].vVelocity+ (Bodies[id].vAngularVelocityGlobal^(a[i] - Bodies[id].vPosition));
				collisionData->vCollisionTangent = ((collisionData->vCollisionNormal)^(collisionData->vRelativeVelocity))^(collisionData->vCollisionNormal);
				collisionData->vCollisionTangent.Reverse();
				collisionData->vCollisionTangent.Normalize();
				collisionData++;
				NumCollisions++;
			}
		}
		return HAVE_COLLISION;
	}
	return NO_CONTACT;
}

int Physics::dCollideSpherePlane(int id, pCollision collisionData)
{
	if(Bodies[id].vPosition.z<=RADIUS_BALL)
	{
		Vector pt1;
		collisionData->body1=id;
		collisionData->body2=-1;
		collisionData->vCollisionNormal.x=0;
		collisionData->vCollisionNormal.y=0;
		collisionData->vCollisionNormal.z=(1<<PHT_EXT);
		collisionData->vCollisionPoint.x=Bodies[id].vPosition.x;
		collisionData->vCollisionPoint.y=Bodies[id].vPosition.y;
		collisionData->vCollisionPoint.z=0;
		
		pt1 =collisionData->vCollisionPoint-Bodies[id].vPosition;
		collisionData->vRelativeAcceleration = 
		Bodies[id].vAcceleration + (Bodies[id].vAngularVelocity^(Bodies[id].vAngularVelocity^pt1))+(Bodies[id].vAngularAcceleration ^ pt1);
			
		collisionData->vRelativeVelocity = Bodies[id].vVelocity+ (Bodies[id].vAngularVelocityGlobal^(collisionData->vCollisionPoint - Bodies[id].vPosition));
		collisionData->vCollisionTangent = ((collisionData->vCollisionNormal)^(collisionData->vRelativeVelocity))^(collisionData->vCollisionNormal);
		collisionData->vCollisionTangent.Reverse();
		collisionData->vCollisionTangent.Normalize();
		collisionData++;
		NumCollisions++;
		return HAVE_COLLISION;
	}
	return NO_CONTACT;
}

int Physics::dCollideCylinderTopBlock(int id, pCollision collisionData,int &count)
{
	Vector z(Bodies[id].R.e13>>MULT_BTW_TRI_PHT,Bodies[id].R.e23>>MULT_BTW_TRI_PHT,Bodies[id].R.e33>>MULT_BTW_TRI_PHT);
	Vector z0(0,0,-(1<<PHT_EXT));
	Vector c1 = Bodies[id].vPosition+(z*(HEIGHT_TENPIN/2));
	Vector c2 = Bodies[id].vPosition-(z*(HEIGHT_TENPIN/2));
	Vector n = (z^z0)^z;
	Vector a[4];
	Vector pt1;
	int i;
	int bCollision = 0;
	int num=0;

	a[0] = c1+(n*(RADIUS_TENPIN*2));
	a[1] = c1-(n*(RADIUS_TENPIN*2));
	a[2] = c2+(n*(RADIUS_TENPIN*2));
	a[3] = c2-(n*(RADIUS_TENPIN*2));

	for(i=0;i<4;i++)
	{		
		if(num<2 && a[i].z>=BLOCKS_HEIGHT && a[i].x<=BLOCKS_POSITION_0_X && a[i].x>=(BLOCKS_POSITION_0_X+BLOCK_WIDTH) && a[i].y>=BLOCKS_POSITION_0_Y && a[i].y<=(BLOCKS_POSITION_0_Y+BLOCKS_LENGTH))
		{
			bCollision = 1;
			collisionData->body1 = id;
			collisionData->body2 = -1;
			collisionData->vCollisionNormal.x=0;
			collisionData->vCollisionNormal.y=0;
			collisionData->vCollisionNormal.z=-(1<<PHT_EXT);
			collisionData->vCollisionPoint = a[i];
			pt1=collisionData->vCollisionPoint-Bodies[id].vPosition;
			collisionData->vRelativeAcceleration = Bodies[id].vAcceleration + (Bodies[id].vAngularVelocity^(Bodies[id].vAngularVelocity^pt1))+(Bodies[id].vAngularAcceleration ^ pt1);
			collisionData->vRelativeVelocity = Bodies[id].vVelocity+ (Bodies[id].vAngularVelocityGlobal^pt1);
			collisionData->vCollisionTangent = ((collisionData->vCollisionNormal)^(collisionData->vRelativeVelocity))^(collisionData->vCollisionNormal);
			collisionData->vCollisionTangent.Reverse();
			collisionData->vCollisionTangent.Normalize();
			collisionData++;
			NumCollisions++;
			num++;
			count++;
		}		
	}
	
	return bCollision;
}

int Physics::dCollideCylinderLRBlock(int id, pCollision collisionData,int &count)
{
	Vector z(Bodies[id].R.e13>>MULT_BTW_TRI_PHT,Bodies[id].R.e23>>MULT_BTW_TRI_PHT,Bodies[id].R.e33>>MULT_BTW_TRI_PHT);
	Vector z0((1<<PHT_EXT),0,0);
	Vector c1 = Bodies[id].vPosition+(z*(HEIGHT_TENPIN/2));
	Vector c2 = Bodies[id].vPosition-(z*(HEIGHT_TENPIN/2));
	Vector n = (z^z0)^z;
	Vector a[4];
	Vector pt1;
	int i;
	int bCollision = 0;
	int num=0;

	a[0] = c1+(n*(RADIUS_TENPIN*2));
	a[1] = c1-(n*(RADIUS_TENPIN*2));
	a[2] = c2+(n*(RADIUS_TENPIN*2));
	a[3] = c2-(n*(RADIUS_TENPIN*2));
	for(i=0;i<4;i++)
	{
		if(a[i].y>=BLOCKS_POSITION_0_Y && a[i].y<=(BLOCKS_POSITION_0_Y+BLOCKS_LENGTH) && a[i].z>=BLOCKS_POSITION_0_Z &&a[i].z<=(BLOCKS_POSITION_0_Z+BLOCKS_HEIGHT))
		{
			if(a[i].x<=-WIDTH_BLOCK /*&& num<2*/)//if(a[i].x<=BLOCKS_POSITION_0_X /*&& num<2*/)
			{
				bCollision = 1;
				collisionData->body1 = id;
				collisionData->body2 = -1;
				collisionData->vCollisionNormal.x=(1<<PHT_EXT);
				collisionData->vCollisionNormal.y=0;
				collisionData->vCollisionNormal.z=0;
				collisionData->vCollisionPoint = a[i];
				pt1=collisionData->vCollisionPoint-Bodies[id].vPosition;
				collisionData->vRelativeAcceleration = Bodies[id].vAcceleration + (Bodies[id].vAngularVelocity^(Bodies[id].vAngularVelocity^pt1))+(Bodies[id].vAngularAcceleration ^ pt1);
				collisionData->vRelativeVelocity = Bodies[id].vVelocity+ (Bodies[id].vAngularVelocityGlobal^pt1);
				collisionData->vCollisionTangent = ((collisionData->vCollisionNormal)^(collisionData->vRelativeVelocity))^(collisionData->vCollisionNormal);
				collisionData->vCollisionTangent.Reverse();
				collisionData->vCollisionTangent.Normalize();
				collisionData++;
				NumCollisions++;
				num++;
				count++;

				// not to go through
				Bodies[id].vPosition.x-=(a[i].x+WIDTH_BLOCK);
			}			
		}
	}
	if(bCollision)
		return bCollision;
	for (i=0;i<4;i++)
	{
		if(a[i].y>=BLOCKS_POSITION_0_Y && a[i].y<=(BLOCKS_POSITION_0_Y+BLOCKS_LENGTH) && a[i].z>=BLOCKS_POSITION_0_Z &&a[i].z<=(BLOCKS_POSITION_0_Z+BLOCKS_HEIGHT))
		{
			if(a[i].x>=WIDTH_BLOCK /*&& num<2*/)//if(a[i].x>=(BLOCKS_POSITION_0_X+BLOCKS_WIDTH) /*&& num<2*/)
			{
				bCollision = 1;
				collisionData->body1 = id;
				collisionData->body2 = -1;
				collisionData->vCollisionNormal.x=-(1<<PHT_EXT);
				collisionData->vCollisionNormal.y=0;
				collisionData->vCollisionNormal.z=0;
				collisionData->vCollisionPoint = a[i];
				pt1=collisionData->vCollisionPoint-Bodies[id].vPosition;
				collisionData->vRelativeAcceleration = Bodies[id].vAcceleration + (Bodies[id].vAngularVelocity^(Bodies[id].vAngularVelocity^pt1))+(Bodies[id].vAngularAcceleration ^ pt1);
				collisionData->vRelativeVelocity = Bodies[id].vVelocity+ (Bodies[id].vAngularVelocityGlobal^pt1);
				collisionData->vCollisionTangent = ((collisionData->vCollisionNormal)^(collisionData->vRelativeVelocity))^(collisionData->vCollisionNormal);
				collisionData->vCollisionTangent.Reverse();
				collisionData->vCollisionTangent.Normalize();
				collisionData++;
				NumCollisions++;
				num++;
				count++;

				// not to go through
				Bodies[id].vPosition.x-=(a[i].x-WIDTH_BLOCK);
			}
		}
	}
	return bCollision;
}

int Physics::dCollideCylinderBlock(int id, pCollision collisionData,int &count)
{
	int lr = dCollideCylinderLRBlock(id,collisionData,count);
	int top = dCollideCylinderTopBlock(id,collisionData,count);
	if(lr>0 || top>0)
		return 1;
	return 0;
}




void Physics::ResolveCollisions(void)
{
	int		i,k;
	PhType	j;
	PhType	j1,j2;
	Vector	pt1, pt2;
	PhType	fCr;	
	int		b1, b2;
	PhType	Vrt;
	PhType tmp;
	PhType	mu = FRICTIONCOEFFICIENT;
	bool	dofriction = DOFRICTION;
	
	
	for(i=0; i<NumCollisions; i++)
	{
		b1 = Collisions[i].body1;
		b2 = Collisions[i].body2;

		
		if( (b1 != -1) && (b1 != b2) )
		{
			if(b2 != -1) // not ground plane
			{
				pt1 = Collisions[i].vCollisionPoint - Bodies[b1].vPosition;
				pt2 = Collisions[i].vCollisionPoint - Bodies[b2].vPosition;
				Vrt = Collisions[i].vRelativeVelocity * Collisions[i].vCollisionTangent;
				
				//tmp, used for calculate J 
				tmp= (((1<<(2*PHT_EXT))/Bodies[b1].fMass)+((1<<(2*PHT_EXT))/Bodies[b2].fMass)) +
					(Collisions[i].vCollisionNormal * (((pt1 ^ Collisions[i].vCollisionNormal)*Bodies[b1].mIeInverse)^pt1)) +
					(Collisions[i].vCollisionNormal * (((pt2 ^ Collisions[i].vCollisionNormal)*Bodies[b2].mIeInverse^pt2)));
				
				// calculate impulse
				fCr = COEFFICIENTOFRESTITUTION1;
				j1=	((-((1<<PHT_EXT)+fCr))*(Collisions[i].vRelativeVelocity*Collisions[i].vCollisionNormal)/tmp);
				fCr = COEFFICIENTOFRESTITUTION2;
				j2=	((-((1<<PHT_EXT)+fCr))*(Collisions[i].vRelativeVelocity*Collisions[i].vCollisionNormal)/tmp);
				
				//---------------------------------------
				// set the parameter for the update collision data
				int id[2]={b1,b2};
				int jj[2]={j2,j2};
				Vector pt[2];
				pt[0]=pt1;
				pt[1]=pt2;
				
				int perEnergy[2]={PERCENT_ENERGY_TENPIN,PERCENT_ENERGY_TENPIN};
				int perEngergyVel[2]={PERCENT_ENERGY_TENPIN_VEL,PERCENT_ENERGY_TENPIN_VEL};
				int perEngergyAng[2]={PERCENT_ENERGY_TENPIN_ANG,PERCENT_ENERGY_TENPIN_ANG};
				int coefficient[2]={1,-1};
				if(b1==ID_BALL){
					jj[0]=j1;
					perEnergy[0]=PERCENT_ENERGY_BALL;
					perEngergyVel[0]=PERCENT_ENERGY_BALL_VEL;
					perEngergyAng[0]=PERCENT_ENERGY_BALL_ANG;
				}else if(b2==ID_BALL){
					jj[1]=j1;
					perEnergy[1]=PERCENT_ENERGY_BALL;
					perEngergyVel[1]=PERCENT_ENERGY_BALL_VEL;
					perEngergyAng[1]=PERCENT_ENERGY_BALL_ANG;
				}
				//---------------------------------------
				
				if(Vrt!=0 && dofriction) {
					
					//---------------------------------------
					for(k=0;k<2;k++){// update the collision object
						Bodies[id[k]].vVelocity += (( (coefficient[k]*jj[k] * Collisions[i].vCollisionNormal) + (((mu * jj[k])>>PHT_EXT) * Collisions[i].vCollisionTangent) ) / Bodies[id[k]].fMass)*perEnergy[k];
						Bodies[id[k]].vVelocity=Bodies[id[k]].vVelocity*perEngergyVel[k];
						Bodies[id[k]].vAngularVelocityGlobal += ((pt[k] ^ ((coefficient[k]*jj[k] * Collisions[i].vCollisionNormal) + (((mu * jj[k])>>PHT_EXT) * Collisions[i].vCollisionTangent)))*Bodies[id[k]].mIeInverse)*perEnergy[k];
						Bodies[id[k]].vAngularVelocityGlobal =Bodies[id[k]].vAngularVelocityGlobal *perEngergyAng[k];
						Bodies[id[k]].vAngularVelocity = (QVRotate(~Bodies[id[k]].qOrientation, Bodies[id[k]].vAngularVelocityGlobal));
					}
					//---------------------------------------

					/*	

					if(b1==ID_BALL){
						j=j1;
						Bodies[b1].vVelocity += (( (j * Collisions[i].vCollisionNormal) + (((mu * j)>>PHT_EXT) * Collisions[i].vCollisionTangent) ) / Bodies[b1].fMass)*PERCENT_ENERGY_BALL;
						Bodies[b1].vVelocity=Bodies[b1].vVelocity*PERCENT_ENERGY_BALL_VEL;
						Bodies[b1].vAngularVelocityGlobal += ((pt1 ^ ((j * Collisions[i].vCollisionNormal) + (((mu * j)>>PHT_EXT) * Collisions[i].vCollisionTangent)))*Bodies[b1].mIeInverse)*PERCENT_ENERGY_BALL;
						Bodies[b1].vAngularVelocityGlobal =Bodies[b1].vAngularVelocityGlobal *PERCENT_ENERGY_BALL_ANG;
						Bodies[b1].vAngularVelocity = (QVRotate(~Bodies[b1].qOrientation, Bodies[b1].vAngularVelocityGlobal));
					}
					else{
						j=j2;
						Bodies[b1].vVelocity += (( (j * Collisions[i].vCollisionNormal) + (((mu * j)>>PHT_EXT) * Collisions[i].vCollisionTangent) ) / Bodies[b1].fMass)*PERCENT_ENERGY_TENPIN;