// 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;
}
static void
SimCarUpdateSpeed(tCar *car)
{
t3Dd original;
t3Dd updated;
t3Dd angles;
tdble mass;
mass = car->mass + car->fuel;
{
// fuel consumption
tdble delta_fuel = car->fuel_prev - car->fuel;
car->fuel_prev = car->fuel;
if (delta_fuel > 0) {
car->carElt->_fuelTotal += delta_fuel;
}
tdble fi;
tdble as = sqrt(car->airSpeed2);
if (as<0.1) {
fi = 99.9;
} else {
fi = 100000 * delta_fuel / (as*SimDeltaTime);
}
tdble alpha = 0.1;
car->carElt->_fuelInstant = (1.0-alpha)*car->carElt->_fuelInstant + alpha*fi;
}
// update angles
angles.x = car->DynGCg.pos.ax;
angles.y = car->DynGCg.pos.ay;
angles.z = car->DynGCg.pos.az;
// update linear velocity
car->DynGCg.vel.x += car->DynGCg.acc.x * SimDeltaTime;
car->DynGCg.vel.y += car->DynGCg.acc.y * SimDeltaTime;
car->DynGCg.vel.z += car->DynGCg.acc.z * SimDeltaTime;
/* We need to get the speed on the actual frame of reference
for the car. Now we don't need to worry about the world's
coordinates anymore when we calculate stuff. I.E check
aero.cpp*/
original.x = car->DynGCg.vel.x;
original.y = car->DynGCg.vel.y;
original.z = car->DynGCg.vel.z;
NaiveRotate (original, angles, &updated);
car->DynGC.vel.x = updated.x;
car->DynGC.vel.y = updated.y;
car->DynGC.vel.z = updated.z;
// Update angular momentum
car->rot_mom[SG_X] -= car->rot_acc[0] * SimDeltaTime;
car->rot_mom[SG_Y] -= car->rot_acc[1] * SimDeltaTime;
car->rot_mom[SG_Z] -= car->rot_acc[2] * SimDeltaTime;
#if 0
// spin limitation
if (Rm > fabs(car->rot_mom[SG_Z])) {
Rm = fabs(car->rot_mom[SG_Z]);
}
car->rot_mom[SG_Z] -= Rm * SIGN(car->rot_mom[SG_Z]);
#endif
// 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;
}
