Object *InitObject(char *objfile)
{
int nf;
Object *obj;
if((obj = LoadObject(objfile)) != NULL){
SetConnectivity(obj);
for(nf=0; nf < obj->nFaces; nf++)
CalculatePlane(obj, nf);
}
return obj;
}
bool EpaFace::Initialize()
{
assert( m_pHalfEdge && "Must setup half-edge first!" );
CollectVertices( m_pVertices );
const btVector3 e0 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
const btVector3 e1 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
const btScalar e0Sqrd = e0.length2();
const btScalar e1Sqrd = e1.length2();
const btScalar e0e1 = e0.dot( e1 );
m_determinant = e0Sqrd * e1Sqrd - e0e1 * e0e1;
const btScalar e0v0 = e0.dot( m_pVertices[ 0 ]->m_point );
const btScalar e1v0 = e1.dot( m_pVertices[ 0 ]->m_point );
m_lambdas[ 0 ] = e0e1 * e1v0 - e1Sqrd * e0v0;
m_lambdas[ 1 ] = e0e1 * e0v0 - e0Sqrd * e1v0;
if ( IsAffinelyDependent() )
{
return false;
}
CalcClosestPoint();
#ifdef EPA_POLYHEDRON_USE_PLANES
if ( !CalculatePlane() )
{
return false;
}
#endif
return true;
}
///////////////////
// 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;
}
}
///////////////////
// Update the wheel position
void Car_UpdateWheel(carsim_t *psCar, CModel *pcTrack, int id)
{
wheel_t *w = &psCar->sWheels[id];
CVec relpos = w->cRelPos;
float p[3], t1[9], t2[9];
plane_t plane;
// Calculate the wheel 'top' pos
CVec wheelPos = psCar->X*relpos.GetX() + psCar->Y*relpos.GetY() + psCar->Z*relpos.GetZ() + psCar->cPos;
// Convert the wheel in car coords
relpos -= CVec(0,0,w->fRestLength);
w->cPos = psCar->X*relpos.GetX() + psCar->Y*relpos.GetY() + psCar->Z*relpos.GetZ() + psCar->cPos;
// This is for the rendering
w->fWheelZ = -w->fRestLength;
// Defaults
w->fSuspLength = w->fRestLength;
w->fPistonVelocity = 0;
// Check for collisions
p[0] = w->cPos.GetX(); p[1] = w->cPos.GetY(); p[2] = w->cPos.GetZ();
w->bCollision = false;
float pDist = 0;
if( pcTrack->getCDModel()->sphereCollision( p, w->fRadius ) ) {
float top=999999;
for(int n=0; n<pcTrack->getCDModel()->getNumCollisions(); n++) {
pcTrack->getCDModel()->getCollidingTriangles(n, t1,t2,false);
plane = CalculatePlane(CVec(t1[0],t1[1],t1[2]),
CVec(t1[3],t1[4],t1[5]),
CVec(t1[6],t1[7],t1[8]));
// If the normal is too great for the tyre, ignore the collision
if(DotProduct(plane.vNormal,psCar->Z) < 0.5f)
continue;
// Find the plane that is raised towards the wheel center the most
if( DotProduct(wheelPos, plane.vNormal) + plane.fDistance < top) {
// TODO: Check if the tyre is not too high (ie, over the suspension joint)
top = DotProduct(wheelPos, plane.vNormal) + plane.fDistance;
w->cNormal = plane.vNormal;
pDist = plane.fDistance;
}
w->bCollision = true;
}
}
w->fSpeed = 0;
// If not colliding, just leave
if(!w->bCollision)
return;
// Make the tyre sit on top of the road
float d = DotProduct(w->cPos, w->cNormal) + pDist;
w->fSuspLength = d;
relpos = w->cRelPos - CVec(0,0,w->fSuspLength);
w->fWheelZ = -w->fSuspLength;
w->cPos = psCar->X*relpos.GetX() + psCar->Y*relpos.GetY() + psCar->Z*relpos.GetZ() + psCar->cPos;
// Get the piston velocity for the suspension
CVec vel = CrossProduct(psCar->cPos-w->cPos, psCar->cAngVelocity) + psCar->cVelocity;
w->fPistonVelocity = (psCar->Z * DotProduct(psCar->Z,vel)).GetZ();
// Calculate the rotation speed of the tyre
CVec s = psCar->Y * DotProduct(psCar->cVelocity, psCar->Y);
w->fSpeed = VectorLength(s);
if(DotProduct(psCar->cVelocity, psCar->Y) > 0)
w->fSpin += w->fSpeed / w->fRadius;
else
w->fSpin -= w->fSpeed / w->fRadius;
// Rotation is based on the Torque from the engine
//w->fSpin += w->fTorque / w->fRadius;
// If the torque is greater then the slip torque for the surface, the wheel is slipping and the engine force
// is less
// Note: The slip torque is constant for now
if(id == 1) {
//tMainSR3.f1 = w->fTorque;
}
if(w->fTorque > 100) {
// Slipping
w->fEngineLoad = w->fTorque/50;
w->bSlip = true;
} else {
w->bSlip = false;
w->fEngineLoad = 0;
}
w->fSpin = LimitAngle(w->fSpin);
}
void Poly::SortVerticesCW ( )
{
//
// Calculate center of polygon
//
Vector3 center;
for ( int i = 0; i < GetNumberOfVertices ( ); i++ )
{
center = center + verts[ i ].p;
}
center = center / GetNumberOfVertices ( );
//
// Sort vertices
//
for ( i = 0; i < GetNumberOfVertices ( ) - 2; i++ )
{
Vector3 a;
Plane p;
double SmallestAngle = -1;
int Smallest = -1;
a = verts[ i ].p - center;
a.Normalize ( );
p.PointsToPlane ( verts[ i ].p, center, center + plane.n );
for ( int j = i + 1; j < GetNumberOfVertices ( ); j++ )
{
if ( p.ClassifyPoint ( verts[ j ].p ) != Plane::eCP::BACK )
{
Vector3 b;
double Angle;
b = verts[ j ].p - center;
b.Normalize ( );
Angle = a.Dot ( b );
if ( Angle > SmallestAngle )
{
SmallestAngle = Angle;
Smallest = j;
}
}
}
if ( Smallest == -1 )
{
cout << "Error: Degenerate polygon!" << endl;
abort ( );
}
Vertex t = verts[ Smallest ];
verts[ Smallest ] = verts[ i + 1 ];
verts[ i + 1 ] = t;
}
//
// Check if vertex order needs to be reversed for back-facing polygon
//
Plane oldPlane = plane;
CalculatePlane ( );
if ( plane.n.Dot ( oldPlane.n ) < 0 )
{
int j = GetNumberOfVertices ( );
for ( int i = 0; i < j / 2; i++ )
{
Vertex v = verts[ i ];
verts[ i ] = verts[ j - i - 1 ];
verts[ j - i - 1 ] = v;
}
}
}
