📄 physics.cpp
字号:
Bodies[b1].vVelocity=Bodies[b1].vVelocity*PERCENT_ENERGY_TENPIN_VEL;
Bodies[b1].vAngularVelocityGlobal += ((pt1 ^ ((j * Collisions[i].vCollisionNormal) + (((mu * j)>>PHT_EXT) * Collisions[i].vCollisionTangent)))*Bodies[b1].mIeInverse)*PERCENT_ENERGY_TENPIN;
Bodies[b1].vAngularVelocityGlobal =Bodies[b1].vAngularVelocityGlobal *PERCENT_ENERGY_TENPIN_ANG;
Bodies[b1].vAngularVelocity = QVRotate(~Bodies[b1].qOrientation, Bodies[b1].vAngularVelocityGlobal);
}
if(b2==ID_BALL){
j=j1;
Bodies[b2].vVelocity += (((-j * Collisions[i].vCollisionNormal) + (((mu * j)>>PHT_EXT) * Collisions[i].vCollisionTangent)) / Bodies[b2].fMass)*PERCENT_ENERGY_BALL;
Bodies[b2].vVelocity=Bodies[b2].vVelocity*PERCENT_ENERGY_BALL_VEL;
Bodies[b2].vAngularVelocityGlobal +=((pt2 ^ ((-j * Collisions[i].vCollisionNormal) + (((mu * j)>>PHT_EXT) * Collisions[i].vCollisionTangent)))*Bodies[b2].mIeInverse)*PERCENT_ENERGY_BALL;
Bodies[b2].vAngularVelocityGlobal =Bodies[b2].vAngularVelocityGlobal *PERCENT_ENERGY_BALL_ANG;
Bodies[b2].vAngularVelocity = QVRotate(~Bodies[b2].qOrientation, Bodies[b2].vAngularVelocityGlobal);
}else{
j=j2;
Bodies[b2].vVelocity += (((-j * Collisions[i].vCollisionNormal) + (((mu * j)>>PHT_EXT) * Collisions[i].vCollisionTangent)) / Bodies[b2].fMass)*PERCENT_ENERGY_TENPIN;
Bodies[b2].vVelocity=Bodies[b2].vVelocity*PERCENT_ENERGY_TENPIN_VEL;
Bodies[b2].vAngularVelocityGlobal += ((pt2 ^ ((-j * Collisions[i].vCollisionNormal) + (((mu * j)>>PHT_EXT) * Collisions[i].vCollisionTangent)))*Bodies[b2].mIeInverse)*PERCENT_ENERGY_TENPIN;
Bodies[b2].vAngularVelocityGlobal =Bodies[b2].vAngularVelocityGlobal *PERCENT_ENERGY_TENPIN_ANG;
Bodies[b2].vAngularVelocity = QVRotate(~Bodies[b2].qOrientation, Bodies[b2].vAngularVelocityGlobal);
}*/
} else {
// apply impulse
//---------------------------------------
for(k=0;k<2;k++)// not collision just touch
{
// Bodies[id[k]].vVelocity += (jj[k]*Collisions[i].vCollisionNormal) / Bodies[id[k]].fMass;
// Bodies[id[k]].vAngularVelocityGlobal =coefficient[k]*(pt[k]^(jj[k] * Collisions[i].vCollisionNormal))*Bodies[id[k]].mIeInverse;
// Bodies[id[k]].vAngularVelocity = QVRotate(~Bodies[id[k]].qOrientation, Bodies[id[k]].vAngularVelocityGlobal);
}
//---------------------------------------
/*
if(ID_BALL==b1){
j=j1;
Bodies[b1].vVelocity += (j*Collisions[i].vCollisionNormal) / Bodies[b1].fMass;
Bodies[b1].vAngularVelocityGlobal += (pt1^(j * Collisions[i].vCollisionNormal))*Bodies[b1].mIeInverse;
Bodies[b1].vAngularVelocity = QVRotate(~Bodies[b1].qOrientation, Bodies[b1].vAngularVelocityGlobal);
}else{
j=j2;
Bodies[b1].vVelocity += (j*Collisions[i].vCollisionNormal) / Bodies[b1].fMass;
Bodies[b1].vAngularVelocityGlobal += (pt1^(j * Collisions[i].vCollisionNormal))*Bodies[b1].mIeInverse;
Bodies[b1].vAngularVelocity = QVRotate(~Bodies[b1].qOrientation, Bodies[b1].vAngularVelocityGlobal);
}
if(ID_BALL==b2){
j=j1;
Bodies[b2].vVelocity -= (j*Collisions[i].vCollisionNormal) / Bodies[b2].fMass;
Bodies[b2].vAngularVelocityGlobal -= (pt2 ^ (j * Collisions[i].vCollisionNormal))*Bodies[b2].mIeInverse;
Bodies[b2].vAngularVelocity = QVRotate(~Bodies[b2].qOrientation, Bodies[b2].vAngularVelocityGlobal);
}else{
j=j2;
Bodies[b2].vVelocity -= (j*Collisions[i].vCollisionNormal) / Bodies[b2].fMass;
Bodies[b2].vAngularVelocityGlobal -= (pt2 ^ (j * Collisions[i].vCollisionNormal))*Bodies[b2].mIeInverse;
Bodies[b2].vAngularVelocity = QVRotate(~Bodies[b2].qOrientation, Bodies[b2].vAngularVelocityGlobal);
}*/
}
} else { // it is bound plane
fCr = COEFFICIENTOFRESTITUTIONBOARD;
pt1 = Collisions[i].vCollisionPoint - Bodies[b1].vPosition;
// calculate impulse
j = (-((1<<PHT_EXT)+fCr) * (Collisions[i].vRelativeVelocity*Collisions[i].vCollisionNormal)) /
( ((1<<(2*PHT_EXT))/Bodies[b1].fMass) +
(Collisions[i].vCollisionNormal * (((pt1 ^ Collisions[i].vCollisionNormal)*Bodies[b1].mIeInverse )^pt1)));
Vrt = Collisions[i].vRelativeVelocity * Collisions[i].vCollisionTangent;
if(abs(Vrt) > 0.0 && dofriction) {
Bodies[b1].vVelocity += (( (j * Collisions[i].vCollisionNormal) + (((mu * j)>>PHT_EXT) * Collisions[i].vCollisionTangent) ) / Bodies[b1].fMass)*PERCENT_ENERGY_BOARD;
Bodies[b1].vVelocity =Bodies[b1].vVelocity *PERCENT_ENERGY_BOARD_VEL;
Bodies[b1].vAngularVelocityGlobal += ((pt1 ^ ((j * Collisions[i].vCollisionNormal) + (((mu * j)>>PHT_EXT) * Collisions[i].vCollisionTangent)))*Bodies[b1].mIeInverse)*PERCENT_ENERGY_BOARD;
Bodies[b1].vAngularVelocityGlobal=Bodies[b1].vAngularVelocityGlobal*PERCENT_ENERGY_BOARD_ANG;
Bodies[b1].vAngularVelocity = QVRotate(~Bodies[b1].qOrientation, Bodies[b1].vAngularVelocityGlobal);
} else {
// apply impulse
Bodies[b1].vVelocity += ((j * Collisions[i].vCollisionNormal) / Bodies[b1].fMass)*PERCENT_ENERGY_BOARD;
Bodies[b1].vVelocity =Bodies[b1].vVelocity *PERCENT_ENERGY_BOARD_VEL;
Bodies[b1].vAngularVelocityGlobal += ((pt1 ^ (j * Collisions[i].vCollisionNormal))*Bodies[b1].mIeInverse)*PERCENT_ENERGY_BOARD;
Bodies[b1].vAngularVelocityGlobal=Bodies[b1].vAngularVelocityGlobal*PERCENT_ENERGY_BOARD_ANG;
Bodies[b1].vAngularVelocity = QVRotate(~Bodies[b1].qOrientation, Bodies[b1].vAngularVelocityGlobal);
}
}
}
}
}
void Physics::limitation(int i){
if(i==ID_BALL)
{
if ((Bodies[i].vVelocity.z<0)&&(Bodies[ID_BALL].vPosition.z<RADIUS_BALL))// not to sink
{
Bodies[i].vVelocity.z=0;
Bodies[i].vPosition.z=RADIUS_BALL;
}
if(Bodies[i].vPosition.x<-(GUTTER_WIDTH_X))//out of left block
{
Bodies[i].vPosition.x=-GUTTER_POSITION_X;
Bodies[i].vPosition.z=-GUTTER_POSITION_Z;
//if(Bodies[i].vVelocity.x<0)
Bodies[i].vVelocity.x=0;
}
if(Bodies[i].vPosition.x>(GUTTER_WIDTH_X))//out of right block
{
Bodies[i].vPosition.x=(GUTTER_POSITION_X);
Bodies[i].vPosition.z=-GUTTER_POSITION_Z;
//if(Bodies[i].vVelocity.x>0)
Bodies[i].vVelocity.x=0;
}
if(Bodies[ID_BALL].vPosition.y>=(LENGTH_BLOCK+(4<<PHT_EXT)))//back out
{
Bodies[i].vPosition.y=(LENGTH_BLOCK+(4<<PHT_EXT));
Bodies[i].vPosition.z=-500;
Bodies[i].vVelocity=Vector(0,0,0);
Bodies[i].status=BALL_INVALID;
}
else
{
if(Bodies[i].vVelocity.y<320)// if the ball in the collision box is too slow, make it go
{
Bodies[i].vVelocity.y=320;
}
}
}
if(i!=ID_BALL)//limit the tenpins
{
//make the tenpin on the ground stop shivering
if((Bodies[i].vPosition.z-RADIUS_TENPIN)<=800&&
(Bodies[i].status!=TENPIN_STATE_BACK)&&(Bodies[i].status!=TENPIN_STATE_SIDE_OUT)){//slow down tenpin
if((abs(Bodies[i].R.e33)<=5000 && abs(Bodies[i].R.e33)>=3000)){
Bodies[i].vVelocity*=50;//64;
Bodies[i].vAngularVelocity*=22;//64;
}
else if((abs(Bodies[i].R.e33)<=3000 && abs(Bodies[i].R.e33)>=2000)){
Bodies[i].vVelocity*=45;//54;
Bodies[i].vAngularVelocity*=20;//54;
}
else if((abs(Bodies[i].R.e33)<=2000 && abs(Bodies[i].R.e33)>=1000)){
Bodies[i].vVelocity*=ATTENUATION_TENPIN_VEL;
Bodies[i].vAngularVelocity*=ATTENUATION_TENPIN_ANG;
}
else if(abs(Bodies[i].R.e33)<=1000){
Bodies[i].vVelocity*=ATTENUATION_TENPIN_VEL;
Bodies[i].vAngularVelocity*=ATTENUATION_TENPIN_ANG;
}
}
if(abs(Bodies[i].R.e33)<=500 && (Bodies[i].vPosition.z-RADIUS_TENPIN)<=1000 &&
(Bodies[i].status!=TENPIN_STATE_BACK)&&(Bodies[i].status!=TENPIN_STATE_SIDE_OUT))
{
Bodies[i].vVelocity*=80;
Bodies[i].vAngularVelocity*=80;
}
if((Bodies[i].vPosition.z-RADIUS_TENPIN)<=120&&(abs(Bodies[i].R.e33)<=4000)&&
(Bodies[i].status!=TENPIN_STATE_BACK)&&(Bodies[i].status!=TENPIN_STATE_OUTSIDE))//stop tenpin
{
if (isVelAndAngIn(i,LIMIT_VELOCITY_X,LIMIT_ANGVELOCITY_X))//stop the tenpin
{
Bodies[i].vVelocity.x=0;
Bodies[i].vVelocity.y=0;
Bodies[i].vVelocity.z=0;
Bodies[i].vAngularVelocity.x=0;
Bodies[i].vAngularVelocity.y=0;
Bodies[i].vAngularVelocity.z=0;
Bodies[i].vPosition.z=RADIUS_TENPIN;
Bodies[i].status=TENPIN_STATE_DEAD;
}
if(Bodies[i].status==TENPIN_STATE_DEAD)//make the tenpin still
{
//Bodies[i].vPosition.z=RADIUS_TENPIN;
Bodies[i].vVelocity.x=0;
Bodies[i].vVelocity.y=0;
Bodies[i].vVelocity.z=0;
Bodies[i].vAngularVelocity.x=0;
Bodies[i].vAngularVelocity.y=0;
Bodies[i].vAngularVelocity.z=0;
}
}
//collision with board
if((Bodies[i].vPosition.x<=-(WIDTH_LANE)||
Bodies[i].vPosition.x>=(WIDTH_LANE))&&
Bodies[i].status!=TENPIN_STATE_BACK&&
abs(Bodies[i].vPosition.z-RADIUS_TENPIN_DOWN)<(1<<PHT_EXT)&&
abs(Bodies[i].R.e33)<=1000)//out of the right and left lane and the tenpin is down
{
Bodies[i].vVelocity*=80;//54;
Bodies[i].vAngularVelocity*=60;//54;
Bodies[i].status=TENPIN_STATE_SIDE_OUT;
}
if (Bodies[i].status==TENPIN_STATE_SIDE_OUT&&
(Bodies[i].vPosition.x>=-(WIDTH_LANE-2<<PHT_EXT)||
Bodies[i].vPosition.x<=(WIDTH_LANE-2<<PHT_EXT)))
{
//Bodies[i].status=TENPIN_STATE_MOVING;
}
if(Bodies[i].vPosition.x<=-(WIDTH_BLOCK)||//out of left block
Bodies[i].vPosition.x>=(WIDTH_BLOCK))//out of right block
{
int sig=(Bodies[i].vPosition.x>0)?1:-1;
Bodies[i].vPosition.x=sig*(GUTTER_WIDTH_X);
/*
if (isVelAndAngIn(i,LIMIT_VELOCITY_X,LIMIT_ANGVELOCITY_X))//stop the tenpin
{
Bodies[i].vVelocity.x=0;
Bodies[i].vVelocity.y=0;
Bodies[i].vVelocity.z=0;
Bodies[i].vAngularVelocity.x=0;
Bodies[i].vAngularVelocity.y=0;
Bodies[i].vAngularVelocity.z=0;
Bodies[i].status=TENPIN_STATE_DEAD;
}else
{
if(sig*Bodies[i].vVelocity.x>0)//
{
Bodies[i].vVelocity.x=-sig*abs(Bodies[i].vVelocity.x)*1/2;//Bodies[i].vVelocity.x=-sig*abs(Bodies[i].vVelocity.x)*1/10;
Bodies[i].vAngularVelocity*=80;//Bodies[i].vAngularVelocity*=45;
}
}*/
}
if((Bodies[i].vPosition.z>=819)&&(Bodies[i].vPosition.y<LENGTH_BLOCK))//collision with upper block
{
Bodies[i].vPosition.z=819;
if(Bodies[i].vVelocity.z>0)
Bodies[i].vVelocity.z=-Bodies[i].vVelocity.z;
//Bodies[i].vVelocity.z=0;
}
if(Bodies[i].vPosition.y>(LENGTH_BLOCK))//out of collision box
{
Bodies[i].status=TENPIN_STATE_BACK;
}
if(Bodies[i].vPosition.y>(LENGTH_BLOCK_BACK)&&(TENPIN_STATE_BACK==Bodies[i].status))//out of back block
{
Bodies[i].vPosition.y=(222<<PHT_EXT);
int velz=Bodies[i].vVelocity.z;
Bodies[i].vVelocity=Vector(0,0,velz);
Bodies[i].vAngularVelocity=Vector(0,0,0);
//Bodies[i].status=TENPIN_STATE_OUTSIDE;
}
if(((Bodies[i].vPosition.y>=(LENGTH_BLOCK_BACK)&&Bodies[i].vPosition.z<=-3*HEIGHT_TENPIN)||
(Bodies[i].vPosition.z<=-3*HEIGHT_TENPIN))&&TENPIN_STATE_BACK==Bodies[i].status)//make the outside tenpin invisible
{
Bodies[i].status=TENPIN_STATE_OUTSIDE;
}
if(Bodies[i].vPosition.x<-(10<<PHT_EXT)||
Bodies[i].vPosition.x>(10<<PHT_EXT)||
Bodies[i].vPosition.y>(LENGTH_BLOCK_BACK))// out of the box
{
// Bodies[i].status=TENPIN_STATE_OUTSIDE;
}
if(Bodies[i].status == TENPIN_STATE_STAND)
{
Bodies[i].vPosition.z=COLLISION_TENPIN_Z_DOWN;
}
if(Bodies[i].status==TENPIN_STATE_MOVING&&
isVelAndAngIn(i,5,5)&&
abs(Bodies[i].R.e23)<10&&
abs(Bodies[i].R.e13)<10)//tenpin status is moving but it is stand still. make it stand
{
Vector vNull;
vNull.x = 0;
vNull.y = 0;
vNull.z = 0;
Bodies[i].vVelocity=vNull;
Bodies[i].vAngularVelocity=vNull;
if(Bodies[i].vPosition.z<RADIUS_TENPIN)
{
Bodies[i].vPosition.z=COLLISION_TENPIN_Z_DOWN;
}
Bodies[i].status=TENPIN_STATE_STAND;
}
}
}
bool Physics::isTenpinShaking(int Id){
if(Id<ID_TENPIN_0||Id>ID_TENPIN_9)return false;
bool bvel,bang,bx,by,bz,bdir;
// velocity check
bvel=(Bodies[Id].vVelocity.x<SHAKING_VELOCITY_X)&&(Bodies[Id].vVelocity.y<SHAKING_VELOCITY_Y)&&(Bodies[Id].vVelocity.z<SHAKING_VELOCITY_Z);
// direction check
bdir=((abs(Bodies[Id].R.e23)<2000))&&((abs(Bodies[Id].R.e13)<2000));//stand
// angular velocity check
bang=(Bodies[Id].vAngularVelocity.x<SHAKING_ANGVELOCITY_X)&&(Bodies[Id].vAngularVelocity.y<SHAKING_ANGVELOCITY_Y)&&(Bodies[Id].vAngularVelocity.z<SHAKING_ANGVELOCITY_Z);
// position check
bx=(Bodies[Id].vPosition.x>BLOCKS_POSITION_0_X&&Bodies[Id].vPosition.x<(-BLOCKS_POSITION_0_X));
by=(Bodies[Id].vPosition.y>BLOCKS_POSITION_0_Y&&Bodies[Id].vPosition.y<(BLOCKS_POSITION_0_Y+BLOCKS_LENGTH));
bz=(Bodies[Id].vPosition.z>0);
return bx&&by&&bz&&bvel&&bang&&bdir&&(Bodies[Id].status==TENPIN_STATE_MOVING);
}
void Physics::setBallWeight(int weight){// set the weight of the tenpin
if(weight<0) weight=0;
if(weight>4) weight=4;
Bodies[ID_BALL].fMass=WEIGHT_BALL[weight];
}
void Physics::noSink()
{
int i;
int check;
pCollision pCollisionData=Collisions;
for (i=ID_TENPIN_0;i<ID_TENPIN_10;i++)
{
//clear collision data
NumCollisions=0;
check=UNCHECKED;
MEMSET(pCollisionData,0,NUM_COLLISION*sizeof(Collision));
if(Bodies[i].status==TENPIN_STATE_STAND||Bodies[i].status==TENPIN_STATE_MOVING){
//the object is tenpin
check=dCollideCylinderPlane(i,pCollisionData);
}
if ((Collisions[0].vCollisionPoint.z<-COLLISIONTOLERANCE))
{// the object is under the ground and is going down
if(Bodies[i].status != TENPIN_STATE_STAND&&Bodies[i].status != TENPIN_STATE_BACK)
{
Bodies[i].vPosition.z-=Collisions[0].vCollisionPoint.z;//Bodies[i].vPosition.z=0;
Bodies[i].vVelocity.z/=8;
}
}
}
}
boolean Physics::isVelAndAngIn(int bodyID, int lmtVel, int lmtAng)// test if the velocity and angular velocity is in the limited range
{
return (abs(Bodies[bodyID].vVelocity.x)<lmtVel)&&
(abs(Bodies[bodyID].vVelocity.y)<lmtVel)&&
(abs(Bodies[bodyID].vVelocity.z)<lmtVel)&&
(abs(Bodies[bodyID].vAngularVelocity.x)<lmtAng)&&
(abs(Bodies[bodyID].vAngularVelocity.y)<lmtAng)&&
(abs(Bodies[bodyID].vAngularVelocity.z)<lmtAng);
}
//------------------------------------------------------------------------//
//
//------------------------------------------------------------------------//
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -