📄 physics.cpp
字号:
#include "Physics.h"
#include "mymath.h"
#include "../game/def.h"
#include "stdio.h"
//------------------------------------------------------------------------//
// This function sets the initial state of the objects
//------------------------------------------------------------------------//
void Physics::InitializeObjects(int const ((*pInitTenpinPosition)[2]))
{
AngDegree iRoll, iPitch, iYaw;
int i;
//clear the object
isCollided=0;
//MEMSET(Bodies,0,NUM_TOTAL*sizeof(RigidBody));//error! why??
for(i=0;i<NUM_TOTAL;i++)
MEMSET(&Bodies[i],0,sizeof(RigidBody));
//set tenpin position
int const((*p)[2])=pInitTenpinPosition;
for (i=0;i<10;i++){
Bodies[i+1].vPosition.x=((*(*(p+i) ))<<PHT_EXT)/10;
Bodies[i+1].vPosition.y=((*(*(p+i)+1))<<PHT_EXT)/10;
Bodies[i+1].vPosition.z =COLLISION_TENPIN_Z_DOWN;
}
//set the tenpin and ball initial orientation
for (i=0;i<ID_TENPIN_10;i++){
// Set tenpin and ball initial orientation
iRoll = 0;
iPitch = 0;
iYaw = 0;
Bodies[i].qOrientation = MakeQFromEulerAngles(iRoll, iPitch, iYaw);
Bodies[i].qOrientation.Normalize();
Bodies[i].R = MakeMatrixFromQuaternion(Bodies[i].qOrientation);
if(i!=ID_BALL){
//set tenpin mass property
setMassProperty(&(Bodies[i]),false);
setCollisionData(&(Bodies[i]),false);
}
else{
setMassProperty(&(Bodies[i]),true);
setCollisionData(&(Bodies[i]),true);
}
}
}
void Physics::setMassProperty(RigidBody* obj, boolean isBall){
if(isBall){
obj->fMass=MASS_BALL;
obj->mInertia.e11 = INERTIA_XX_BALL;
obj->mInertia.e22 = INERTIA_YY_BALL;
obj->mInertia.e33 = INERTIA_ZZ_BALL;
}else{
obj->fMass =MASS_TENPIN_UP+MASS_TENPIN_DOWN;
obj->mInertia.e11 = INERTIA_XX_TENPIN;
obj->mInertia.e22 = INERTIA_YY_TENPIN;
obj->mInertia.e33 = INERTIA_ZZ_TENPIN*3+INERTIA_ZZ_TENPIN/2;
}
// Set the mass properties
obj->mInertia.e12 = 0; obj->mInertia.e13 = 0;
obj->mInertia.e21 = 0; obj->mInertia.e23 = 0;
obj->mInertia.e31 = 0; obj->mInertia.e32 = 0;
obj->mInertiaInverse =obj->mInertia.Inverse();
}
void Physics::setCollisionData (RigidBody* obj,boolean isBall){
obj->status=TENPIN_STATE_STAND;
if(isBall){
obj->fRadius=RADIUS_BALL;
}else{
obj->fRadius=(COLLISION_TENPIN_Z_UP>COLLISION_TENPIN_Z_DOWN)?COLLISION_TENPIN_Z_UP:COLLISION_TENPIN_Z_DOWN;
}
}
//------------------------------------------------------------------------//
// This function calculates all of the forces and moments acting on the
// objects at any given time.
//------------------------------------------------------------------------//
void Physics::CalcObjectForces(void)
{
int i;
int v;
for(i=0; i<NUMBODIES; i++)
{
if(Bodies[i].status==TENPIN_STATE_STAND||Bodies[i].status==TENPIN_STATE_MOVING||Bodies[i].status==TENPIN_STATE_BACK||Bodies[i].status==TENPIN_STATE_SIDE_OUT)
{
// reset forces and moments:
Bodies[i].vForces.x = 0;
Bodies[i].vForces.y = 0;
Bodies[i].vForces.z = 0;
Bodies[i].vMoments.x = 0;
Bodies[i].vMoments.y = 0;
Bodies[i].vMoments.z = 0;
v=Bodies[i].vVelocity.Magnitude();
// Apply gravity
if(Bodies[i].status==TENPIN_STATE_BACK)
{
Bodies[i].vForces.z += (6*GRAVITY * Bodies[i].fMass)>>PHT_EXT;
}
else
Bodies[i].vForces.z += (GRAVITY * Bodies[i].fMass)>>PHT_EXT;
if (Bodies[i].status==TENPIN_STATE_SIDE_OUT)
{
Bodies[i].vForces.y += ((-GRAVITY* Bodies[i].fMass)>>PHT_EXT);
}
if(Bodies[i].status==TENPIN_STATE_MOVING)
{
int inx=v/200;
//SYS_ASSERT(inx<SPEED_Y_AMP_MAX);
inx=inx<SPEED_Y_AMP_MAX?inx:(SPEED_Y_AMP_MAX-1);
Bodies[i].vForces.y+=v/SPEED_Y_AMP[inx];
}
// Handle contacts with ground plane
Bodies[i].vAcceleration = Bodies[i].vForces / Bodies[i].fMass;
Bodies[i].vAngularAcceleration = Bodies[i].mInertiaInverse *
(Bodies[i].vMoments -
(Bodies[i].vAngularVelocity^
(Bodies[i].mInertia * Bodies[i].vAngularVelocity)));
}
}
//resolve the ground contact
resolveGroundCollisions();
}
//------------------------------------------------------------------------//
// Using Euler's method
//------------------------------------------------------------------------//
void Physics::StepSimulation(PhType dtime)
{
Vector Ae;
int i;
int start=0;
//setBallWeight(1);
// Calculate all of the forces and moments
CalcObjectForces();
if((Bodies[0].vPosition.x>=GUTTER_WIDTH_X||Bodies[0].vPosition.x<=-GUTTER_WIDTH_X)){//gutter
if(Bodies[0].vPosition.y<=0)
Bodies[0].vPosition.y=50;
Bodies[0].vPosition.y+=(Bodies[0].vVelocity.y * dtime)>>PHT_EXT;
Bodies[0].vPosition.x=(Bodies[0].vPosition.x>0?1:-1)*GUTTER_POSITION_X;
Bodies[0].vPosition.z=GUTTER_POSITION_Z;
if(Bodies[ID_BALL].vPosition.y>=(LENGTH_BLOCK+(4<<PHT_EXT)))//back out
{
Bodies[ID_BALL].vPosition.y=(LENGTH_BLOCK+(4<<PHT_EXT));
Bodies[ID_BALL].vPosition.z=-500;
Bodies[ID_BALL].vVelocity=Vector(0,0,0);
Bodies[ID_BALL].status=BALL_INVALID;
}
start=1;
//return;
}
// Integrate
for(i=start; i<NUMBODIES; i++)
{
if(Bodies[i].status==TENPIN_STATE_STAND||Bodies[i].status==TENPIN_STATE_MOVING||Bodies[i].status==TENPIN_STATE_BACK||Bodies[i].status==TENPIN_STATE_SIDE_OUT){
// calculate the acceleration in earth space:
Ae = Bodies[i].vForces / Bodies[i].fMass;
Bodies[i].vAcceleration = Ae;
// calculate the velocity in earth space:
Bodies[i].vVelocity += (Ae * dtime);
// calculate the position in earth space:
Bodies[i].vPosition += (Bodies[i].vVelocity * dtime);
Bodies[i].vAngularAcceleration = (Bodies[i].mInertiaInverse *
(Bodies[i].vMoments -
(Bodies[i].vAngularVelocity^
(Bodies[i].mInertia * Bodies[i].vAngularVelocity))));
Bodies[i].vAngularVelocity += (Bodies[i].vAngularAcceleration * dtime);
// calculate the new rotation quaternion:
Bodies[i].qOrientation += ((Bodies[i].qOrientation * Bodies[i].vAngularVelocity) *
(dtime/2));
// now normalize the orientation quaternion:
Bodies[i].qOrientation.Normalize();
// calculate the velocity in body space:
// (we'll need this to calculate lift and drag forces)
Bodies[i].vVelocityBody = QVRotate(~Bodies[i].qOrientation, Bodies[i].vVelocity);
// get the angular velocity in global coords:
Bodies[i].vAngularVelocityGlobal = QVRotate(Bodies[i].qOrientation, Bodies[i].vAngularVelocity);
// get the angular acceleration in global coords:
Bodies[i].vAngularAccelerationGlobal = QVRotate(Bodies[i].qOrientation, Bodies[i].vAngularAcceleration);
// Get the inverse inertia tensor in global coordinates
Matrix3x3 R, RT;
R = MakeMatrixFromQuaternion(Bodies[i].qOrientation);
RT = R.Transpose();
Bodies[i].mIeInverse = ((R * Bodies[i].mInertiaInverse) * RT);
Bodies[i].R=R;
// get the Euler angles for our information
Vector u;
u = MakeEulerAnglesFromQ(Bodies[i].qOrientation);
Bodies[i].vEulerAngles.x = u.x; // roll
Bodies[i].vEulerAngles.y = u.y; // pitch
Bodies[i].vEulerAngles.z = u.z; // yaw
}
limitation(i);
/* if(isTenpinShaking(i)){
DBGPRINTF("Id= %d status is %s",i, "shaking");
}
DBGPRINTF("Id= %d status is %s",i,
((Bodies[i].status==TENPIN_STATE_STAND)?"STAND":
((Bodies[i].status==TENPIN_STATE_MOVING)?"MOVING":
((Bodies[i].status==TENPIN_STATE_DEAD)?"DEAD":
((Bodies[i].status==TENPIN_STATE_BACK)?"BACK":
((Bodies[i].status==TENPIN_STATE_OUTSIDE)?"outside":"ERROR"))))));
*/
}
// Handle Collisions
if(checkForCollisions()==HAVE_COLLISION){
ResolveCollisions();
}
noSink();
}
void Physics::resolveGroundCollisions(){
int check=UNCHECKED;
Vector ContactForce,FrictionForce;
Vector pt,pt1;
Vector n,vel1;
PhType Vrn,Arn,J,Vrt;
int i,j;
pCollision pCollisionData;
PhType mu = FRICTIONCOEFFICIENT;
// bool dofriction = DOFRICTION;
PhType fCr;
Vector vtmp;
pCollisionData=Collisions;
for(i=0;i<TOTAL_OBJECT;i++){
//clear collision data
NumCollisions=0;
check=UNCHECKED;
MEMSET(pCollisionData,0,NUM_COLLISION*sizeof(Collision));
//check the collision or contact with the ground
if(Bodies[i].status==TENPIN_STATE_STAND||Bodies[i].status==TENPIN_STATE_MOVING){
if(ID_BALL==i){//the object is ball
check=dCollideSpherePlane(i,pCollisionData);
}
else{//the object is tenpin
check=dCollideCylinderPlane(i,pCollisionData);
}
}
//resolve the ground contact and collision
if(check == CONTACT||check==HAVE_COLLISION){//have the contact or collision
for (j=0;j<NumCollisions;j++){
if(Collisions[j].vCollisionPoint.z<COLLISIONTOLERANCE){
Vrn = Collisions[j].vRelativeVelocity * Collisions[j].vCollisionNormal;//relative speed
if(abs(Vrn)<=VELOCITYTOLERANCE){//the object is rest
// Check the relative acceleration
Arn = Collisions[j].vRelativeAcceleration * Collisions[j].vCollisionNormal;
if(Arn <= 0){// the acceleration is minus, the object is towards the ground
//resolve the contact
ContactForce = (((Bodies[i].fMass/NumCollisions) * ((-Collisions[j].vRelativeAcceleration)*Collisions[j].vCollisionNormal))>>PHT_EXT) * Collisions[j].vCollisionNormal;
FrictionForce = ((ContactForce.Magnitude()*FRICTIONCOEFFICIENT)>>PHT_EXT)*Collisions[j].vCollisionTangent;
Bodies[i].vForces += (ContactForce+FrictionForce);
//Bodies[i].vForces += ContactForce;
//Bodies[i].vForces += FrictionForce;
ContactForce = QVRotate(~Bodies[i].qOrientation, ContactForce);
FrictionForce = QVRotate(~Bodies[i].qOrientation, FrictionForce);
pt = Collisions[j].vCollisionPoint - Bodies[i].vPosition;
pt = QVRotate(~Bodies[i].qOrientation, pt);
Bodies[i].vMoments +=(pt^(ContactForce+FrictionForce))*PERCENT_GROUND_MOVEMENT_COEFFICIENT;
//Bodies[i].vMoments += (pt^ContactForce)*PERCENT_GROUND_MOVEMENT_COEFFICIENT;
//Bodies[i].vMoments += (pt^FrictionForce)*PERCENT_GROUND_MOVEMENT_COEFFICIENT;
}
}
if ((Vrn<=-CONTACTTOLERANCE)&&(Bodies[i].status==TENPIN_STATE_MOVING||Bodies[i].status==TENPIN_STATE_SIDE_OUT)){// the velocity is minus, the object is collide with ground
fCr = COEFFICIENTOFRESTITUTIONGROUND;
pt1 = Collisions[j].vCollisionPoint - Bodies[i].vPosition;
// calculate impulse
J = (-((1<<PHT_EXT)+fCr) * (Collisions[j].vRelativeVelocity*Collisions[j].vCollisionNormal)) /
( ((1<<(PHT_EXT*2))/Bodies[i].fMass) +
(Collisions[j].vCollisionNormal * (((pt1^Collisions[j].vCollisionNormal)*Bodies[i].mIeInverse)^pt1)));
Vrt = Collisions[j].vRelativeVelocity * Collisions[j].vCollisionTangent;
Bodies[i].vVelocity += (((J*Collisions[j].vCollisionNormal) + (((mu * J)>>PHT_EXT) * Collisions[j].vCollisionTangent)) / Bodies[i].fMass);
Bodies[i].vVelocity=Bodies[i].vVelocity*PERCENT_ENERGY_GROUND_VEL;
Bodies[i].vAngularVelocityGlobal += ((pt1^((J * Collisions[j].vCollisionNormal) + (((mu * J)>>PHT_EXT) * Collisions[j].vCollisionTangent)))*Bodies[i].mIeInverse);
Bodies[i].vAngularVelocityGlobal=Bodies[i].vAngularVelocityGlobal*PERCENT_ENERGY_GROUND_ANG;
Bodies[i].vAngularVelocity = QVRotate(~Bodies[i].qOrientation, Bodies[i].vAngularVelocityGlobal);
}
}
}
}
}
}
//check the collision
int Physics::checkForCollisions(){
int status=UNCHECKED;
int checked=UNCHECKED;
int i,j;
pCollision pCollisionData;
Vector d;
PhType dm,rm,r;
int ContactCount;
pCollisionData=Collisions;
//clear the collision data
for(i=0;i<TOTAL_OBJECT;i++){for(j=0;j<TOTAL_OBJECT;j++){colState[i][j]=UNCHECKED;}}
NumCollisions=0;
MEMSET(pCollisionData,0,NUM_COLLISION*sizeof(Collision));
for(i=0;i<TOTAL_OBJECT;i++){
for (j=0;j<TOTAL_OBJECT;j++){
status=UNCHECKED;
ContactCount=0;
if (i!=j&&
((Bodies[i].status==TENPIN_STATE_STAND||Bodies[i].status==TENPIN_STATE_MOVING||Bodies[i].status==TENPIN_STATE_SIDE_OUT)&&
(Bodies[j].status==TENPIN_STATE_STAND||Bodies[j].status==TENPIN_STATE_MOVING||Bodies[i].status==TENPIN_STATE_SIDE_OUT))
){//do not do collision between body[i] and body[i]
if(colState[i][j]==UNCHECKED||colState[j][i]==UNCHECKED){//if body[i] and body[j] haven't checked
//check the sphere sphere first
d = Bodies[i].vPosition - Bodies[j].vPosition;
dm=((d.x*d.x)>>PHT_EXT)+((d.y*d.y)>>PHT_EXT)+((d.z*d.z)>>PHT_EXT);
r=Bodies[i].fRadius + Bodies[j].fRadius;
rm=r*r>>PHT_EXT;
if(dm<rm){//object maybe collision
if (i==ID_BALL || j==ID_BALL){//one object is ball
if (i==ID_BALL)
status=dCollideCylinderSphere(j,i,pCollisionData,ContactCount);
else
status=dCollideCylinderSphere(i,j,pCollisionData,ContactCount);
}
else{
status=dCollideCylinders(i,j,pCollisionData,ContactCount);
}
if(status==HAVE_COLLISION){//collision
Bodies[i].status=TENPIN_STATE_MOVING;
Bodies[j].status=TENPIN_STATE_MOVING;
if(i==ID_BALL||j==ID_BALL)
isCollided=1;
if (NumCollisions<=NUM_COLLISION){
pCollisionData+=ContactCount;
checked=HAVE_COLLISION;
}
else{//error
// DBGPRINTF("error: max collision data reached!!");
return checked;
}
}
}
//set the status of collision
colState[i][j]=status; colState[j][i]=status;
}
}
}
}
//check for collision with board
for(i=0;i<TOTAL_OBJECT;i++){
status=UNCHECKED;
ContactCount=0;
if (Bodies[i].status==TENPIN_STATE_MOVING){
status=dCollideCylinderBlock(i,pCollisionData,ContactCount);
}
if(status){//collision
if (NumCollisions<=NUM_COLLISION){
pCollisionData+=ContactCount;
checked=HAVE_COLLISION;
}
else{//error
// DBGPRINTF("error: max collision data reached!!");
return checked;
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -