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 = wheel->pos.z - Zroad;
wheel->rideHeight = max_extend;
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->rel_vel = 0.0f;
}
tdble prex = wheel->susp.x;
wheel->susp.x = new_susp_x;
// verify the suspension travel
SimSuspCheckIn(&(wheel->susp));
wheel->susp.v = (prex - wheel->susp.x) / SimDeltaTime;
// update wheel brake
SimBrakeUpdate(car, wheel, &(wheel->brake));
}
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));
// Wheel susp.x is not the wheel movement, look at SimSuspCheckIn, it becomes there scaled with
// susp->spring.bellcrank, so we invert this here.
tdble prexwheel = wheel->susp.x / wheel->susp.spring.bellcrank;
tdble new_susp_x= prexwheel - wheel->rel_vel * SimDeltaTime;
tdble max_extend = wheel->pos.z - Zroad;
wheel->rideHeight = max_extend;
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->rel_vel = 0.0f;
}
tdble prex = wheel->susp.x;
wheel->susp.x = new_susp_x;
// verify the suspension travel, beware, wheel->susp.x will be scaled by SimSuspCheckIn
SimSuspCheckIn(&(wheel->susp));
wheel->susp.v = (prex - wheel->susp.x) / SimDeltaTime;
// update wheel brake
SimBrakeUpdate(car, wheel, &(wheel->brake));
}
static void
SimCarUpdatePos(tCar *car)
{
tdble vx, vy, vz;
vx = car->DynGCg.vel.x;
vy = car->DynGCg.vel.y;
vz = car->DynGCg.vel.z;
car->DynGCg.pos.x = car->DynGC.pos.x;
car->DynGCg.pos.y = car->DynGC.pos.y;
car->DynGCg.pos.z = car->DynGC.pos.z;
car->DynGCg.pos.x += vx * SimDeltaTime;
car->DynGCg.pos.y += vy * SimDeltaTime;
car->DynGCg.pos.z += vz * SimDeltaTime;
car->DynGC.pos.x = car->DynGCg.pos.x;
car->DynGC.pos.y = car->DynGCg.pos.y;
car->DynGC.pos.z = car->DynGCg.pos.z;
SimCarAddAngularVelocity(car);
NORM_PI_PI(car->DynGC.pos.ax);
NORM_PI_PI(car->DynGC.pos.ay);
NORM_PI_PI(car->DynGC.pos.az);
car->DynGCg.pos.ax = car->DynGC.pos.ax;
car->DynGCg.pos.ay = car->DynGC.pos.ay;
car->DynGCg.pos.az = car->DynGC.pos.az;
//printf ("a %f %f %f\n", car->DynGC.pos.ax, car->DynGC.pos.ay, car->DynGC.pos.az);
RtTrackGlobal2Local(car->trkPos.seg, car->DynGCg.pos.x, car->DynGCg.pos.y, &(car->trkPos), TR_LPOS_MAIN);
}
static void
SimCarUpdatePos(tCar *car)
{
tdble vx, vy;
vx = car->DynGCg.vel.x;
vy = car->DynGCg.vel.y;
car->DynGCg.pos.x += vx * SimDeltaTime;
car->DynGCg.pos.y += vy * SimDeltaTime;
car->DynGCg.pos.z += car->DynGCg.vel.z * SimDeltaTime;
car->DynGCg.pos.ax += car->DynGCg.vel.ax * SimDeltaTime;
car->DynGCg.pos.ay += car->DynGCg.vel.ay * SimDeltaTime;
car->DynGCg.pos.az += car->DynGCg.vel.az * SimDeltaTime;
FLOAT_NORM_PI_PI(car->DynGCg.pos.az);
if (car->DynGCg.pos.ax > aMax) car->DynGCg.pos.ax = aMax;
if (car->DynGCg.pos.ax < -aMax) car->DynGCg.pos.ax = -aMax;
if (car->DynGCg.pos.ay > aMax) car->DynGCg.pos.ay = aMax;
if (car->DynGCg.pos.ay < -aMax) car->DynGCg.pos.ay = -aMax;
car->DynGC.pos.x = car->DynGCg.pos.x;
car->DynGC.pos.y = car->DynGCg.pos.y;
car->DynGC.pos.z = car->DynGCg.pos.z;
car->DynGC.pos.ax = car->DynGCg.pos.ax;
car->DynGC.pos.ay = car->DynGCg.pos.ay;
car->DynGC.pos.az = car->DynGCg.pos.az;
RtTrackGlobal2Local(car->trkPos.seg, car->DynGCg.pos.x, car->DynGCg.pos.y, &(car->trkPos), TR_LPOS_MAIN);
}
/** Returns the absolute height in meters of the road
at the Global position (segment, X, Y)
@ingroup tracktools
@param seg Segment
@param X Global X position
@param Y Global Y position
@return Height in meters
*/
tdble
RtTrackHeightG(tTrackSeg *seg, tdble X, tdble Y)
{
tTrkLocPos p;
RtTrackGlobal2Local(seg, X, Y, &p, TR_LPOS_SEGMENT);
return RtTrackHeightL(&p);
}
double MyTrack::CalcPos( double x, double y, const Seg* hint, bool sides ) const
{
tTrackSeg* pTrackSeg = m_pSegs[0].pSeg;
if( hint != 0 )
pTrackSeg = hint->pSeg;
tTrkLocPos pos;
RtTrackGlobal2Local( pTrackSeg, x, y, &pos, sides );
double dist = RtGetDistFromStart2(&pos);
return dist;
}
//==========================================================================*
// Calc position from coordinates
//--------------------------------------------------------------------------*
double TTrackDescription::CalcPos
(float X, float Y, const TSection* Hint, bool Sides) const
{
tTrackSeg* Seg = oSections[0].Seg;
if(Hint != 0)
Seg = Hint->Seg;
tTrkLocPos Pos;
RtTrackGlobal2Local(Seg, X, Y, &Pos, Sides );
return RtGetDistFromStart2(&Pos);
}
// Collision of car/track borders.
// Be aware that it does not work for convex edges (e.g. e-track-2, end of the straight, left),
// because under these conditions it is possible that all car corners are on the track and
// the car body is partially outside the track.
void SimCarCollideXYScene(tCar *car)
{
tTrackSeg *seg = car->trkPos.seg;
tTrkLocPos trkpos;
int i;
tDynPt *corner;
tdble initDotProd;
tdble dotProd, cx, cy, dotprod2;
tTrackBarrier *curBarrier;
tdble dmg;
if (car->carElt->_state & RM_CAR_STATE_NO_SIMU) {
return;
}
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;
car->DynGCg.pos.x -= nx * toSide;
car->DynGCg.pos.y -= ny * toSide;
// Corner position relative to center of gravity.
cx = corner->pos.ax - car->DynGCg.pos.x;
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;
// Compute dmgDotProd (base value for later damage) with a heuristic.
tdble absvel = 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;
dotProd = initDotProd * curBarrier->surface->kFriction;
car->DynGCg.vel.x -= nx * dotProd;
car->DynGCg.vel.y -= ny * dotProd;
dotprod2 = (nx * cx + ny * cy);
// Angular velocity change caused by friction of colliding car part with wall.
static tdble VELSCALE = 10.0f;
static tdble VELMAX = 6.0f;
car->DynGCg.vel.az -= dotprod2 * dotProd / VELSCALE;
if (fabs(car->DynGCg.vel.az) > VELMAX) {
car->DynGCg.vel.az = SIGN(car->DynGCg.vel.az) * VELMAX;
}
// Damage.
dotProd = initDotProd;
if (dotProd < 0.0f && (car->carElt->_state & RM_CAR_STATE_FINISH) == 0) {
dmg = curBarrier->surface->kDammage * fabs(0.5*dmgDotProd*dmgDotProd) * simDammageFactor[car->carElt->_skillLevel];
car->dammage += (int)dmg;
} else {
dmg = 0.0f;
}
dotProd *= curBarrier->surface->kRebound;
// If the car moves toward the barrier, rebound.
if (dotProd < 0.0f) {
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;
car->DynGCg.vel.x -= nx * dotProd;
car->DynGCg.vel.y -= ny * dotProd;
}
}
}
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;
}
}
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
}
}
