// Note that this differs in the calculation of sx, sy, sa
// from simuv2, but is exactly the same when everything is horizontal.
// I changed the implementation so that what happens is mathematically
// clearer, though the code is not very clear :/
void
SimWheelUpdateForce(tCar *car, int index)
{
tWheel *wheel = &(car->wheel[index]);
tdble axleFz = wheel->axleFz;
// tdble vt, v, v2;
tdble wrl; /* wheel related velocity */
tdble Fn, Ft, Ft2;
tdble waz;
tdble CosA, SinA;
tdble s = 0.0;
tdble sa, sx, sy; /* slip vector */
tdble stmp, F, Bx;
tdble mu;
tdble reaction_force;
tdble f_z = 0.0;
t3Dd angles;
t3Dd normal;
t3Dd rel_normal;
bool right_way_up = true;
static long wcnt = 0;
#ifdef USE_THICKNESS
int seg_id = (int) ((tdble) N_THICKNESS_SEGMENTS * (wheel->relPos.ay/(2*M_PI))) % N_THICKNESS_SEGMENTS;
if (seg_id<0) seg_id += N_THICKNESS_SEGMENTS;
tdble adjRadius = wheel->radius + wheel->thickness[seg_id];
#else
tdble adjRadius = wheel->radius;
#endif
wheel->T_current = car->carElt->_tyreT_mid(index);
wheel->condition = car->carElt->_tyreCondition(index);
waz = wheel->relPos.az;//wheel->steer + wheel->staticPos.az;
/* Get normal of road relative to the wheel's axis
This should help take into account the camber.*/
BEGIN_PROFILE(timer_coordinate_transform);
//RtTrackSurfaceNormalL(&(wheel->trkPos), &normal);
normal = wheel->normal;
// now rel_normal.x is the effective camber angle
if (USE_QUATERNIONS==0) {
angles.x = car->DynGCg.pos.ax + wheel->relPos.ax;
angles.y = car->DynGCg.pos.ay;
angles.z = car->DynGCg.pos.az + waz;
NaiveRotate (normal, angles, &rel_normal);
} else {
sgQuat Q;
sgCopyQuat (Q, car->posQuat);
sgPreRotQuat (Q, FLOAT_RAD2DEG(wheel->relPos.ax), 1.0f, 0.0f, 0.0f);
sgPreRotQuat (Q, FLOAT_RAD2DEG(waz), 0.0f, 0.0f, 1.0f);
sgVec3 P = {normal.x, normal.y, normal.z};
sgRotateVecQuat (P, Q);
sg2t3 (P, rel_normal);
}
wheel->state = 0;
END_PROFILE(timer_coordinate_transform);
BEGIN_PROFILE(timer_reaction);
Ft = 0.0;
Fn = 0.0;
wheel->forces.x = 0.0;
wheel->forces.y = 0.0;
wheel->forces.z = 0.0;
/* Now uses the normal, so it should work */
/* update suspension force */
SimSuspUpdate(&(wheel->susp));
/* check suspension state */
wheel->state |= wheel->susp.state;
reaction_force = 0.0;
wheel->forces.z = 0;
Ft = Fn = 0;
reaction_force = 0.0;
if ((wheel->state & SIM_SUSP_EXT) == 0) {
f_z = axleFz + wheel->susp.force;
wheel->rel_vel -= SimDeltaTime * wheel->susp.force / wheel->mass;
if ((f_z < 0)) {
f_z = 0;
}
/* project the reaction force. Only wheel->forces.z is
actually interesting for friction. The rest is just
reaction. Now we have included the reaction from the sides
which is fake.
The suspension pushes the wheel down with f_z, but the reaction
of the surface is just f_z*rel_normal.z;!
*/
if ((right_way_up) && (rel_normal.z > MIN_NORMAL_Z)) {
// reaction force on track z axis should be equal to
// suspension reaction if suspension is perpendicular
// to the track plane. We assume other reaction forces
// proportional, but things break down when car is
// tilted a lot with respect to the track plane.
tdble invrel_normal = 1.0f/rel_normal.z;
if (invrel_normal>= 4.0) {
invrel_normal = 4.0;
} else if (invrel_normal<=-4.0) {
invrel_normal = -4.0;
}
reaction_force = f_z; //* invrel_normal;
// the other reactions are then:
Ft = reaction_force*rel_normal.x;//*invrel_normal;
Fn = reaction_force*rel_normal.y;//*invrel_normal;
} else {
f_z = 0;
wheel->susp.force = 0;
wheel->forces.z = 0;
Ft = Fn = 0;
reaction_force = 0.0;
}
} else {
//if (wheel->rel_vel < 0.0) {
//wheel->rel_vel = 0.0;
//}
wheel->rel_vel -= SimDeltaTime * wheel->susp.force / wheel->mass;
wheel->forces.z = 0.0f;
}
wheel->relPos.z = - wheel->susp.x / wheel->susp.spring.bellcrank + adjRadius; /* center relative to GC */
END_PROFILE(timer_reaction);
BEGIN_PROFILE(timer_angles);
/* HORIZONTAL FORCES */
CosA = cos(waz);
SinA = sin(waz);
/* tangent velocity */
// This is speed of the wheel relative to the track, so we have
// to take the projection to the track.
tdble rel_normal_xz = sqrt (rel_normal.z*rel_normal.z
+ rel_normal.x*rel_normal.x);
tdble rel_normal_yz = sqrt (rel_normal.z*rel_normal.z
+ rel_normal.y*rel_normal.y);
//tdble rel_normal_xy = sqrt (rel_normal.x*rel_normal.x
// + rel_normal.y*rel_normal.y);
END_PROFILE(timer_angles);
#ifndef FREE_MOVING_WHEELS
wheel->bodyVel.z = 0.0;
#endif
wrl = (wheel->spinVel + car->DynGC.vel.ay) * adjRadius;
{
// this thing here should be faster than quat?
t3Dd angles = {wheel->relPos.ax, 0.0, waz};
NaiveRotate (wheel->bodyVel, angles, &wheel->bodyVel);
}
tdble wvx = wheel->bodyVel.x * rel_normal_yz;
tdble wvy = wheel->bodyVel.y * rel_normal_xz;
tdble absolute_speed = sqrt(wvx*wvx + wvy*wvy);
wvx -= wrl;
wheel->bodyVel.x = wvx;
wheel->bodyVel.y = wvy;
BEGIN_PROFILE(timer_friction);
tdble relative_speed = sqrt(wvx*wvx + wvy*wvy);
tdble camber_gain = +0.1f;
tdble camber_shift = camber_gain * rel_normal.x;
if ((wheel->state & SIM_SUSP_EXT) != 0) {
sx = sy = sa = 0;
} else if (absolute_speed < ABSOLUTE_SPEED_CUTOFF) {
sx = wvx/ABSOLUTE_SPEED_CUTOFF;
sy = wvy/ABSOLUTE_SPEED_CUTOFF;
sa = atan2(wvy, wvx);
} else {
// The division with absolute_speed is a bit of a hack.
// But the assumption is that the profile of friction
// scales linearly with speed.
sx = wvx/absolute_speed;
sy = wvy/absolute_speed;
sa = atan2(wvy, wvx);
}
s = sqrt(sx*sx+sy*sy);
sa -= camber_shift;
sx = cos(sa)*s;
sy = sin(sa)*s;
wcnt++;
access_times = (float) wcnt;
//if (index==0) {
//wcnt--;
//}
if (wcnt<0) {
//printf ("%f", reaction_force);
if (index==3) {
wcnt = 10;
//printf ("#RCT\n");
} else {
//printf (" ");
}
}
if (right_way_up) {
if (fabs(absolute_speed) < 2.0f && fabs(wrl) < 2.0f) {
car->carElt->_skid[index] = 0.0f;
} else {
car->carElt->_skid[index] = MIN(1.0f, (s*reaction_force*0.0002f));
}
//0.0002*(MAX(0.2, MIN(s, 1.2)) - 0.2)*reaction_force;
car->carElt->_reaction[index] = reaction_force;
} else {
car->carElt->_skid[index] = 0.0f;
car->carElt->_reaction[index] = 0.0f;
}
stmp = MIN(s, 1.5f);
/* MAGIC FORMULA */
Bx = wheel->mfB * stmp;
tdble dynamic_grip = wheel->mfT * sin(wheel->mfC * atan(Bx * (1 - wheel->mfE) + wheel->mfE * atan(Bx))) * (1.0f + stmp * simSkidFactor[car->carElt->_skillLevel]);
//printf ("%f\n", simSkidFactor[car->carElt->_skillLevel]);
/* load sensitivity */
mu = wheel->mu * (wheel->lfMin + (wheel->lfMax - wheel->lfMin) * exp(wheel->lfK * reaction_force / wheel->opLoad));
//mu = wheel->mu;
tdble static_grip = wheel->condition * reaction_force * mu * wheel->trkPos.seg->surface->kFriction;
//tdble static_grip = wheel->condition * reaction_force * mu * wheel->trkPos.seg->surface->kFriction/0.7f;
F = dynamic_grip * static_grip;
// This is the steering torque
{
tdble Bx = wheel->mfB * (sa);// + camber_shift);
//printf ("%f %f\n", sa, camber_shift);
car->carElt->_wheelFy(index) = (tdble)(cos(sa)*wheel->mfT * sin(wheel->mfC * atan(Bx * (1 - wheel->mfE) + wheel->mfE * atan(Bx))) * (1.0 + stmp * simSkidFactor[car->carElt->_skillLevel]) * static_grip);
}
END_PROFILE(timer_friction);
BEGIN_PROFILE(timer_temperature);
if (car->options->tyre_temperature) {
// heat transfer function with air
tdble htrf = (tdble)((0.002 + fabs(absolute_speed)*0.0005)*SimDeltaTime);
tdble T_current = wheel->T_current;
tdble T_operating = wheel->T_operating;
tdble T_range = wheel->T_range;
tdble mfT;
// friction heat transfer
T_current += (tdble)(0.00003*((fabs(relative_speed)+0.1*fabs(wrl))*reaction_force)*SimDeltaTime);
T_current = (tdble)(T_current * (1.0-htrf) + htrf * 25.0);
tdble dist = (T_current - T_operating)/T_range;
//mfT = 100.0f * exp(-0.5f*(dist*dist))/T_range;
mfT = 0.85f + 3.0f * exp(-0.5f*(dist*dist))/T_range;
if (T_current>200.0) T_current=200.0;
wheel->mfT = mfT;
wheel->T_current = T_current;
}
if (car->options->tyre_damage > 0.0f && s>0.01f) {
tdble compound_melt_point = wheel->T_operating + wheel->T_range;
tdble adherence = wheel->Ca * 500.0f;
tdble melt = (exp (2.0f*(wheel->T_current - compound_melt_point)/compound_melt_point)) * car->options->tyre_damage;
tdble removal = exp (2.0f*(F - adherence)/adherence);
tdble wheel_damage = (tdble)(0.001 * melt * relative_speed * removal / (2.0 * M_PI * wheel->radius * wheel->width * wheel->Ca));
if (wheel_damage>0.01f) {
wheel_damage = 0.01f;
}
tdble delta_dam = wheel_damage * SimDeltaTime;
wheel->condition -= 0.5f*delta_dam;
if (wheel->condition < 0.5f) wheel->condition = 0.5f;
} else {
wheel->mfT = 1.0f;
}
END_PROFILE(timer_temperature);
BEGIN_PROFILE(timer_force_calculation);
wheel->rollRes = reaction_force * wheel->trkPos.seg->surface->kRollRes;
car->carElt->priv.wheel[index].rollRes = wheel->rollRes;
// Calculate friction forces
Ft2 = 0.0f;
tdble Fn2 = 0.0f;
tdble epsilon = 0.00001f;
if (s > epsilon) {
/* wheel axis based - no control after an angle*/
if (rel_normal.z > MIN_NORMAL_Z) {
// When the tyre is tilted there is less surface
// touching the road. Modelling effect simply with rel_normal_xz.
// Constant 1.05f for equality with simuv2.
tdble sur_f = 1.05f * rel_normal_xz;
sur_f = 1.0;
Ft2 = - sur_f*F*sx/s;
Fn2 = - sur_f*F*sy/s;
} else {
Ft2 = 0.0f;
Fn2 = 0.0f;
}
//Ft2 -= camber_shift*F;
} else {
tdble sur_f = rel_normal_xz;
Ft2 = - sur_f*F*sx/epsilon;
Fn2 = - sur_f*F*sy/epsilon;
}
Ft2 -= tanh(wvx) * fabs(wheel->rollRes);
Fn2 -= tanh(wvy) * fabs(wheel->rollRes);
wheel->forces.x = Ft2 * rel_normal_yz;
wheel->forces.y = Fn2 * rel_normal_xz;
wheel->forces.z = Ft2 * rel_normal.x + Fn2 * rel_normal.y;
END_PROFILE(timer_force_calculation);
if (0) {
// EXPERIMENTAL code - estimate amount of mass linked to this
// wheel. Maybe useful for adjusting the slope of the
// static friction function. Currently not used.
tdble Ftot = sqrt(Ft2*Ft2 + Fn2*Fn2);
tdble ds = wheel->s_old-s;
tdble EF = wheel->Em * ds;
tdble dF = wheel->F_old - EF;
wheel->Em += (float)(0.1 * dF*ds);
wheel->F_old = Ftot;
wheel->s_old = s;
}
wheel->relPos.az = waz;
if (rel_normal.z > MIN_NORMAL_Z) {
right_way_up = true;
} else {
right_way_up = false;
}
if (car->collide_timer < 0.00) {
right_way_up = false;
}
if (right_way_up==false) {
Fn = 0.0f;
Ft = 0.0f;
wheel->forces.x = 0.0f;
wheel->forces.y = 0.0f;
wheel->forces.z = 0.0f;
Ft2 = 0.0f;
wheel->spinTq = 0.0f;
} else {
BEGIN_PROFILE (timer_wheel_to_car);
t3Dd f;
// send friction and reaction forces to the car
// normally we would not need to add f.z here, as that
// would be purely coming from the suspension. However
// that would only be the case if the wheels were really
// independent objects. Right now their position is determined
// in update ride, so we have no choice but to transmit the
// suspension-parallel friction forces magically to the car.
f.x = wheel->forces.x;
f.y = wheel->forces.y;
f.z = wheel->forces.z;
// TODO: Check whether this is correct.
angles.x = wheel->relPos.ax + asin(rel_normal.x);
angles.y = asin(rel_normal.y);
angles.z = waz;
NaiveInverseRotate (f, angles, &wheel->forces);
// transmit reaction forces to the car
wheel->forces.x +=(Ft* CosA - Fn * SinA);
wheel->forces.y +=(Ft* SinA + Fn * CosA);
//RELAXATION2(wheel->forces.x, wheel->preFn, 50.0f);
//RELAXATION2(wheel->forces.y, wheel->preFt, 50.0f);
wheel->forces.z = f_z + wheel->bump_force; // only suspension acts on z axis.
car->carElt->_wheelFx(index) = wheel->forces.x;
car->carElt->_wheelFz(index) = wheel->forces.z;
wheel->spinTq = (Ft2 + tanh(wrl)*fabs(wheel->rollRes))* adjRadius;
wheel->sa = sa;
wheel->sx = sx;
END_PROFILE (timer_wheel_to_car);
}
wheel->feedBack.spinVel = wheel->spinVel;
wheel->feedBack.Tq = wheel->spinTq;
wheel->feedBack.brkTq = wheel->brake.Tq;
car->carElt->_tyreT_mid(index) = wheel->T_current;
car->carElt->_tyreCondition(index) = wheel->condition;
car->carElt->_wheelSlipSide(index) = wvy;
car->carElt->_wheelSlipAccel(index) = wvx;
}
void
SimWheelUpdateRide(tCar *car, int index)
{
tWheel *wheel = &(car->wheel[index]);
tdble Zroad;
/* compute suspension travel */
RtTrackGlobal2Local(car->trkPos.seg, wheel->pos.x, wheel->pos.y, &(wheel->trkPos), TR_LPOS_SEGMENT);
wheel->zRoad = Zroad = RtTrackHeightL(&(wheel->trkPos));
tdble prexwheel = wheel->susp.x / wheel->susp.spring.bellcrank;
tdble new_susp_x= prexwheel - wheel->rel_vel * SimDeltaTime;
tdble max_extend;
t3Dd normal;
t3Dd rel_normal;
// Find normal of track.
RtTrackSurfaceNormalL(&(wheel->trkPos), &normal);
wheel->normal = normal;
{
sgQuat Q;
sgCopyQuat (Q, car->posQuat);
sgPreRotQuat (Q, FLOAT_RAD2DEG(wheel->relPos.ax), 1.0f, 0.0f, 0.0f);
sgVec3 P = {normal.x, normal.y, normal.z};
sgRotateVecQuat (P, Q);
sg2t3 (P, rel_normal);
}
tdble dZ = wheel->pos.z - Zroad;
//NaiveRotate (d, angles, &d);
#ifdef USE_THICKNESS
int seg_id = ((int) ((tdble) N_THICKNESS_SEGMENTS * (wheel->relPos.ay/(2*M_PI)))) % N_THICKNESS_SEGMENTS;
if (seg_id<0) seg_id += N_THICKNESS_SEGMENTS;
tdble adjRadius = wheel->radius + wheel->thickness[seg_id];
#else
tdble adjRadius = wheel->radius;
#endif
if (rel_normal.z > MIN_NORMAL_Z) {
wheel->susp.fx = adjRadius - adjRadius/rel_normal.z;
wheel->susp.fy = 0.0;
//wheel->susp.x = wheel->rideHeight =
max_extend = adjRadius + ((dZ)*normal.z - adjRadius)/rel_normal.z;
} else {
//wheel->susp.x = wheel->rideHeight = (wheel->pos.z - Zroad);
// wheel->susp.x = wheel->rideHeight = wheel->susp.spring.packers;
wheel->susp.fx = 0.0;
wheel->state = wheel->state | SIM_SUSP_COMP;
max_extend = 0.0;
}
/* Note from Christos about the 'max_extend' variable set right above :
This variable's name a left-over from simuv2. It has a slightly extended use.
Perhaps the name should be changed.
In simuv2, when the suspension was fully compressed then the suspension reaction
was ignored, the car speed was set to be tangential to the track,
and the car height was set to be just above the track.
Now it basically signifies that the car is not a 'normal' regime, meaning either:
a) The suspension is maxed out, and perhaps the car is touching the ground.
b) The car has rolled over more than a certain amount.
In either case, the collision code is used to see how to react.
In the collision code, we check if the car is upright :
If the car is upright:
* If the car is not very low, then the simuv2 approach is used.
* If the car is under the track (beyond a margin) then damages are taken
and there is some friction.
If the car is not upright, then we use the collision code with the ground.
Now, this collision code with the ground (collidez) seems to be somewhat problematic,
but I do not know why. Virtually the same code is used for collisions with the walls,
and there the car behaves nicely.
Possibly there is a mixup with the frames of reference.
*/
wheel->rideHeight = max_extend;
wheel->bump_force = 0.0; // force of the wheel bumping up into the frame
if (max_extend < new_susp_x) {
new_susp_x = max_extend;
wheel->rel_vel = 0.0f;
} else if (new_susp_x <= wheel->susp.spring.packers) {
wheel->bump_force = wheel->mass * wheel->rel_vel / SimDeltaTime;
wheel->susp.x = wheel->susp.spring.packers;
wheel->rel_vel = 0.0f;
}
tdble prex = wheel->susp.x;
wheel->susp.x = new_susp_x;
wheel->relPos.az = wheel->steer + wheel->staticPos.az;
// Transform from world to suspension FOR
if (index % 2) {
wheel->relPos.ax = -wheel->staticPos.ax;
} else {
wheel->relPos.ax = wheel->staticPos.ax;
}
wheel->relPos.ax += wheel->dynamic_camber*wheel->steer;
if (car->options->alignment_damage && wheel->rotational_damage_x>0.0) {
wheel->relPos.ax += wheel->rotational_damage_x*sin(wheel->relPos.ay + wheel->bent_damage_x);
wheel->relPos.az += wheel->rotational_damage_z*cos(wheel->relPos.ay + wheel->bent_damage_z);
}
//wheel->relPos.z = - wheel->susp.x / wheel->susp.spring.bellcrank + wheel->radius; /* center relative to GC */
/* verify the suspension travel */
SimSuspCheckIn(&(wheel->susp));
//wheel->rideHeight = wheel->susp.x / wheel->susp.spring.bellcrank;
if (index % 2) {
wheel->relPos.ax -= wheel->susp.dynamic_angles.x;
} else {
wheel->relPos.ax += wheel->susp.dynamic_angles.x;
}
wheel->susp.v = (prex - wheel->susp.x) / SimDeltaTime;
/* update wheel brake */
SimBrakeUpdate(car, wheel, &(wheel->brake));
}
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;
}
}
static void
SimCarUpdateForces(tCar *car)
{
tForces F;
int i;
tdble m, w, minv;
tdble v, R, Rv, Rm, Rx, Ry, Rz;
car->preDynGC = car->DynGCg;
/* total mass */
m = car->mass + car->fuel;
minv = 1.0 / m;
w = -m * G;
/* Weight - Bring weight vector to the car's frame of reference */
sgVec3 pos = {0.0f, 0.0f, w};
sgRotateVecQuat (pos, car->posQuat);
F.F.x = pos[SG_X];
F.F.y = pos[SG_Y];
F.F.z = pos[SG_Z];
// initial torque 0.
F.M.x = F.M.y = F.M.z = 0;
/* Wheels */
for (i = 0; i < 4; i++) {
tWheel* wheel = &(car->wheel[i]);
/* forces */
tdble susp_pos_y = wheel->staticPos.y - sin(wheel->staticPos.ax)*SIGN(wheel->staticPos.y);
//printf ("%f %f\n", wheel->staticPos.y, sin(wheel->staticPos.ax)*SIGN(wheel->staticPos.y));
F.F.x += wheel->forces.x;
F.F.y += wheel->forces.y;
F.F.z += wheel->forces.z;
//printf ("%f\n", car->statGC.z + wheel->rideHeight);
/* moments */
t3Dd d;
d.y = susp_pos_y;
d.x = wheel->staticPos.x;
d.z = car->statGC.z + wheel->rideHeight;
F.M.x += (wheel->forces.z * d.y + //susp_pos_y +
wheel->forces.y * d.z);
//(car->statGC.z + wheel->rideHeight));
F.M.y -= (wheel->forces.z * d.x + //wheel->staticPos.x +
wheel->forces.x * d.z);
//(car->statGC.z + wheel->rideHeight));
F.M.z += (-wheel->forces.x * d.y + //susp_pos_y +
wheel->forces.y * d.x); //wheel->staticPos.x);
}
F.M.x += car->aero.Mx;
F.M.y += car->aero.My;
F.M.z += car->aero.Mz;
/* Aero Drag */
F.F.x += car->aero.drag;
F.F.y += car->aero.lateral_drag;
F.F.z += car->aero.vertical_drag;
/* Wings & Aero Downforce */
for (i = 0; i < 2; i++) {
/* forces */
F.F.z += car->wing[i].forces.z + car->aero.lift[i];
F.F.x += car->wing[i].forces.x;
/* moments */
float My = car->wing[i].forces.z* car->wing[i].staticPos.x
+ car->wing[i].forces.x * car->wing[i].staticPos.z
+ car->aero.lift[i] * car->wing[i].staticPos.x;
F.M.y -= My;
}
/* Rolling Resistance */
// This method updates rolling resistance using the wheels' resistance.
if (1) {
v = 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);
R = 0;
for (i = 0; i < 4; i++) {
R += car->wheel[i].rollRes;
}
if (v > 0.00001) {
Rv = R / v;
if ((Rv * minv * SimDeltaTime) > v) {
Rv = v * m / SimDeltaTime;
}
} else {
Rv = 0;
}
Rx = Rv * car->DynGC.vel.x; //car->DynGCg.vel.x;
Ry = Rv * car->DynGC.vel.y; //car->DynGCg.vel.x;
Rz = Rv * car->DynGC.vel.z; //car->DynGCg.vel.x;
if ((R * car->wheelbase / 2.0f) > fabs(car->rot_mom[SG_Z])) {
//car->DynGC.vel.az = -car->rot_mom[SG_Z] * car->Iinv.z;
//Rm = -car->rot_mom[SG_Z];//car->DynGCg.vel.az / car->Iinv.z;
Rm = car->rot_mom[SG_Z];//car->DynGCg.vel.az / car->Iinv.z;
} else {
Rm = SIGN(car->rot_mom[SG_Z]) * R * car->wheelbase / 2.0;
}
} else {
Rx = Ry = Rz = 0.0f;
Rm = 0.0;
}
/* compute accelerations */
if (1) {
/* This should work as long as all forces have been computed for
the car's frame of reference */
car->DynGC.acc.x = (F.F.x - Rx) * minv;
car->DynGC.acc.y = (F.F.y - Ry) * minv;
car->DynGC.acc.z = (F.F.z - Rz) * minv;
sgVec3 accel = {car->DynGC.acc.x, car->DynGC.acc.y, car->DynGC.acc.z};
sgRotateCoordQuat (accel, car->posQuat);
car->DynGCg.acc.x = accel[SG_X];
car->DynGCg.acc.y = accel[SG_Y];
car->DynGCg.acc.z = accel[SG_Z];
car->rot_acc[0] = F.M.x;
car->rot_acc[1] = F.M.y;
car->rot_acc[2] = (F.M.z - Rm);
}
}
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
}
}