void SimAeroDamage(tCar *car, sgVec3 poc, tdble F)
{
tAero* aero = &car->aero;
tdble dmg = F*0.0001;
aero->rot_front[0] += dmg*(urandom()-.5);
aero->rot_front[1] += dmg*(urandom()-.5);
aero->rot_front[2] += dmg*(urandom()-.5);
if (sgLengthVec3(car->aero.rot_front) > 1.0) {
sgNormaliseVec3 (car->aero.rot_front);
}
aero->rot_lateral[0] += dmg*(urandom()-.5);
aero->rot_lateral[1] += dmg*(urandom()-.5);
aero->rot_lateral[2] += dmg*(urandom()-.5);
if (sgLengthVec3(car->aero.rot_lateral) > 1.0) {
sgNormaliseVec3 (car->aero.rot_lateral);
}
aero->rot_vertical[0] += dmg*(urandom()-.5);
aero->rot_vertical[1] += dmg*(urandom()-.5);
aero->rot_vertical[2] += dmg*(urandom()-.5);
if (sgLengthVec3(car->aero.rot_vertical) > 1.0) {
sgNormaliseVec3 (car->aero.rot_vertical);
}
//printf ("aero damage:%f (->%f %f %f)\n", dmg, sgLengthVec3(car->aero.rot_front),
// sgLengthVec3(car->aero.rot_lateral), sgLengthVec3(car->aero.rot_vertical));
}
void
SimCarCollideXYScene(tCar *car)
{
tTrackSeg *seg = car->trkPos.seg;
tTrkLocPos trkpos;
int i;
tDynPt *corner;
//t3Dd normal;
tdble initDotProd;
tdble dotProd;
tTrackBarrier *curBarrier;
tdble dmg;
if (car->carElt->_state & RM_CAR_STATE_NO_SIMU) {
return;
}
tdble energy_restitution = 0.999f;
corner = &(car->corner[0]);
for (i = 0; i < 4; i++, corner++) {
seg = car->trkPos.seg;
RtTrackGlobal2Local(seg, corner->pos.ax, corner->pos.ay, &trkpos, TR_LPOS_TRACK);
seg = trkpos.seg;
tdble toSide;
if (trkpos.toRight < 0.0) {
// collision with right border.
curBarrier = seg->barrier[TR_SIDE_RGT];
toSide = trkpos.toRight;
} else if (trkpos.toLeft < 0.0) {
// collision with left border.
curBarrier = seg->barrier[TR_SIDE_LFT];
toSide = trkpos.toLeft;
} else {
continue;
}
const tdble& nx = curBarrier->normal.x;
const tdble& ny = curBarrier->normal.y;
t3Dd normal = {nx, ny, 0.0f};
car->DynGCg.pos.x -= nx * toSide;
car->DynGCg.pos.y -= ny * toSide;
car->DynGC.pos.x = car->DynGCg.pos.x;
car->DynGC.pos.y = car->DynGCg.pos.y;
// Corner position relative to center of gravity.
//tdble cx = corner->pos.ax - car->DynGCg.pos.x;
//tdble cy = corner->pos.ay - car->DynGCg.pos.y;
car->blocked = 1;
car->collision |= SEM_COLLISION;
// Impact speed perpendicular to barrier (of corner).
initDotProd = nx * corner->vel.x + ny * corner->vel.y;
//printf("%f = (%f %f)'(%f %f)\n", initDotProd, nx, ny, corner->vel.x, corner->vel.y);
// Compute dmgDotProd (base value for later damage) with a heuristic.
tdble absvel = (float)MAX(1.0, sqrt(car->DynGCg.vel.x*car->DynGCg.vel.x + car->DynGCg.vel.y*car->DynGCg.vel.y));
tdble GCgnormvel = car->DynGCg.vel.x*nx + car->DynGCg.vel.y*ny;
tdble cosa = GCgnormvel/absvel;
tdble dmgDotProd = GCgnormvel*cosa;
// veolcity projected to normal
tdble vPx = nx * corner->vel.x;
tdble vPy = ny * corner->vel.y;
//tdble vP = sqrt(vPx*vPx + vPy*vPy);
// veolcity projected to tangent plane
tdble vQx = corner->vel.x - vPx;
tdble vQy = corner->vel.y - vPy;
tdble vQ = sqrt(vQx*vQx + vQy*vQy);
// Fix this to be applied only perpendicular to the normal
dotProd = initDotProd * curBarrier->surface->kFriction;
// calculate projection of velocity to perpendicular
{
// this is only used for propagating response to other layers
sgVec3 normal_l;
tdble d2 = dotProd;
t2sg3(normal, normal_l);
sgRotateVecQuat (normal_l, car->posQuat);
car->DynGC.acc.x -= normal_l[SG_X] * d2;
car->DynGC.acc.y -= normal_l[SG_Y] * d2;
car->carElt->_accel_x -= normal_l[SG_X] * d2;
car->carElt->_accel_y -= normal_l[SG_Y] * d2;
}
// Dammage.
dotProd = initDotProd;
dmg = 0.0f;
if (curBarrier->surface->kRebound > 1.0) {
printf("warning: rebound constant %f > 1\n", curBarrier->surface->kRebound);
} else {
dotProd *= curBarrier->surface->kRebound;
}
// If the car moves toward the barrier, rebound.
tdble normal_impulse_x = - nx * dotProd;
tdble normal_impulse_y = - ny * dotProd;
tdble dP3 = initDotProd * curBarrier->surface->kFriction / vQ;// could divide by vQ, but it's better (I think) to have it proportional to speed.
tdble friction_impulse_x = vQx * dP3;
tdble friction_impulse_y = vQy * dP3;
if (dotProd < 0.0f) {
//printf ("CollideXY\n");
tdble E_prev = SimCarDynamicEnergy(car);
// propagate damages
if ((car->carElt->_state & RM_CAR_STATE_FINISH) == 0) {
dmgDotProd = (float)(dmgDotProd*dmgDotProd*0.5
+ friction_impulse_x*friction_impulse_x
+ friction_impulse_y*friction_impulse_y);
dmg = curBarrier->surface->kDammage * dmgDotProd * simDammageFactor[car->carElt->_skillLevel];
car->dammage += (int)dmg;
}
car->collision |= SEM_COLLISION_XYSCENE;
car->normal.x = nx * dmg;
car->normal.y = ny * dmg;
car->collpos.x = corner->pos.ax;
car->collpos.y = corner->pos.ay;
//printf ("ColXY: (%f %f) + (%f %f)\n",
//normal_impulse_x, normal_impulse_y,
//friction_impulse_x, friction_impulse_y);
// Calculate change in rotational momentum.
// ----------------------------------------
// Put the impulse in a 3d vector
sgVec3 impulse = {normal_impulse_x + friction_impulse_x,
normal_impulse_y + friction_impulse_y,
0.0};
// rotate it to the target frame
sgRotateVecQuat (impulse, car->posQuat);
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 = {car->statGC.x + corner->pos.x,
car->statGC.y + corner->pos.y,
-car->statGC.z};
// 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;// * SimDeltaTime;
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;
//printf ("M_w:%f J:%f M_c:%g\n", car->rot_acc[SG_Z], car->rot_mom[SG_Z], rot_mom_scale * Mz);
for (int i=0; i<3; i++) {
if (fabs(car->rot_mom[i]) > 2000.0) {
//printf ("rot_mom: %f\n", (car->rot_mom[i]));
car->rot_mom[i] = (float)(2000*SIGN(car->rot_mom[i]));
}
}
// 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;
}
SimCarLimitDynamicEnergy(car, energy_restitution*E_prev);
}
#if 0
static tdble DEFORMATION_THRESHOLD = 0.01f;
if (car->options->aero_damage
|| sgLengthVec3(force) > DEFORMATION_THRESHOLD) {
sgVec3 poc;
poc[0] = corner->pos.x;
poc[1] = corner->pos.y;
poc[2] = (urandom()-0.5)*2.0;
sgRotateVecQuat (force, car->posQuat);
sgNormaliseVec3(force);
for (int i=0; i<3; i++) {
force[i]*=dmg;
}
// just compute values, gr does deformation later.
// must average position and add up force.
SimCarCollideAddDeformation(car, poc, force);
// add aero damage if applicable
if (car->options->aero_damage) {
SimAeroDamage (car, poc, sgLengthVec3(force));
}
}
#endif
}
}
