///////////////////
// Check Resolve a collision between 2 cars
void Car_CheckCarCollisions(CCar *pcCar1, CCar *pcCar2)
{
float m1[16], m2[16];
carsim_t *psCar1 = pcCar1->getCarSim();
carsim_t *psCar2 = pcCar2->getCarSim();
// First, do a quick check to make sure the cars are within a small
// distance of each other using a seperating axis check
if( fabs(psCar1->cPos.GetX() - psCar2->cPos.GetX()) > 50 )
return;
if( fabs(psCar1->cPos.GetY() - psCar2->cPos.GetY()) > 50 )
return;
if( fabs(psCar1->cPos.GetZ() - psCar2->cPos.GetZ()) > 50 )
return;
// Clear it
for(int i=0;i<16;i++) {
m1[i] = 0;
m2[i] = 0;
}
for(i=0;i<3;i++) {
m1[i] = psCar1->X.GetIndex(i);
m2[i] = psCar2->X.GetIndex(i);
}
for(i=4;i<7;i++) {
m1[i] = psCar1->Y.GetIndex(i-4);
m2[i] = psCar2->Y.GetIndex(i-4);
}
for(i=8;i<11;i++) {
m1[i] = psCar1->Z.GetIndex(i-8);
m2[i] = psCar2->Z.GetIndex(i-8);
}
for(i=12;i<15;i++) {
m1[i] = psCar1->cPos.GetIndex(i-12);
m2[i] = psCar2->cPos.GetIndex(i-12);
}
m1[15] = 1;
m2[15] = 1;
// Rase it a bit
m1[14] += 2;
m2[14] += 2;
CollisionModel3D *col = pcCar1->getColMod();
CollisionModel3D *col2 = pcCar2->getColMod();
col->setTransform( m1 );
//col->setTransform( m2 );
CVec relVel = psCar1->cVelocity - psCar2->cVelocity;
CVec posNorm = psCar1->cPos - psCar2->cPos;
VectorNormalize(&posNorm);
// Check for collision
if(col->collision( col2, -1, 0, m2 )) {
float t1[9], t2[9];
plane_t plane1, plane2;
for(int n=0; n<col->getNumCollisions(); n++) {
// Get collision normal
col->getCollidingTriangles(n,t1,t2,false);
plane2 = CalculatePlane(CVec(t1[0],t1[1],t1[2]),
CVec(t1[3],t1[4],t1[5]),
CVec(t1[6],t1[7],t1[8]));
plane1 = CalculatePlane(CVec(t2[0],t2[1],t2[2]),
CVec(t2[3],t2[4],t2[5]),
CVec(t2[6],t2[7],t2[8]));
//plane1.vNormal.SetZ( 0 );//MIN(plane1.vNormal.GetZ(), 0) );
//plane2.vNormal.SetZ( 0 );//MIN(plane2.vNormal.GetZ(), 0) );
// Make sure we are going into the plane
//if(DotProduct(psCar1->cVelocity, plane1.vNormal) > 0)
//continue;
// Check to make sure the car position is the good side of the plane
//if(DotProduct(psCar1->cPos,plane1.vNormal) + plane1.fDistance < 0)
//continue;
//float nRV = DotProduct(relVel, posNorm);
//if(nRV > 0)
//continue;
break;
}
// Car 1
CVec norm = plane1.vNormal;
float dot = DotProduct(plane1.vNormal, psCar1->cVelocity);
norm = norm * dot;
CVec vt = psCar1->cVelocity - norm;
float coef = 0.01f;
// Ball-on-ball collision
CVec norm2 = norm * coef;
psCar1->cVelocity = vt + norm2;
norm2 = norm * (1.0f - coef);
psCar2->cVelocity += norm2;
// Car 2
/*norm = plane2.vNormal;
dot = DotProduct(plane2.vNormal, psCar2->cVelocity);
norm = norm * dot;
vt = psCar2->cVelocity - norm;
coef = 0.01f;
// Ball-on-ball collision
norm2 = norm * coef;
psCar2->cVelocity = vt + norm2;
norm2 = norm * (1.0f - coef);
psCar1->cVelocity += norm2;*/
}
}
///////////////////
// Check collisions between the car & track
void Car_CheckCollisions(carsim_t *psCar, CCar *pcCar, CModel *pcTrack)
{
int i;
float m[16], t1[9], t2[9];
float p[3];
// Clear it
for(i=0;i<16;i++)
m[i] = 0;
for(i=0;i<3;i++)
m[i] = psCar->X.GetIndex(i);
for(i=4;i<7;i++)
m[i] = psCar->Y.GetIndex(i-4);
for(i=8;i<11;i++)
m[i] = psCar->Z.GetIndex(i-8);
for(i=12;i<15;i++)
m[i] = psCar->cPos.GetIndex(i-12);
m[15] = 1;
// Raise the car up a bit
m[14] += 2;
psCar->bCollision = false;
CollisionModel3D *col = pcCar->getColMod();//pcCar->getModel()->GetCDModel();
col->setTransform(m);
if(col->collision( pcTrack->getCDModel(),-1,500 )) {
// Get collision point
//carcol->getCollisionPoint(v1);
//for(i=0;i<3;i++) cont->Pos.SetIndex(i,v1[i]);
//cont->Pos = cont->Pos + offset;
CVec oldVel = psCar->cVelocity;
CVec oldAng = psCar->cAngVelocity;
for(int n=0; n<col->getNumCollisions(); n++) {
// Get collision normal
col->getCollidingTriangles(n,t1,t2,false);
col->getCollisionPoint(n,p,false);
plane_t plane = CalculatePlane(CVec(t2[0],t2[1],t2[2]),
CVec(t2[3],t2[4],t2[5]),
CVec(t2[6],t2[7],t2[8]));
// Resolve the collision
// Check to make sure the plane we're rebounding off is facing opposite the velocity
if(DotProduct(psCar->cVelocity, plane.vNormal) > 0)
continue;
// Check to make sure the car position is the good side of the plane
if(DotProduct(psCar->cPos,plane.vNormal) + plane.fDistance < 0)
continue;
float Mass = 2000;
float coef = 0.2f;
// If the velocity against the mesh is too low, make sure we fully bounce back so we don't slowly
// go into the mesh
if( DotProduct(psCar->cVelocity, plane.vNormal) > -1)
coef = 1.0f;
psCar->bCollision = true;
psCar->cColNormal = plane.vNormal;
psCar->cColPoint = CVec(p[0], p[1], p[2]);
// Linear velocity rebound
float j = DotProduct(psCar->cVelocity * -(1+coef),plane.vNormal) / DotProduct(plane.vNormal,plane.vNormal * (1/Mass));
psCar->cVelocity += plane.vNormal * (j*(1/Mass));
// Angular velocity rebound
float pnt[3];
col->getCollisionPoint(n, pnt);
//CVec p = CVec(pnt[0],pnt[1],pnt[2]);
CVec p = psCar->X*pnt[0] + psCar->Y*pnt[1] + psCar->Z*pnt[2];
CVec v = CrossProduct(oldAng, p) + oldVel;
if(DotProduct(plane.vNormal,v) > EPSILON)
continue;
if(DotProduct(p+psCar->cPos,plane.vNormal) + plane.fDistance > EPSILON)
continue;
v = CrossProduct(p, plane.vNormal*-DotProduct(v,plane.vNormal) * 0.01f);
//psCar->cAngVelocity += v;
}
//rbody.ResolveCollisions();
//return true;
}
}
