void
SimCarCollideCars(tSituation *s)
{
tCar *car;
tCarElt *carElt;
int i;
for (i = 0; i < s->_ncars; i++) {
carElt = s->cars[i];
if (carElt->_state & RM_CAR_STATE_NO_SIMU) {
continue;
}
car = &(SimCarTable[carElt->index]);
dtSelectObject(car);
// Fit the bounding box around the car, statGC's are the static offsets.
dtLoadIdentity();
dtTranslate(-carElt->_statGC_x, -carElt->_statGC_y, 0.0f);
// Place the bounding box such that it fits the car in the world.
dtMultMatrixf((const float *)(carElt->_posMat));
memset(&(car->VelColl), 0, sizeof(tPosd));
}
// Running the collision detection. If no collision is detected, call dtProceed.
// dtProceed just works if all objects are disjoint.
if (dtTest() == 0) {
dtProceed();
}
for (i = 0; i < s->_ncars; i++) {
carElt = s->cars[i];
if (carElt->_state & RM_CAR_STATE_NO_SIMU) {
continue;
}
car = &(SimCarTable[carElt->index]);
if (car->collision & SEM_COLLISION_CAR) {
car->DynGCg.vel.x = car->VelColl.x;
car->DynGCg.vel.y = car->VelColl.y;
car->rot_mom[SG_Z] = car->VelColl.az/car->Iinv.z;
car->DynGC.vel.az = car->DynGCg.vel.az = -2.0f*car->rot_mom[SG_Z] * car->Iinv.z;
}
}
}
// Collision response for walls.
// TODO: Separate common code with car-car collision response.
static void SimCarWallCollideResponse(void *clientdata, DtObjectRef obj1, DtObjectRef obj2, const DtCollData *collData)
{
tCar* car; // The car colliding with the wall.
float nsign; // Normal direction correction for collision plane.
sgVec2 p; // Cars collision point delivered by solid.
// TODO: If other movable objects are added which could collide with the wall, it will be
// necessary to validate if the object is actually a car.
if (obj1 == clientdata) {
car = (tCar*) obj2;
nsign = -1.0f;
p[0] = (float) collData->point2[0];
p[1] = (float) collData->point2[1];
} else {
car = (tCar*) obj1;
nsign = 1.0f;
p[0] = (float) collData->point1[0];
p[1] = (float) collData->point1[1];
}
sgVec2 n; // Collision normal delivered by solid, corrected such that it points away from the wall.
n[0] = nsign * (float) collData->normal[0];
n[1] = nsign * (float) collData->normal[1];
float pdist = sgLengthVec2(n); // Distance of collision points.
sgNormaliseVec2(n);
sgVec2 r;
sgSubVec2(r, p, (const float*)&(car->statGC));
tCarElt *carElt = car->carElt;
sgVec2 vp; // Speed of car collision point in global frame of reference.
sgVec2 rg; // raduis oriented in global coordinates, still relative to CG (rotated aroung CG).
float sina = sin(carElt->_yaw);
float cosa = cos(carElt->_yaw);
rg[0] = r[0]*cosa - r[1]*sina;
rg[1] = r[0]*sina + r[1]*cosa;
vp[0] = car->DynGCg.vel.x - car->DynGCg.vel.az * rg[1];
vp[1] = car->DynGCg.vel.y + car->DynGCg.vel.az * rg[0];
sgVec2 tmpv;
static const float CAR_MIN_MOVEMENT = 0.02f;
static const float CAR_MAX_MOVEMENT = 0.05f;
sgScaleVec2(tmpv, n, MIN(MAX(pdist, CAR_MIN_MOVEMENT), CAR_MAX_MOVEMENT));
if (car->blocked == 0) {
sgAddVec2((float*)&(car->DynGCg.pos), tmpv);
car->blocked = 1;
}
// Doing no dammage and correction if the cars are moving out of each other.
if (sgScalarProductVec2(vp, n) > 0) {
return;
}
float rp = sgScalarProductVec2(rg, n);
// Pesudo cross product to find out if we are left or right.
// TODO: SIGN, scrap value?
float rpsign = n[0]*rg[1] - n[1]*rg[0];
const float e = 1.0f; // energy restitution
float j = -(1.0f + e) * sgScalarProductVec2(vp, n) / (car->Minv + rp * rp * car->Iinv.z);
const float ROT_K = 0.5f;
// Damage.
tdble damFactor, atmp;
atmp = atan2(r[1], r[0]);
if (fabs(atmp) < (PI / 3.0)) {
// Front collision gives more damage.
damFactor = 1.5f;
} else {
// Rear collision gives less damage.
damFactor = 1.0f;
}
static const float DMGFACTOR = 0.00002f;
if ((car->carElt->_state & RM_CAR_STATE_FINISH) == 0) {
car->dammage += (int)(CAR_DAMMAGE * (DMGFACTOR*j*j) * damFactor * simDammageFactor[car->carElt->_skillLevel]);
}
sgScaleVec2(tmpv, n, j * car->Minv);
sgVec2 v2a;
if (car->collision & SEM_COLLISION_CAR) {
sgAddVec2(v2a, (const float*)&(car->VelColl.x), tmpv);
car->VelColl.az = car->VelColl.az + j * rp * rpsign * car->Iinv.z * ROT_K;
} else {
sgAddVec2(v2a, (const float*)&(car->DynGCg.vel), tmpv);
car->VelColl.az = car->DynGCg.vel.az + j * rp * rpsign * car->Iinv.z * ROT_K;
}
static float VELMAX = 3.0f;
if (fabs(car->VelColl.az) > VELMAX) {
car->VelColl.az = SIGN(car->VelColl.az) * VELMAX;
}
sgCopyVec2((float*)&(car->VelColl.x), v2a);
// Move the car for the collision lib.
sgMakeCoordMat4(carElt->pub.posMat, car->DynGCg.pos.x, car->DynGCg.pos.y,
car->DynGCg.pos.z - carElt->_statGC_z, RAD2DEG(carElt->_yaw),
RAD2DEG(carElt->_roll), RAD2DEG(carElt->_pitch));
dtSelectObject(car);
dtLoadIdentity();
dtTranslate(-carElt->_statGC_x, -carElt->_statGC_y, 0.0f);
dtMultMatrixf((const float *)(carElt->_posMat));
car->collision |= SEM_COLLISION_CAR;
}
static void SimCarCollideResponse(void * /*dummy*/, DtObjectRef obj1, DtObjectRef obj2, const DtCollData *collData)
{
sgVec2 n; // Collision normal delivered by solid: Global(point1) - Global(point2)
tCar *car[2]; // The cars.
sgVec2 p[2]; // Collision points delivered by solid, in body local coordinates.
sgVec2 r[2]; // Collision point relative to center of gravity.
sgVec2 vp[2]; // Speed of collision point in world coordinate system.
sgVec3 pt[2]; // Collision points in global coordinates.
int i;
car[0] = (tCar*)obj1;
car[1] = (tCar*)obj2;
// Handle cars collisions during pit stops as well.
static const int NO_SIMU_WITHOUT_PIT = RM_CAR_STATE_NO_SIMU & ~RM_CAR_STATE_PIT;
if ((car[0]->carElt->_state & NO_SIMU_WITHOUT_PIT) ||
(car[1]->carElt->_state & NO_SIMU_WITHOUT_PIT))
{
return;
}
if (car[0]->carElt->index < car[1]->carElt->index) {
// vector conversion from double to float.
p[0][0] = (float)collData->point1[0];
p[0][1] = (float)collData->point1[1];
p[1][0] = (float)collData->point2[0];
p[1][1] = (float)collData->point2[1];
n[0] = (float)collData->normal[0];
n[1] = (float)collData->normal[1];
} else {
// swap the cars (not the same for the simu).
car[0] = (tCar*)obj2;
car[1] = (tCar*)obj1;
p[0][0] = (float)collData->point2[0];
p[0][1] = (float)collData->point2[1];
p[1][0] = (float)collData->point1[0];
p[1][1] = (float)collData->point1[1];
n[0] = -(float)collData->normal[0];
n[1] = -(float)collData->normal[1];
}
sgNormaliseVec2(n);
sgVec2 rg[2]; // radius oriented in global coordinates, still relative to CG (rotated aroung CG).
tCarElt *carElt;
for (i = 0; i < 2; i++) {
// vector GP (Center of gravity to collision point). p1 and p2 are delivered from solid as
// points in the car coordinate system.
sgSubVec2(r[i], p[i], (const float*)&(car[i]->statGC));
// Speed of collision points, linear motion of center of gravity (CG) plus rotational
// motion around the CG.
carElt = car[i]->carElt;
float sina = sin(carElt->_yaw);
float cosa = cos(carElt->_yaw);
rg[i][0] = r[i][0]*cosa - r[i][1]*sina;
rg[i][1] = r[i][0]*sina + r[i][1]*cosa;
vp[i][0] = car[i]->DynGCg.vel.x - car[i]->DynGCg.vel.az * rg[i][1];
vp[i][1] = car[i]->DynGCg.vel.y + car[i]->DynGCg.vel.az * rg[i][0];
}
// Relative speed of collision points.
sgVec2 v1ab;
sgSubVec2(v1ab, vp[0], vp[1]);
// try to separate the cars. The computation is necessary because dtProceed is not called till
// the collision is resolved.
for (i = 0; i < 2; i++) {
sgCopyVec2(pt[i], r[i]);
pt[i][2] = 0.0f;
// Transform points relative to cars local coordinate system into global coordinates.
sgFullXformPnt3(pt[i], car[i]->carElt->_posMat);
}
// Compute distance of collision points.
sgVec3 pab;
sgSubVec2(pab, pt[1], pt[0]);
float distpab = sgLengthVec2(pab);
sgVec2 tmpv;
sgScaleVec2(tmpv, n, MIN(distpab, 0.05));
// No "for" loop here because of subtle difference AddVec/SubVec.
if (car[0]->blocked == 0 && !(car[0]->carElt->_state & RM_CAR_STATE_NO_SIMU)) {
sgAddVec2((float*)&(car[0]->DynGCg.pos), tmpv);
car[0]->blocked = 1;
}
if (car[1]->blocked == 0 && !(car[1]->carElt->_state & RM_CAR_STATE_NO_SIMU)) {
sgSubVec2((float*)&(car[1]->DynGCg.pos), tmpv);
car[1]->blocked = 1;
}
// Doing no dammage and correction if the cars are moving out of each other.
if (sgScalarProductVec2(v1ab, n) > 0) {
return;
}
// impulse.
float rpn[2];
rpn[0] = sgScalarProductVec2(rg[0], n);
rpn[1] = sgScalarProductVec2(rg[1], n);
// Pseudo cross product to find out if we are left or right.
// TODO: SIGN, scrap value?
float rpsign[2];
rpsign[0] = n[0]*rg[0][1] - n[1]*rg[0][0];
rpsign[1] = -n[0]*rg[1][1] + n[1]*rg[1][0];
const float e = 1.0f; // energy restitution
float j = -(1.0f + e) * sgScalarProductVec2(v1ab, n) /
((car[0]->Minv + car[1]->Minv) +
rpn[0] * rpn[0] * car[0]->Iinv.z + rpn[1] * rpn[1] * car[1]->Iinv.z);
for (i = 0; i < 2; i++) {
if (car[i]->carElt->_state & RM_CAR_STATE_NO_SIMU) {
continue;
}
// Damage.
tdble damFactor, atmp;
atmp = atan2(r[i][1], r[i][0]);
if (fabs(atmp) < (PI / 3.0)) {
// Front collision gives more damage.
damFactor = 1.5f;
} else {
// Rear collision gives less damage.
damFactor = 1.0f;
}
if ((car[i]->carElt->_state & RM_CAR_STATE_FINISH) == 0) {
car[i]->dammage += (int)(CAR_DAMMAGE * fabs(j) * damFactor * simDammageFactor[car[i]->carElt->_skillLevel]);
}
// Compute collision velocity.
const float ROT_K = 1.0f;
float js = (i == 0) ? j : -j;
sgScaleVec2(tmpv, n, js * car[i]->Minv);
sgVec2 v2a;
if (car[i]->collision & SEM_COLLISION_CAR) {
sgAddVec2(v2a, (const float*)&(car[i]->VelColl.x), tmpv);
car[i]->VelColl.az = car[i]->VelColl.az + js * rpsign[i] * rpn[i] * car[i]->Iinv.z * ROT_K;
} else {
sgAddVec2(v2a, (const float*)&(car[i]->DynGCg.vel), tmpv);
car[i]->VelColl.az = car[i]->DynGCg.vel.az + js * rpsign[i] * rpn[i] * car[i]->Iinv.z * ROT_K;
}
static float VELMAX = 3.0f;
if (fabs(car[i]->VelColl.az) > VELMAX) {
car[i]->VelColl.az = SIGN(car[i]->VelColl.az) * VELMAX;
}
sgCopyVec2((float*)&(car[i]->VelColl.x), v2a);
// Move the car for the collision lib.
tCarElt *carElt = car[i]->carElt;
sgMakeCoordMat4(carElt->pub.posMat, car[i]->DynGCg.pos.x, car[i]->DynGCg.pos.y,
car[i]->DynGCg.pos.z - carElt->_statGC_z, RAD2DEG(carElt->_yaw),
RAD2DEG(carElt->_roll), RAD2DEG(carElt->_pitch));
dtSelectObject(car[i]);
dtLoadIdentity();
dtTranslate(-carElt->_statGC_x, -carElt->_statGC_y, 0.0f);
dtMultMatrixf((const float *)(carElt->_posMat));
car[i]->collision |= SEM_COLLISION_CAR;
}
}
void display(void)
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);
printOpenGLError();
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity();
gluLookAt(m_camera.position.x, m_camera.position.y, m_camera.position.z,
m_camera.lookat.x, m_camera.lookat.y, m_camera.lookat.z,
m_camera.up.x, m_camera.up.y, m_camera.up.z);
printOglError(0,0);
if(true)
{
gl2psEnable(GL2PS_LINE_STIPPLE);
if(!noxyz)
UIHelper::drawXYZ();
glPolygonMode(GL_FRONT_AND_BACK, polygon_mode);
glColor3f(0.1, 0.8, 0);
UIHelper::drawBoundingBox(boxobj2);
{
mat4 rotateMatrix = rotateDeg == 0 ? mat4::Identity : mat4::GetRotate(rotateDeg, rotateAxis);
mat4 translateMatrix = mat4::GetTranslate(translatePos);
mat4 transformMatrix = translateMatrix * rotateMatrix;
{
mat4 indentity = mat4::Identity;
//double tx = 1.51, ty = 0, tz = 0;
double tx = translatePos.x, ty = translatePos.y, tz = translatePos.z;
double m [16] = {
1.0, 0, 0, 0,
0.0, 1.0, 0, 0,
0.0, 0, 1.0, 0,
tx, ty, tz, 1.0
};
{
dtSelectObject(&object1);
dtLoadMatrixd(m);
//dtLoadIdentity();
//dtTranslate(tx, ty, tz);
if(dtTestPair(&object1, &object2))
//if(dtTest() > 0)
glColor3f(1.0, 0, 0);
else
glColor3f(0.1, 0.8, 0);
}
}
}
if(rotateDeg != 0)
glRotatef(rotateDeg, rotateAxis.x, rotateAxis.y, rotateAxis.z);
glTranslatef(translatePos.x, translatePos.y, translatePos.z);
UIHelper::drawBoundingBox(boxobj1);
gl2psDisable(GL2PS_LINE_STIPPLE);
}else
{
glColor3f(1, 0, 1);
glPolygonMode(GL_FRONT_AND_BACK ,GL_LINE);
glLineWidth(1.0);
GLUquadricObj *qobj = gluNewQuadric();
gluSphere(qobj,3,16,16);
glLineWidth(2.0);
UIHelper::drawNormals(normals, 3);
glPointSize(5.0f);
UIHelper::drawPoints(normals);
}
glPopMatrix();
glFlush();
if( finish_without_update )
glFinish();
else
glutSwapBuffers();
}