void
SimCarUpdate(tCar *car, tSituation * /* s */)
{
SimCarUpdateForces(car);
CHECK(car);
SimCarUpdateSpeed(car);
CHECK(car);
SimCarUpdateCornerPos(car);
CHECK(car);
SimCarUpdatePos(car);
CHECK(car);
SimCarCollideZ(car);
CHECK(car);
SimCarCollideXYScene(car);
CHECK(car);
}
void
SimCarUpdate(tCar *car, tSituation * /* s */)
{
SimCarUpdateForces(car);
CHECK(car);
SimCarUpdateSpeed(car);
CHECK(car);
SimCarUpdateCornerPos(car);
CHECK(car);
SimCarUpdatePos(car);
CHECK(car);
SimCarCollideZ(car);
CHECK(car);
SimCarCollideXYScene(car);
CHECK(car);
car->speed = sqrt(car->DynGC.vel.x*car->DynGC.vel.x + car->DynGC.vel.y*car->DynGC.vel.y + car->DynGC.vel.z*car->DynGC.vel.z);
}
void
SimCarCollideZ(tCar *car)
{
int i;
t3Dd car_normal;
t3Dd rel_car_normal;
tdble dotProd;
tWheel *wheel;
tdble corner_factor = 0.9f; // how much to shrink the bounding box
if (car->collide_timer < 10.0) {
car->collide_timer += SimDeltaTime;
}
if (car->carElt->_state & RM_CAR_STATE_NO_SIMU) {
return;
}
tdble energy_restitution = 0.99f;
tdble E_prev = SimCarEnergy(car);
bool collide = false;
// Get normal N
//RtTrackSurfaceNormalL(&(car->trkPos), &car_normal);
car_normal = car->normal; // Use precalculated values
// Get normal N_q in local coordinate system
QuatInverseRotate(car_normal, car->posQuat, rel_car_normal);
// Increment the upside down timer. This can be used later to
// remove cars that have been upside down for too long.
if (rel_car_normal.z > 0) {
car->upside_down_timer = 0.0f;
} else {
car->upside_down_timer += (float)(0.01*SimDeltaTime);
}
tdble gc_height_difference = (float)MIN(0.0, car->DynGCg.pos.z - RtTrackHeightL(&(car->trkPos)));
// Go through all corners and check for collision.
tdble min_height_difference = (float)MIN(0.0, gc_height_difference);
for (i = 0; i < 4; i++) {
wheel = &(car->wheel[i]);
// We only need to check for body collision with the floor if
// the suspension is maximally compressed or the car is upside
// down.
for (int j=0; j<2; j++) {
t3Dd orig; // corner position in local coordinates
t3Dd delta; // corner position in global coordinates
t3Dd normal; // normal at corner position
t3Dd rel_normal; // normal at corner position (local coords)
tDynPt *corner = &(car->corner[i]);
//if (rel_car_normal.z <= 0) {
if (j==0) {
// check top of car
orig.x = corner->pos.x;
orig.y = corner->pos.y;
orig.z = car->dimension.z - car->statGC.z;
} else {
// check bottom of car
orig.x = corner->pos.x;
orig.y = corner->pos.y;
orig.z = - car->statGC.z;
/*if (!(wheel->state & SIM_SUSP_COMP)) {
continue;
}*/
}
orig.x*= corner_factor;
orig.y*= corner_factor;
orig.z*= corner_factor;
// get relative coordinates in global frame
QuatRotate (orig, car->posQuat, delta);
tTrkLocPos trkPos;
//RtTrackGlobal2Local(car->trkPos.seg,
//car->DynGCg.pos.x + orig.x,
//car->DynGCg.pos.y + orig.y,
//&trkPos, TR_LPOS_SEGMENT);
RtTrackGlobal2Local(car->trkPos.seg,
corner->pos.ax,
corner->pos.ay,
&trkPos, TR_LPOS_SEGMENT);
tdble height_difference = car->DynGCg.pos.z + delta.z - RtTrackHeightL(&trkPos);
//printf ("%d %d %f %f %f\n", i, j, height_difference, car->statGC.z, car->DynGCg.pos.z - RtTrackHeightL(&trkPos));
if (height_difference > 0) {
continue;
} else if (height_difference < min_height_difference) {
min_height_difference = height_difference;
}
// get the track normal n_g for the wheel
//RtTrackSurfaceNormalL(&(wheel->trkPos), &normal);
normal = wheel->normal;
// transform it to local coordinates: n = q' n_g q
QuatInverseRotate (normal, car->posQuat, rel_normal);
// calculate the velocity of the corner
#if 1
// option 1: just use the car velocity
// this works fine when more than 1 corner hits at the same time
tdble cvx = (car->DynGCg.vel.x);
tdble cvy = (car->DynGCg.vel.y);
tdble cvz = (car->DynGCg.vel.z);
#else
// option 2: add the hopefully correctly calculated corner velocity
// to use this, the code must take special consideration
// of multiple corner hits, or we can just update corner
// velocity and position after every hit
tdble cvx = corner->vel.x;
tdble cvy = corner->vel.y;
tdble cvz = corner->vel.z; // NOTE: this last one is an approximation, sadly
#endif
// option 3: recalculate the velocity
// TODO
// c = K n'v, v|n = cn
dotProd = (cvx * normal.x
+ cvy * normal.y
+ cvz * normal.z);
if (dotProd < 0) {
//tdble dotProd2 = 0.25*dotProd *car->mass / SimDeltaTime;
tdble mu = 0.5;
// normal
tdble nx = normal.x;
tdble ny = normal.y;
tdble nz = normal.z;
#ifdef DEBUG_COLLIDE_Z
printf("CollideZ: %d %d %f\n N = (%f %f %f)\n", i, j, dotProd, nx, ny, nz);
#endif
// veolcity projected to normal
tdble vPx = nx * cvx;
tdble vPy = ny * cvy;
tdble vPz = nz * cvz;
//tdble vP = sqrt(vPx*vPx + vPy*vPy + vPz*vPz);
#ifdef DEBUG_COLLIDE_Z
printf(" V = (%.2f %.2f %.2f) -(P)-> (%.2f %.2f %.2f)\n",
cvx, cvy, cvz,
vPx, vPy, vPz);
#endif
// veolcity projected to tangent plane
tdble vQx = cvx - vPx;
tdble vQy = cvy - vPy;
tdble vQz = cvz - vPz;
tdble vQ = sqrt(vQx*vQx + vQy*vQy + vQz*vQz);
// v|n = n'v'n = cn
// reaction force - by definition has the
// same direction as the normal
t3Dd forces;
forces.x = - dotProd * nx;
forces.y = - dotProd * ny;
forces.z = - dotProd * nz;
tdble dP3 = (float)(dotProd * mu / (0.001 + vQ));
t3Dd friction;
friction.x = vQx * dP3;
friction.y = vQy * dP3;
friction.z = vQz * dP3;
#ifdef DEBUG_COLLIDE_Z
printf (" Fn= %.2f %.2f %.2f\n",
forces.x,
forces.y,
forces.z);
printf (" Ff= %.2f %.2f %.2f\n",
friction.x,
friction.y,
friction.z);
#endif
// propagate damage to deformation
//car->normal.x = nx * dmg;
//car->normal.y = ny * dmg;
//car->normal.z = nz * dmg;
// change car physical state
// TODO: duplicate code: fix
// Calculate change in rotational momentum.
// ----------------------------------------
// Put the impulse in a 3d vector
sgVec3 impulse = {(forces.x + friction.x),
(forces.y + friction.y),
forces.z + friction.z};
#ifdef DEBUG_COLLIDE_Z
printf (" F = %.2f %.2f %.2f\n",
impulse[SG_X],
impulse[SG_Y],
impulse[SG_Z]);
#endif
// rotate it to the car's frame
sgRotateVecQuat (impulse, car->posQuat);
//tdble E_prev = SimCarEnergy(car);
// add to local-frame speed
car->DynGC.vel.x += impulse[SG_X];
car->DynGC.vel.y += impulse[SG_Y];
car->DynGC.vel.z += impulse[SG_Z];
// Put the point of impact in a 3d vector
sgVec3 v = {orig.x,
orig.y,
orig.z};
#ifdef DEBUG_COLLIDE_Z
printf (" F' = %.2f %.2f %.2f @ %.2f %.2f %.2f (%.2f %.2f)\n",
impulse[SG_X],
impulse[SG_Y],
impulse[SG_Z],
v[SG_X],
v[SG_Y],
v[SG_Z], orig.x, orig.y);
#endif
// Calculate moments
tdble Mx = + impulse[SG_Z] * v[SG_Y] - impulse[SG_Y] * v[SG_Z];
tdble My = - impulse[SG_Z] * v[SG_X] + impulse[SG_X] * v[SG_Z];
tdble Mz = - impulse[SG_X] * v[SG_Y] + impulse[SG_Y] * v[SG_X];
// Add moments to rotational inertia
tdble rot_mom_scale = 0.25f*car->mass;
#ifdef DEBUG_COLLIDE_Z
printf (" J = (%f %f %f)\n",
car->rot_mom[SG_X],
car->rot_mom[SG_Y],
car->rot_mom[SG_Z]);
#endif
car->rot_mom[SG_X] -= rot_mom_scale * Mx;// * car->Iinv.x;
car->rot_mom[SG_Y] -= rot_mom_scale * My;// * car->Iinv.y;
car->rot_mom[SG_Z] -= rot_mom_scale * Mz;// * car->Iinv.z;
for (int i=0; i<3; i++) {
if (fabs(car->rot_mom[i]) >500.0) {
//printf ("rot_mom: %f\n", (car->rot_mom[i]));
car->rot_mom[i] = (float)(250*SIGN(car->rot_mom[i]));
}
}
#ifdef DEBUG_COLLIDE_Z
printf (" M = (%f %f %f), s = %f\n",
Mx, My, Mz, rot_mom_scale);
printf (" -> J = (%f %f %f)\n",
car->rot_mom[SG_X],
car->rot_mom[SG_Y],
car->rot_mom[SG_Z]);
#endif
// transform velocity to global frame
if (1) {
t3Dd original;
t3Dd updated;
original.x = car->DynGC.vel.x;
original.y = car->DynGC.vel.y;
original.z = car->DynGC.vel.z;
QuatRotate(original, car->posQuat, updated);
car->DynGCg.vel.x = updated.x;
car->DynGCg.vel.y = updated.y;
car->DynGCg.vel.z = updated.z;
// Translate angular momentum to angular velocity
// NOTE: This translation is done again in SimCarAddAngularVelocity()
car->DynGCg.vel.ax = car->DynGC.vel.ax = -2.0f*car->rot_mom[SG_X] * car->Iinv.x;
car->DynGCg.vel.ay = car->DynGC.vel.ay = -2.0f*car->rot_mom[SG_Y] * car->Iinv.y;
car->DynGCg.vel.az = car->DynGC.vel.az = -2.0f*car->rot_mom[SG_Z] * car->Iinv.z;
}
SimCarUpdateCornerPos(car);
SimCarLimitEnergy(car, E_prev);
collide = true;
}
if (dotProd < 0) {
// should be this way..
if (dotProd <-5.0) {
// if it's hard, do a damage thing
static tdble WHEEL_ROT_DAMAGE = 0.001f;
static tdble WHEEL_BENT_DAMAGE = 0.01f;
static tdble WHEEL_DAMAGE_LIMIT = 0.25f;
static tdble SUSP_DAMAGE_CONST = 1.0f;
static tdble SUSP_DAMAGE = 0.1f;
car->collision |= 16;
wheel->rotational_damage_x -= dotProd*WHEEL_ROT_DAMAGE*urandom();
wheel->rotational_damage_z -= dotProd*WHEEL_ROT_DAMAGE*urandom();
wheel->bent_damage_x += (float)(WHEEL_BENT_DAMAGE*(urandom()-0.5));
wheel->bent_damage_z += (float)(WHEEL_BENT_DAMAGE*(urandom()-0.5));
if (wheel->rotational_damage_x > WHEEL_DAMAGE_LIMIT) {
wheel->rotational_damage_x = WHEEL_DAMAGE_LIMIT;
}
if (wheel->rotational_damage_z > WHEEL_DAMAGE_LIMIT) {
wheel->rotational_damage_z = WHEEL_DAMAGE_LIMIT;
}
if (car->options->suspension_damage) {
SimSuspDamage (&wheel->susp,
SUSP_DAMAGE*dotProd + SUSP_DAMAGE_CONST);
}
car->collision |= 1;
}
car->collision |= 1;
car->collision |= 8;
if (wheel->susp.state & SIM_SUSP_OVERCOMP) {
car->collision |= 1;
}
if ((car->carElt->_state & RM_CAR_STATE_FINISH) == 0) {
car->dammage += (int)(wheel->trkPos.seg->surface->kDammage * fabs(dotProd) * simDammageFactor[car->carElt->_skillLevel]);
}
}
} // for j
//if (wheel->state & SIM_SUSP_COMP) {
//car->DynGCg.pos.z += wheel->susp.spring.packers - wheel->rideHeight;
//}
} // for i
car->DynGCg.pos.z -= MIN(gc_height_difference, min_height_difference);
gc_height_difference = car->DynGCg.pos.z - RtTrackHeightL(&(car->trkPos));
if (gc_height_difference < 0) {
car->DynGCg.pos.z -= gc_height_difference;
} else if (gc_height_difference > 100) {
car->DynGCg.pos.z = RtTrackHeightL(&(car->trkPos)) + 100;
car->DynGCg.vel.x = car->DynGCg.vel.y = car->DynGCg.vel.z =
car->DynGC.vel.x = car->DynGC.vel.y = car->DynGC.vel.z = 0.0;
// Translate angular momentum to angular velocity
// NOTE: This translation is done again in SimCarAddAngularVelocity()
car->DynGCg.vel.ax = car->DynGC.vel.ax =
car->DynGCg.vel.ay = car->DynGC.vel.ay =
car->DynGCg.vel.az = car->DynGC.vel.az = 0.0;
car->rot_mom[0] = car->rot_mom[1] = car->rot_mom[2] = 0.0;
}
car->DynGC.pos.z = car->DynGCg.pos.z;
if (collide) {
SimCarLimitEnergy(car, energy_restitution * E_prev);
car->collide_timer = 0.0;
}
}
