/** Used to get the normal vector of the road (pointing upward).
Local coordinates are used to locate the point where to
get the road normal vector.
The vector is normalized.
@ingroup tracktools
@param p Local position
@param norm Returned normalized road normal vector
*/
void
RtTrackSurfaceNormalL(tTrkLocPos *p, t3Dd *norm)
{
tTrkLocPos p1;
t3Dd px1, px2, py1, py2;
t3Dd v1, v2;
tdble lg;
p1.seg = p->seg;
p1.toStart = 0;
p1.toRight = p->toRight;
RtTrackLocal2Global(&p1, &px1.x, &px1.y, TR_TORIGHT);
px1.z = RtTrackHeightL(&p1);
if (p1.seg->type == TR_STR) {
p1.toStart = p1.seg->length;
} else {
p1.toStart = p1.seg->arc;
}
RtTrackLocal2Global(&p1, &px2.x, &px2.y, TR_TORIGHT);
px2.z = RtTrackHeightL(&p1);
p1.toRight = 0;
p1.toStart = p->toStart;
RtTrackLocal2Global(&p1, &py1.x, &py1.y, TR_TORIGHT);
py1.z = RtTrackHeightL(&p1);
p1.toRight = p1.seg->width;
RtTrackLocal2Global(&p1, &py2.x, &py2.y, TR_TORIGHT);
py2.z = RtTrackHeightL(&p1);
v1.x = px2.x - px1.x;
v1.y = px2.y - px1.y;
v1.z = px2.z - px1.z;
v2.x = py2.x - py1.x;
v2.y = py2.y - py1.y;
v2.z = py2.z - py1.z;
norm->x = v1.y * v2.z - v2.y * v1.z;
norm->y = v2.x * v1.z - v1.x * v2.z;
norm->z = v1.x * v2.y - v2.x * v1.y;
lg = sqrt(norm->x * norm->x + norm->y * norm->y + norm->z * norm->z);
if (lg == 0.0) {
lg = 1.0f;
} else {
lg = 1.0f / lg;
}
norm->x *= lg;
norm->y *= lg;
norm->z *= lg;
}
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));
}
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));
}
/** 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);
}
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));
}
/*
* Function
* initStartingGrid
*
* Description
* Place the cars on the starting grid
*
* Parameters
* Race Information structure initialized
*
* Return
* none
*/
static void
initStartingGrid(void)
{
char path[64];
int i;
tTrackSeg *curseg;
int rows;
tdble a, b;
//tdble wi2; // Never used.
tdble d1, d2,d3;
tdble startpos, tr, ts;
tdble speedInit;
tdble heightInit;
tCarElt *car;
const char *pole;
void *trHdle = ReInfo->track->params;
void *params = ReInfo->params;
snprintf(path, sizeof(path), "%s/%s", ReInfo->_reRaceName, RM_SECT_STARTINGGRID);
/* Search for the first turn for find the pole side */
curseg = ReInfo->track->seg->next;
while (curseg->type == TR_STR) {
/* skip the straight segments */
curseg = curseg->next;
}
/* Set the pole for the inside of the first turn */
if (curseg->type == TR_LFT) {
pole = GfParmGetStr(params, path, RM_ATTR_POLE, "left");
} else {
pole = GfParmGetStr(params, path, RM_ATTR_POLE, "right");
}
/* Tracks definitions can force the pole side */
pole = GfParmGetStr(trHdle, RM_SECT_STARTINGGRID, RM_ATTR_POLE, pole);
if (strcmp(pole, "left") == 0) {
a = ReInfo->track->width;
b = -a;
} else {
a = 0;
b = ReInfo->track->width;
}
//wi2 = ReInfo->track->width * 0.5f; // Never used.
rows = (int)GfParmGetNum(params, path, RM_ATTR_ROWS, (char*)NULL, 2);
rows = (int)GfParmGetNum(trHdle, RM_SECT_STARTINGGRID, RM_ATTR_ROWS, (char*)NULL, (tdble)rows);
d1 = GfParmGetNum(params, path, RM_ATTR_TOSTART, (char*)NULL, 10);
d1 = GfParmGetNum(trHdle, RM_SECT_STARTINGGRID, RM_ATTR_TOSTART, (char*)NULL, d1);
d2 = GfParmGetNum(params, path, RM_ATTR_COLDIST, (char*)NULL, 10);
d2 = GfParmGetNum(trHdle, RM_SECT_STARTINGGRID, RM_ATTR_COLDIST, (char*)NULL, d2);
d3 = GfParmGetNum(params, path, RM_ATTR_COLOFFSET, (char*)NULL, 5);
d3 = GfParmGetNum(trHdle, RM_SECT_STARTINGGRID, RM_ATTR_COLOFFSET, (char*)NULL, d3);
speedInit = GfParmGetNum(params, path, RM_ATTR_INITSPEED, (char*)NULL, 0.0);
heightInit = GfParmGetNum(params, path, RM_ATTR_INITHEIGHT, (char*)NULL, 0.3f);
heightInit = GfParmGetNum(trHdle, RM_SECT_STARTINGGRID, RM_ATTR_INITHEIGHT, (char*)NULL, heightInit);
if (rows < 1) {
rows = 1;
}
for (i = 0; i < ReInfo->s->_ncars; i++) {
car = &(ReInfo->carList[i]);
car->_speed_x = speedInit;
startpos = ReInfo->track->length - (d1 + (i / rows) * d2 + (i % rows) * d3);
tr = a + b * ((i % rows) + 1) / (rows + 1);
curseg = ReInfo->track->seg; /* last segment */
while (startpos < curseg->lgfromstart) {
curseg = curseg->prev;
}
ts = startpos - curseg->lgfromstart;
car->_trkPos.seg = curseg;
car->_trkPos.toRight = tr;
switch (curseg->type) {
case TR_STR:
car->_trkPos.toStart = ts;
RtTrackLocal2Global(&(car->_trkPos), &(car->_pos_X), &(car->_pos_Y), TR_TORIGHT);
car->_yaw = curseg->angle[TR_ZS];
break;
case TR_RGT:
car->_trkPos.toStart = ts / curseg->radius;
RtTrackLocal2Global(&(car->_trkPos), &(car->_pos_X), &(car->_pos_Y), TR_TORIGHT);
car->_yaw = curseg->angle[TR_ZS] - car->_trkPos.toStart;
break;
case TR_LFT:
car->_trkPos.toStart = ts / curseg->radius;
RtTrackLocal2Global(&(car->_trkPos), &(car->_pos_X), &(car->_pos_Y), TR_TORIGHT);
car->_yaw = curseg->angle[TR_ZS] + car->_trkPos.toStart;
break;
}
car->_pos_Z = RtTrackHeightL(&(car->_trkPos)) + heightInit;
FLOAT_NORM0_2PI(car->_yaw);
RePhysicsEngine().configureCar(car);
}
}
static void
RemoveCar(tCar *car, tSituation *s)
{
int i;
tCarElt *carElt;
tTrkLocPos trkPos;
int trkFlag;
tdble travelTime;
tdble dang;
#define PULL_Z_OFFSET 3.0
#define PULL_SPD 0.5
carElt = car->carElt;
if (carElt->_state & RM_CAR_STATE_PULLUP) {
carElt->_pos_Z += car->restPos.vel.z * SimDeltaTime;
carElt->_yaw += car->restPos.vel.az * SimDeltaTime;
carElt->_roll += car->restPos.vel.ax * SimDeltaTime;
carElt->_pitch += car->restPos.vel.ay * SimDeltaTime;
sgMakeCoordMat4(carElt->pub.posMat, carElt->_pos_X, carElt->_pos_Y, carElt->_pos_Z - carElt->_statGC_z,
RAD2DEG(carElt->_yaw), RAD2DEG(carElt->_roll), RAD2DEG(carElt->_pitch));
if (carElt->_pos_Z > (car->restPos.pos.z + PULL_Z_OFFSET)) {
carElt->_state &= ~RM_CAR_STATE_PULLUP;
carElt->_state |= RM_CAR_STATE_PULLSIDE;
travelTime = DIST(car->restPos.pos.x, car->restPos.pos.y, carElt->_pos_X, carElt->_pos_Y) / PULL_SPD;
car->restPos.vel.x = (car->restPos.pos.x - carElt->_pos_X) / travelTime;
car->restPos.vel.y = (car->restPos.pos.y - carElt->_pos_Y) / travelTime;
}
return;
}
if (carElt->_state & RM_CAR_STATE_PULLSIDE) {
carElt->_pos_X += car->restPos.vel.x * SimDeltaTime;
carElt->_pos_Y += car->restPos.vel.y * SimDeltaTime;
sgMakeCoordMat4(carElt->pub.posMat, carElt->_pos_X, carElt->_pos_Y, carElt->_pos_Z - carElt->_statGC_z,
RAD2DEG(carElt->_yaw), RAD2DEG(carElt->_roll), RAD2DEG(carElt->_pitch));
if ((fabs(car->restPos.pos.x - carElt->_pos_X) < 0.5) && (fabs(car->restPos.pos.y - carElt->_pos_Y) < 0.5)) {
carElt->_state &= ~RM_CAR_STATE_PULLSIDE;
carElt->_state |= RM_CAR_STATE_PULLDN;
}
return;
}
if (carElt->_state & RM_CAR_STATE_PULLDN) {
carElt->_pos_Z -= car->restPos.vel.z * SimDeltaTime;
sgMakeCoordMat4(carElt->pub.posMat, carElt->_pos_X, carElt->_pos_Y, carElt->_pos_Z - carElt->_statGC_z,
RAD2DEG(carElt->_yaw), RAD2DEG(carElt->_roll), RAD2DEG(carElt->_pitch));
if (carElt->_pos_Z < car->restPos.pos.z) {
carElt->_state &= ~RM_CAR_STATE_PULLDN;
carElt->_state |= RM_CAR_STATE_OUT;
}
return;
}
if (carElt->_state & RM_CAR_STATE_NO_SIMU) {
return;
}
if ((s->_maxDammage) && (car->dammage > s->_maxDammage)) {
carElt->_state |= RM_CAR_STATE_BROKEN;
} else {
carElt->_state |= RM_CAR_STATE_OUTOFGAS;
}
carElt->_gear = car->transmission.gearbox.gear = 0;
carElt->_enginerpm = car->engine.rads = 0;
if (!(carElt->_state & RM_CAR_STATE_DNF)) {
if (fabs(carElt->_speed_x) > 1.0) {
return;
}
}
carElt->_state |= RM_CAR_STATE_PULLUP;
carElt->priv.collision = car->collision = 0;
for(i = 0; i < 4; i++) {
carElt->_skid[i] = 0;
carElt->_wheelSpinVel(i) = 0;
carElt->_brakeTemp(i) = 0;
}
carElt->pub.DynGC = car->DynGC;
carElt->_speed_x = 0;
/* compute the target zone for the wrecked car */
trkPos = car->trkPos;
if (trkPos.toRight > trkPos.seg->width / 2.0) {
while (trkPos.seg->lside != 0) {
trkPos.seg = trkPos.seg->lside;
}
trkPos.toLeft = -3.0;
trkFlag = TR_TOLEFT;
} else {
while (trkPos.seg->rside != 0) {
trkPos.seg = trkPos.seg->rside;
}
trkPos.toRight = -3.0;
trkFlag = TR_TORIGHT;
}
trkPos.type = TR_LPOS_SEGMENT;
RtTrackLocal2Global(&trkPos, &(car->restPos.pos.x), &(car->restPos.pos.y), trkFlag);
car->restPos.pos.z = RtTrackHeightL(&trkPos) + carElt->_statGC_z;
car->restPos.pos.az = RtTrackSideTgAngleL(&trkPos);
car->restPos.pos.ax = 0;
car->restPos.pos.ay = 0;
car->restPos.vel.z = PULL_SPD;
travelTime = (car->restPos.pos.z + PULL_Z_OFFSET - carElt->_pos_Z) / car->restPos.vel.z;
dang = car->restPos.pos.az - carElt->_yaw;
NORM_PI_PI(dang);
car->restPos.vel.az = dang / travelTime;
dang = car->restPos.pos.ax - carElt->_roll;
NORM_PI_PI(dang);
car->restPos.vel.ax = dang / travelTime;
dang = car->restPos.pos.ay - carElt->_pitch;
NORM_PI_PI(dang);
car->restPos.vel.ay = dang / travelTime;
}
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;
}
}
/* Drive during race. */
static void
drive(int index, tCarElt* car, tSituation *s)
{
total_tics[index]++;
char line[UDP_MSGLEN];
#ifdef __PRINT_RACE_RESULTS__
bestLap[index]=car->_bestLapTime;
damages[index]=car->_dammage;
totalTime[index]=car->_timeBehindLeader;
#endif
#ifdef __DISABLE_RESTART__
if (RESTARTING[index]==1)
{
clientAddressLength[index] = sizeof(clientAddress[index]);
// Set line to all zeroes
memset(line, 0x0, 101);
if (recvfrom(listenSocket[index], line, 100, 0,
(struct sockaddr *) &clientAddress[index],
&clientAddressLength[index]) < 0)
{
std::cerr << "Error: problem in receiving from the listen socket";
exit(1);
}
#ifdef __UDP_SERVER_VERBOSE__
// show the client's IP address
std::cout << " from " << inet_ntoa(clientAddress[index].sin_addr);
// show the client's port number.
std::cout << ":" << ntohs(clientAddress[index].sin_port) << "\n";
// Show the line
std::cout << " Received: " << line << "\n";
#endif
// compare received string with the ID
if (strncmp(line,UDP_ID,3)==0)
{
#ifdef __UDP_SERVER_VERBOSE__
std::cout << "IDENTIFIED" << std::endl;
#endif
// char line[UDP_MSGLEN];
sprintf(line,"***identified***");
// Sending the car state to the client
if (sendto(listenSocket[index], line, strlen(line) + 1, 0,
(struct sockaddr *) &clientAddress[index],
sizeof(clientAddress[index])) < 0)
std::cerr << "Error: cannot send identification message";
RESTARTING[index]=0;
}
}
#endif
// local variables for UDP
struct timeval timeVal;
fd_set readSet;
// computing distance to middle
float dist_to_middle = 2*car->_trkPos.toMiddle/(car->_trkPos.seg->width);
// computing the car angle wrt the track axis
float angle = RtTrackSideTgAngleL(&(car->_trkPos)) - car->_yaw;
NORM_PI_PI(angle); // normalize the angle between -PI and + PI
//Update focus sensors' angle
for (int i = 0; i < 5; ++i) {
focusSens[index]->setSensor(i,(car->_focusCmd)+i-2,200);
}
// update the value of track sensors only as long as the car is inside the track
float trackSensorOut[19];
float focusSensorOut[5];//ML
if (dist_to_middle<=1.0 && dist_to_middle >=-1.0 )
{
trackSens[index]->sensors_update();
for (int i = 0; i < 19; ++i)
{
trackSensorOut[i] = trackSens[index]->getSensorOut(i);
if (getNoisy())
trackSensorOut[i] *= normRand(1,__NOISE_STD__);
}
focusSens[index]->sensors_update();//ML
if ((car->_focusCD <= car->_curLapTime + car->_curTime)//ML Only send focus sensor reading if cooldown is over
&& (car->_focusCmd != 360))//ML Only send focus reading if requested by client
{//ML
for (int i = 0; i < 5; ++i)
{
focusSensorOut[i] = focusSens[index]->getSensorOut(i);
if (getNoisy())
focusSensorOut[i] *= normRand(1,__FOCUS_NOISE_STD__);
}
car->_focusCD = car->_curLapTime + car->_curTime + 1.0;//ML Add cooldown [seconds]
}//ML
else//ML
{//ML
for (int i = 0; i < 5; ++i)//ML
focusSensorOut[i] = -1;//ML During cooldown send invalid focus reading
}//ML
}
else
{
for (int i = 0; i < 19; ++i)
{
trackSensorOut[i] = -1;
}
for (int i = 0; i < 5; ++i)
{
focusSensorOut[i] = -1;
}
}
// update the value of opponent sensors
float oppSensorOut[36];
oppSens[index]->sensors_update(s);
for (int i = 0; i < 36; ++i)
{
oppSensorOut[i] = oppSens[index]->getObstacleSensorOut(i);
if (getNoisy())
oppSensorOut[i] *= normRand(1,__OPP_NOISE_STD__);
}
float wheelSpinVel[4];
for (int i=0; i<4; ++i)
{
wheelSpinVel[i] = car->_wheelSpinVel(i);
}
if (prevDist[index]<0)
{
prevDist[index] = car->race.distFromStartLine;
}
float curDistRaced = car->race.distFromStartLine - prevDist[index];
prevDist[index] = car->race.distFromStartLine;
if (curDistRaced>100)
{
curDistRaced -= curTrack->length;
}
if (curDistRaced<-100)
{
curDistRaced += curTrack->length;
}
distRaced[index] += curDistRaced;
float totdist = curTrack->length * (car->race.laps -1) + car->race.distFromStartLine;
// std::cerr << "totraced: " << totdist << std::endl;
/**********************************************************************
****************** Building state string *****************************
**********************************************************************/
string stateString;
stateString = SimpleParser::stringify("angle", angle);
stateString += SimpleParser::stringify("curLapTime", float(car->_curLapTime));
stateString += SimpleParser::stringify("damage", ( getDamageLimit() ? car->_dammage : car->_fakeDammage ) );
stateString += SimpleParser::stringify("distFromStart", car->race.distFromStartLine);
stateString += SimpleParser::stringify("totalDistFromStart", totdist);
stateString += SimpleParser::stringify("distRaced", distRaced[index]);
stateString += SimpleParser::stringify("fuel", car->_fuel);
stateString += SimpleParser::stringify("gear", car->_gear);
stateString += SimpleParser::stringify("lastLapTime", float(car->_lastLapTime));
stateString += SimpleParser::stringify("opponents", oppSensorOut, 36);
stateString += SimpleParser::stringify("racePos", car->race.pos);
stateString += SimpleParser::stringify("rpm", car->_enginerpm*10);
stateString += SimpleParser::stringify("speedX", float(car->_speed_x * 3.6));
stateString += SimpleParser::stringify("speedY", float(car->_speed_y * 3.6));
stateString += SimpleParser::stringify("speedZ", float(car->_speed_z * 3.6));
stateString += SimpleParser::stringify("track", trackSensorOut, 19);
stateString += SimpleParser::stringify("trackPos", dist_to_middle);
stateString += SimpleParser::stringify("wheelSpinVel", wheelSpinVel, 4);
stateString += SimpleParser::stringify("z", car->_pos_Z - RtTrackHeightL(&(car->_trkPos)));
stateString += SimpleParser::stringify("focus", focusSensorOut, 5);//ML
//GIUSE - VISION HERE!
// printf("size: %d\n",car->vision->imgsize);
if( getVision() ){
// std::cout << car->vision->imgsize << std::endl;
stateString += SimpleParser::stringify("img", car->vision->img, car->vision->imgsize);
}
// for(int i=0; i < car->vision->imgsize; i++){
// std::cout << (int)car->vision->img[i] << " ";
// }
// GIUSE - that's UGLY, can we stay coherent with either char* or string??
// char line[UDP_MSGLEN];
// memset(line, 0x0,UDP_MSGLEN ); //
// sprintf(line,"%s",stateString.c_str());
if (RESTARTING[index]==0)
{
#ifdef __UDP_SERVER_VERBOSE__
std::cout << "Sending: " << stateString.c_str() << std::endl;
std::cout << "Sending: " << stateString.c_str() << std::endl;
#endif
#ifdef __STEP_LIMIT__
if (total_tics[index]>__STEP_LIMIT__)
{
RESTARTING[index] = 1;
car->RESTART=1;
char fileName[200];
sprintf(fileName,"%s.txt",trackName);
printf("%s.txt\n",trackName);
FILE *f = fopen (fileName,"a");
printf("Dist_raced %lf\n",distRaced[index]);
fprintf(f,"Dist_raced %lf\n",distRaced[index]);
fclose(f);
return;
}
#endif
// Sending the car state to the client
// if (sendto(listenSocket[index], line, strlen(line) + 1, 0,
if (sendto(listenSocket[index], stateString.c_str(), stateString.length() + 1, 0,
(struct sockaddr *) &clientAddress[index],
sizeof(clientAddress[index])) < 0)
std::cerr << "Error: cannot send car state";
// Set timeout for client answer
FD_ZERO(&readSet);
FD_SET(listenSocket[index], &readSet);
timeVal.tv_sec = 0;
timeVal.tv_usec = UDP_TIMEOUT;
memset(line, 0x0,UDP_MSGLEN ); // GIUSE - BUG, THERE WAS A 1000 HARDCODED
if (select(listenSocket[index]+1, &readSet, NULL, NULL, &timeVal))
{
// Read the client controller action
// memset(line, 0x0,UDP_MSGLEN ); // Zero out the buffer. GIUSE - already done
int numRead = recv(listenSocket[index], line, UDP_MSGLEN, 0);
if (numRead < 0)
{
std::cerr << "Error, cannot get any response from the client!";
CLOSE(listenSocket[index]);
exit(1);
}
#ifdef __UDP_SERVER_VERBOSE__
std::cout << "Received: " << line << std::endl;
#endif
std::string lineStr(line);
CarControl carCtrl(lineStr);
if (carCtrl.getMeta()==RACE_RESTART)
{
RESTARTING[index] = 1;
#ifdef __DISABLE_RESTART__
// char line[UDP_MSGLEN];
memset(line, 0x0,UDP_MSGLEN );
sprintf(line,"***restart***");
// Sending the car state to the client
if (sendto(listenSocket[index], line, strlen(line) + 1, 0,
(struct sockaddr *) &clientAddress[index],
sizeof(clientAddress[index])) < 0)
std::cerr << "Error: cannot send restart message";
#else
car->RESTART=1;
#endif
}
// Set controls command and store them in variables
oldAccel[index] = car->_accelCmd = carCtrl.getAccel();
oldBrake[index] = car->_brakeCmd = carCtrl.getBrake();
oldGear[index] = car->_gearCmd = carCtrl.getGear();
oldSteer[index] = car->_steerCmd = carCtrl.getSteer();
oldClutch[index] = car->_clutchCmd = carCtrl.getClutch();
oldFocus[index] = car->_focusCmd = carCtrl.getFocus();//ML
}
else
{
//#ifdef __UDP_SERVER_VERBOSE__
std::cout << "Timeout for client answer\n";
//#endif
// If no new controls are availables uses old ones...
car->_accelCmd = oldAccel[index];
car->_brakeCmd = oldBrake[index];
car->_gearCmd = oldGear[index];
car->_steerCmd = oldSteer[index];
car->_clutchCmd = oldClutch[index];
car->_focusCmd = oldFocus[index];//ML
}
}
else
{
car->_accelCmd = oldAccel[index];
car->_brakeCmd = oldBrake[index];
car->_gearCmd = oldGear[index];
car->_steerCmd = oldSteer[index];
car->_clutchCmd = oldClutch[index];
car->_focusCmd = oldFocus[index];//ML
}
}
static void
RemoveCar(tCar *car, tSituation *s)
{
int i;
tCarElt *carElt;
tTrkLocPos trkPos;
int trkFlag;
tdble travelTime;
tdble dang;
static tdble PULL_Z_OFFSET = 3.0;
static tdble PULL_SPD = 0.5;
carElt = car->carElt;
if (carElt->_state & RM_CAR_STATE_PULLUP) {
carElt->_pos_Z += car->restPos.vel.z * SimDeltaTime;
carElt->_yaw += car->restPos.vel.az * SimDeltaTime;
carElt->_roll += car->restPos.vel.ax * SimDeltaTime;
carElt->_pitch += car->restPos.vel.ay * SimDeltaTime;
sgMakeCoordMat4(carElt->pub.posMat, carElt->_pos_X, carElt->_pos_Y, carElt->_pos_Z - carElt->_statGC_z,
(float) RAD2DEG(carElt->_yaw), (float) RAD2DEG(carElt->_roll), (float) RAD2DEG(carElt->_pitch));
if (carElt->_pos_Z > (car->restPos.pos.z + PULL_Z_OFFSET)) {
carElt->_state &= ~RM_CAR_STATE_PULLUP;
carElt->_state |= RM_CAR_STATE_PULLSIDE;
// Moved pullside velocity computation down due to floating point error accumulation.
}
return;
}
if (carElt->_state & RM_CAR_STATE_PULLSIDE) {
// Recompute speed to avoid missing the parking point due to error accumulation (the pos might be
// in the 0-10000 range, depending on the track and vel*dt is around 0-0.001, so basically all
// but the most significant digits are lost under bad conditions, happens e.g on e-track-4).
// Should not lead to a division by zero because the pullside process stops if the car is within
// [0.5, 0.5]. Do not move it back.
travelTime = DIST(car->restPos.pos.x, car->restPos.pos.y, carElt->_pos_X, carElt->_pos_Y) / PULL_SPD;
car->restPos.vel.x = (car->restPos.pos.x - carElt->_pos_X) / travelTime;
car->restPos.vel.y = (car->restPos.pos.y - carElt->_pos_Y) / travelTime;
carElt->_pos_X += car->restPos.vel.x * SimDeltaTime;
carElt->_pos_Y += car->restPos.vel.y * SimDeltaTime;
sgMakeCoordMat4(carElt->pub.posMat, carElt->_pos_X, carElt->_pos_Y, carElt->_pos_Z - carElt->_statGC_z,
(float) RAD2DEG(carElt->_yaw), (float) RAD2DEG(carElt->_roll), (float) RAD2DEG(carElt->_pitch));
if ((fabs(car->restPos.pos.x - carElt->_pos_X) < 0.5) && (fabs(car->restPos.pos.y - carElt->_pos_Y) < 0.5)) {
carElt->_state &= ~RM_CAR_STATE_PULLSIDE;
carElt->_state |= RM_CAR_STATE_PULLDN;
}
return;
}
if (carElt->_state & RM_CAR_STATE_PULLDN) {
carElt->_pos_Z -= car->restPos.vel.z * SimDeltaTime;
sgMakeCoordMat4(carElt->pub.posMat, carElt->_pos_X, carElt->_pos_Y, carElt->_pos_Z - carElt->_statGC_z,
(float) RAD2DEG(carElt->_yaw), (float) RAD2DEG(carElt->_roll), (float) RAD2DEG(carElt->_pitch));
if (carElt->_pos_Z < car->restPos.pos.z) {
carElt->_state &= ~RM_CAR_STATE_PULLDN;
carElt->_state |= RM_CAR_STATE_OUT;
}
return;
}
if (carElt->_state & (RM_CAR_STATE_NO_SIMU & ~RM_CAR_STATE_PIT)) {
return;
}
if (carElt->_state & RM_CAR_STATE_PIT) {
if ((s->_maxDammage) && (car->dammage > s->_maxDammage)) {
// Broken during pit stop.
carElt->_state &= ~RM_CAR_STATE_PIT;
carElt->_pit->pitCarIndex = TR_PIT_STATE_FREE;
} else {
return;
}
}
if ((s->_maxDammage) && (car->dammage > s->_maxDammage)) {
carElt->_state |= RM_CAR_STATE_BROKEN;
} else {
carElt->_state |= RM_CAR_STATE_OUTOFGAS;
}
carElt->_gear = car->transmission.gearbox.gear = 0;
carElt->_enginerpm = car->engine.rads = 0;
if (!(carElt->_state & RM_CAR_STATE_DNF)) {
if (fabs(carElt->_speed_x) > 1.0) {
return;
}
}
carElt->_state |= RM_CAR_STATE_PULLUP;
// RM_CAR_STATE_NO_SIMU evaluates to > 0 from here, so we remove the car from the
// collision detection.
SimCollideRemoveCar(car, s->_ncars);
carElt->priv.collision = car->collision = 0;
for(i = 0; i < 4; i++) {
carElt->_skid[i] = 0;
carElt->_wheelSpinVel(i) = 0;
carElt->_brakeTemp(i) = 0;
}
carElt->pub.DynGC = car->DynGC;
carElt->_speed_x = 0;
// Compute the target zone for the wrecked car.
trkPos = car->trkPos;
if (trkPos.toRight > trkPos.seg->width / 2.0) {
while (trkPos.seg->lside != 0) {
trkPos.seg = trkPos.seg->lside;
}
trkPos.toLeft = -3.0;
trkFlag = TR_TOLEFT;
} else {
while (trkPos.seg->rside != 0) {
trkPos.seg = trkPos.seg->rside;
}
trkPos.toRight = -3.0;
trkFlag = TR_TORIGHT;
}
trkPos.type = TR_LPOS_SEGMENT;
RtTrackLocal2Global(&trkPos, &(car->restPos.pos.x), &(car->restPos.pos.y), trkFlag);
car->restPos.pos.z = RtTrackHeightL(&trkPos) + carElt->_statGC_z;
car->restPos.pos.az = RtTrackSideTgAngleL(&trkPos);
car->restPos.pos.ax = 0;
car->restPos.pos.ay = 0;
car->restPos.vel.z = PULL_SPD;
travelTime = (car->restPos.pos.z + PULL_Z_OFFSET - carElt->_pos_Z) / car->restPos.vel.z;
dang = car->restPos.pos.az - carElt->_yaw;
FLOAT_NORM_PI_PI(dang);
car->restPos.vel.az = dang / travelTime;
dang = car->restPos.pos.ax - carElt->_roll;
FLOAT_NORM_PI_PI(dang);
car->restPos.vel.ax = dang / travelTime;
dang = car->restPos.pos.ay - carElt->_pitch;
FLOAT_NORM_PI_PI(dang);
car->restPos.vel.ay = dang / travelTime;
}
