/* Drive during race. */
static void
drive(int index, tCarElt* car, tSituation *s)
{
float angle;
const float SC = 1.0;
// memset((void *)&car->ctrl, 0, sizeof(tCarCtrl));
memset(&car->ctrl, 0, sizeof(tCarCtrl));
if (isStuck(car)) {
angle = -RtTrackSideTgAngleL(&(car->_trkPos)) + car->_yaw;
NORM_PI_PI(angle); // put the angle back in the range from -PI to PI
car->ctrl.steer = angle / car->_steerLock;
car->ctrl.gear = -1; // reverse gear
car->ctrl.accelCmd = 0.3; // 30% accelerator pedal
car->ctrl.brakeCmd = 0.0; // no brakes
}
else {
angle = RtTrackSideTgAngleL(&(car->_trkPos)) - car->_yaw;
NORM_PI_PI(angle); // put the angle back in the range from -PI to PI
angle -= SC*car->_trkPos.toMiddle/car->_trkPos.seg->width;
car->ctrl.steer = angle / car->_steerLock;
car->ctrl.gear = 1; // first gear
car->ctrl.accelCmd = 0.3; // 30% accelerator pedal
car->ctrl.brakeCmd = 0.0; // no brakes
}
}
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);
}
/* Hold car on the track */
float Driver::filterTrk(float accel)
{
tTrackSeg* seg = car->_trkPos.seg;
float speedangle = trackangle - atan2(car->_speed_Y, car->_speed_X);
NORM_PI_PI(speedangle);
if (car->_speed_x < MAX_UNSTUCK_SPEED ||
pit->getInPit() ||
car->_trkPos.toMiddle*speedangle > 0.0) return accel;
if (seg->type == TR_STR) {
float tm = fabs(car->_trkPos.toMiddle);
float w = seg->width/WIDTHDIV;
if (tm > w) return 0.0; else return accel;
} else {
float sign = (seg->type == TR_RGT) ? -1.0 : 1.0;
if (car->_trkPos.toMiddle*sign > 0.0) {
return accel;
} else {
float tm = fabs(car->_trkPos.toMiddle);
float w = seg->width/WIDTHDIV;
if (tm > w) return 0.0; else return accel;
}
}
}
void SingleCardata::update() {
trackangle = RtTrackSideTgAngleL(const_cast<tTrkLocPos*>(&(car->_trkPos)));
speed = getSpeed(car, trackangle);
angle = trackangle - car->_yaw;
NORM_PI_PI(angle);
width =
MAX(car->_dimension_y,
fabs(car->_dimension_x * sin(angle) +
car->_dimension_y * cos(angle))) + 0.1;
length =
MAX(car->_dimension_x,
fabs(car->_dimension_y * sin(angle) +
car->_dimension_x * cos(angle))) + 0.1;
for (int i = 0; i < 4; i++) {
corner2[i].ax = corner1[i].ax;
corner2[i].ay = corner1[i].ay;
corner1[i].ax = car->_corner_x(i);
corner1[i].ay = car->_corner_y(i);
} // for i
lastspeed[2].ax = lastspeed[1].ax;
lastspeed[2].ay = lastspeed[1].ay;
lastspeed[1].ax = lastspeed[0].ax;
lastspeed[1].ay = lastspeed[0].ay;
lastspeed[0].ax = car->_speed_X;
lastspeed[0].ay = car->_speed_Y;
} // update
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;
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);
}
void SingleSensor::update()
{
//Angolo del sensore relativo al segmento del tracciato
float relative_sensor_angle = RtTrackSideTgAngleL(&(car->_trkPos)) - car->_yaw + sensor_angle;
/* printf ("\nabsolute_car_angle: %f (degree)", (car->_yaw*180)/PI);
printf ("\nabsolute_sensor_angle: %f (degree)", (absolute_sensor_angle*180)/PI);
printf ("\nabsolute_track_angle: %f (degree)", (absolute_track_angle*180)/PI);
printf ("\nrelative_sensor_angle: %f (degree)", (relative_sensor_angle*180)/PI);
printf ("\nsensor_angle: %f (degree)", (sensor_angle*180)/PI);*/
NORM_PI_PI(relative_sensor_angle);
/* printf ("\nrelative_sensor_angle normalized: %f (degree)", (relative_sensor_angle*180)/PI);
printf ("\nX1: %f (meters)", car->_trkPos.toLeft);
printf ("\nX2: %f (meters)", car->_trkPos.toRight);
printf ("\nY: %f (meters or arc)", car->_trkPos.toStart);
printf ("\nTo Middle: %f (meters)", car->_trkPos.toMiddle);
printf ("\nTrack Segment Start Width: %f (meters)", car->_trkPos.seg->startWidth);
printf ("\nTrack Segment End Width: %f (meters)", car->_trkPos.seg->endWidth);
printf ("\nTrack Segment Length: %f (meters)\n", car->_trkPos.seg->length);*/
switch (car->_trkPos.seg->type)
{
case 3: sensor_out = sensor_calc_str(car->_trkPos.seg, car->_trkPos.toLeft, car->_trkPos.toStart, -relative_sensor_angle, sensor_range);
break;
case 2: sensor_out = sensor_calc_lft_rgt(car->_trkPos.seg, car->_trkPos.toLeft, car->_trkPos.toStart, -relative_sensor_angle, sensor_range);
break;
case 1: sensor_out = sensor_calc_lft_rgt(car->_trkPos.seg, car->_trkPos.toLeft, car->_trkPos.toStart, -relative_sensor_angle, sensor_range);
break;
}
}
/* Update my private data every timestep - and reset control */
void Driver::update(tCarElt* car, tSituation *s)
{
trackangle = RtTrackSideTgAngleL(&(car->_trkPos));
angle = trackangle - car->_yaw;
NORM_PI_PI(angle);
memset(&car->ctrl, 0, sizeof(tCarCtrl));
}
// Compute steer value.
float Driver::getSteer()
{
float targetAngle;
vec2f target = getTargetPoint();
targetAngle = atan2(target.y - car->_pos_Y, target.x - car->_pos_X);
targetAngle -= car->_yaw;
NORM_PI_PI(targetAngle);
return targetAngle / car->_steerLock;
}
/* Update my private data every timestep */
void Driver::update(tSituation *s)
{
trackangle = RtTrackSideTgAngleL(&(car->_trkPos));
angle = trackangle - car->_yaw;
NORM_PI_PI(angle);
mass = CARMASS + car->_fuel;
currentspeedsqr = car->_speed_x*car->_speed_x;
speed = Opponent::getSpeed(car);
opponents->update(s, this);
pit->update();
}
/* Steer filter for collision avoidance */
float Driver::filterSColl(float steer)
{
int i;
float sidedist = 0.0, fsidedist = 0.0, minsidedist = FLT_MAX;
Opponent *o = NULL;
/* get the index of the nearest car (o) */
for (i = 0; i < opponents->getNOpponents(); i++) {
if (opponent[i].getState() & OPP_SIDE) {
sidedist = opponent[i].getSideDist();
fsidedist = fabs(sidedist);
if (fsidedist < minsidedist) {
minsidedist = fsidedist;
o = &opponent[i];
}
}
}
/* if there is another car handle the situation */
if (o != NULL) {
float d = fsidedist - o->getWidth();
/* near enough */
if (d < SIDECOLL_MARGIN) {
/* compute angle between cars */
tCarElt *ocar = o->getCarPtr();
float diffangle = ocar->_yaw - car->_yaw;
NORM_PI_PI(diffangle);
/* we are near and heading toward the car */
if (diffangle*o->getSideDist() < 0.0) {
const float c = SIDECOLL_MARGIN/2.0;
d = d - c;
if (d < 0.0) d = 0.0;
float psteer = diffangle/car->_steerLock;
myoffset = car->_trkPos.toMiddle;
float w = o->getCarPtr()->_trkPos.seg->width/WIDTHDIV-BORDER_OVERTAKE_MARGIN;
if (fabs(myoffset) > w) {
myoffset = (myoffset > 0) ? w : -w;
}
psteer = steer*(d/c) + 2.0*psteer*(1.0-d/c);
if (psteer*steer > 0.0 && fabs(steer) > fabs(psteer)) {
return steer;
} else {
return psteer;
}
}
}
}
return steer;
}
/* follow the center of the track */
void Driver::followcenter(tCarElt* car)
{
angle -= STEERING_CONSTANT * car->_trkPos.toMiddle / car->_trkPos.seg->width;
NORM_PI_PI(angle); // put the angle back in the range from -PI to PI
car->ctrl.steer = angle / car->_steerLock;
car->ctrl.gear = 2; // first gear
car->ctrl.accelCmd = 0.8; // accelerator pedal
car->ctrl.brakeCmd = 0.0; // no brakes
printf("%d ", car->vision->img[1]);
// printf("Lap: %d", car->_laps);
// std::cout << "Lap: " << car->_lap << std::endl;
}
static void drive(int index, tCarElt* car, tSituation *s)
{
memset(&car->ctrl, 0, sizeof(tCarCtrl));
if (isStuck(car)) {
float angle = -RtTrackSideTgAngleL(&(car->_trkPos)) + car->_yaw;
NORM_PI_PI(angle); // put the angle back in the range from -PI to PI
car->ctrl.steer = angle / car->_steerLock;
car->ctrl.gear = -1; // reverse gear
car->ctrl.accelCmd = 0.3; // 30% accelerator pedal
car->ctrl.brakeCmd = 0.0; // no brakes
}
else {
float angle;
const float SC = 1.0;
angle = RtTrackSideTgAngleL(&(car->_trkPos)) - car->_yaw;
NORM_PI_PI(angle); // put the angle back in the range from -PI to PI
angle -= SC*(car->_trkPos.toMiddle+keepLR)/car->_trkPos.seg->width;
// set up the values to return
car->ctrl.steer = angle / car->_steerLock;
car->ctrl.gear = getGear(car);
if (car->_speed_x>desired_speed) {
car->ctrl.brakeCmd=0.5;
car->ctrl.accelCmd=0.0;
}
else if (car->_speed_x<desired_speed) {
car->ctrl.accelCmd=0.5;
car->ctrl.brakeCmd=0.0;
}
}
}
/* check if the car is stuck */
bool isStuck(tCarElt* car)
{
float angle = RtTrackSideTgAngleL(&(car->_trkPos)) - car->_yaw;
NORM_PI_PI(angle);
// angle smaller than 30 degrees?
if (fabs(angle) < 30.0/180.0*PI) {
stuck = 0;
return false;
}
if (stuck < 100) {
stuck++;
return false;
} else {
return true;
}
}
void
SimWheelUpdateRotation(tCar *car)
{
int i;
tWheel *wheel;
for (i = 0; i < 4; i++) {
wheel = &(car->wheel[i]);
wheel->spinVel = wheel->in.spinVel;
RELAXATION2(wheel->spinVel, wheel->prespinVel, 50.0f);
wheel->relPos.ay += wheel->spinVel * SimDeltaTime;
NORM_PI_PI(wheel->relPos.ay);
car->carElt->_wheelSpinVel(i) = wheel->spinVel;
}
}
bool isStuck(tCarElt* car)
{
float angle = RtTrackSideTgAngleL(&(car->_trkPos)) - car->_yaw;
NORM_PI_PI(angle);
if (fabs(angle) > MAX_UNSTUCK_ANGLE &&
car->_speed_x < MAX_UNSTUCK_SPEED &&
fabs(car->_trkPos.toMiddle) > MIN_UNSTUCK_DIST) {
if (stuck > MAX_UNSTUCK_COUNT && car->_trkPos.toMiddle*angle < 0.0) {
return true;
} else {
stuck++;
return false;
}
} else {
stuck = 0;
return false;
}
}
// Update my private data every timestep.
void Driver::update(tSituation *s)
{
// Update global car data (shared by all instances) just once per timestep.
if (currentsimtime != s->currentTime) {
currentsimtime = s->currentTime;
cardata->update();
}
// Update the local data rest.
speedangle = mycardata->getTrackangle() - atan2(car->_speed_Y, car->_speed_X);
NORM_PI_PI(speedangle);
mass = CARMASS + car->_fuel;
currentspeedsqr = car->_speed_x*car->_speed_x;
opponents->update(s, this);
strategy->update(car, s);
if (!pit->getPitstop()) {
pit->setPitstop(strategy->needPitstop(car, s));
}
pit->update();
alone = isAlone();
learn->update(s, track, car, alone, myoffset, car->_trkPos.seg->width/WIDTHDIV-BORDER_OVERTAKE_MARGIN, radius);
}
void
SimAeroUpdate(tCar *car, tSituation *s)
{
//tdble hm;
int i;
tdble airSpeed;
tdble dragK = 1.0;
airSpeed = car->DynGC.vel.x;
if (airSpeed > 10.0) {
tdble x = car->DynGC.pos.x;
tdble y = car->DynGC.pos.y;
// tdble x = car->DynGC.pos.x + cos(yaw)*wing->staticPos.x;
// tdble y = car->DynGC.pos.y + sin(yaw)*wing->staticPos.x;
tdble yaw = car->DynGC.pos.az;
tdble spdang = atan2(car->DynGCg.vel.y, car->DynGCg.vel.x);
for (i = 0; i < s->_ncars; i++) {
if (i == car->carElt->index) {
continue;
}
tdble tmpas = 1.00;
tCar* otherCar = &(SimCarTable[i]);
tdble otherYaw = otherCar->DynGC.pos.az;
tdble tmpsdpang = spdang - atan2(y - otherCar->DynGC.pos.y, x - otherCar->DynGC.pos.x);
NORM_PI_PI(tmpsdpang);
tdble dyaw = yaw - otherYaw;
NORM_PI_PI(dyaw);
if ((otherCar->DynGC.vel.x > 10.0) &&
(fabs(dyaw) < 0.1396)) {
if (fabs(tmpsdpang) > 2.9671) { /* 10 degrees */
/* behind another car - reduce overall airflow */
tdble factor = (fabs(tmpsdpang)-2.9671)/(M_PI-2.9671);
tmpas = 1.0 - factor * exp(- 2.0 * DIST(x, y, otherCar->DynGC.pos.x, otherCar->DynGC.pos.y)/(otherCar->aero.Cd * otherCar->DynGC.vel.x));
airSpeed = airSpeed * tmpas;
} else if (fabs(tmpsdpang) < 0.1396f) { /* 8 degrees */
tdble factor = 0.5f * (0.1396f-fabs(tmpsdpang))/(0.1396f);
/* before another car - breaks down rear eddies, reduces only drag*/
tmpas = 1.0f - factor * exp(- 8.0 * DIST(x, y, otherCar->DynGC.pos.x, otherCar->DynGC.pos.y) / (car->aero.Cd * car->DynGC.vel.x));
dragK = dragK * tmpas;
}
}
}
}
car->airSpeed2 = airSpeed * airSpeed;
tdble v2 = car->airSpeed2;
tdble dmg_coef = ((tdble)car->dammage / 10000.0);
car->aero.drag = -SIGN(car->DynGC.vel.x) * car->aero.SCx2 * v2 * (1.0 + dmg_coef) * dragK * dragK;
// Since we have the forces ready, we just multiply.
// Should insert constants here.
// Also, no torque is produced since the effect can be
// quite dramatic. Interesting idea to make all drags produce
// torque when the car is damaged.
car->aero.Mx = car->aero.drag * dmg_coef * car->aero.rot_front[0];
car->aero.My = car->aero.drag * dmg_coef * car->aero.rot_front[1];
car->aero.Mz = car->aero.drag * dmg_coef * car->aero.rot_front[2];
v2 = car->DynGC.vel.y;
car->aero.lateral_drag = -SIGN(v2)*v2*v2*0.7;
car->aero.Mx += car->aero.lateral_drag * dmg_coef * car->aero.rot_lateral[0];
car->aero.My += car->aero.lateral_drag * dmg_coef * car->aero.rot_lateral[1];
car->aero.Mz += car->aero.lateral_drag * dmg_coef * car->aero.rot_lateral[2];
v2 = car->DynGC.vel.z;
car->aero.vertical_drag = -SIGN(v2)*v2*v2*1.5;
car->aero.Mx += car->aero.vertical_drag * dmg_coef * car->aero.rot_vertical[0];
car->aero.My += car->aero.vertical_drag * dmg_coef * car->aero.rot_vertical[1];
car->aero.Mz += car->aero.vertical_drag * dmg_coef * car->aero.rot_vertical[2];
}
// Steer filter for collision avoidance.
float Driver::filterSColl(float steer)
{
int i;
float sidedist = 0.0f, fsidedist = 0.0f, minsidedist = FLT_MAX;
Opponent *o = NULL;
// Get the index of the nearest car (o).
for (i = 0; i < opponents->getNOpponents(); i++) {
if (opponent[i].getState() & OPP_SIDE) {
sidedist = opponent[i].getSideDist();
fsidedist = fabs(sidedist);
if (fsidedist < minsidedist) {
minsidedist = fsidedist;
o = &opponent[i];
}
}
}
// If there is another car handle the situation.
if (o != NULL) {
float d = fsidedist - o->getWidth();
// Near, so we need to look at it.
if (d < SIDECOLL_MARGIN) {
/* compute angle between cars */
tCarElt *ocar = o->getCarPtr();
float diffangle = ocar->_yaw - car->_yaw;
NORM_PI_PI(diffangle);
// We are near and heading toward the car.
if (diffangle*o->getSideDist() < 0.0f) {
const float c = SIDECOLL_MARGIN/2.0f;
d = d - c;
if (d < 0.0f) {
d = 0.0f;
}
// Steer delta required to drive parallel to the opponent.
float psteer = diffangle/car->_steerLock;
myoffset = car->_trkPos.toMiddle;
// Limit myoffset to suitable limits.
float w = o->getCarPtr()->_trkPos.seg->width/WIDTHDIV-BORDER_OVERTAKE_MARGIN;
if (fabs(myoffset) > w) {
myoffset = (myoffset > 0.0f) ? w : -w;
}
// On straights the car near to the middle can correct more, in turns the car inside
// the turn does (because if you leave the track on the turn "inside" you will skid
// back to the track.
if (car->_trkPos.seg->type == TR_STR) {
if (fabs(car->_trkPos.toMiddle) > fabs(ocar->_trkPos.toMiddle)) {
// Its me, I do correct not that much.
psteer = steer*(d/c) + 1.5f*psteer*(1.0f - d/c);
} else {
// Its the opponent, so I correct more.
psteer = steer*(d/c) + 2.0f*psteer*(1.0f - d/c);
}
} else {
// Who is outside, heavy corrections are less dangerous
// if you drive near the middle of the track.
float outside = car->_trkPos.toMiddle - ocar->_trkPos.toMiddle;
float sign = (car->_trkPos.seg->type == TR_RGT) ? 1.0f : -1.0f;
if (outside*sign > 0.0f) {
psteer = steer*(d/c) + 1.5f*psteer*(1.0f - d/c);
} else {
psteer = steer*(d/c) + 2.0f*psteer*(1.0f - d/c);
}
}
if (psteer*steer > 0.0f && fabs(steer) > fabs(psteer)) {
return steer;
} else {
return psteer;
}
}
}
}
return steer;
}
void Opponent::UpdateSit(const CarElt* myCar, const TeamInfo* pTeamInfo, double myDirX, double myDirY)
{
CarElt* oCar = m_path.GetCar();
if( (oCar->_state & RM_CAR_STATE_NO_SIMU) )
return;
// word out speed of car.
m_info.sit.spd = hypot(oCar->_speed_X, oCar->_speed_Y);
// work out track relative speed of other car.
Vec2d norm = m_path.GetTrack()->CalcNormal(oCar->_distFromStartLine);
m_info.sit.tVX = norm.x * oCar->_speed_Y - norm.y * oCar->_speed_X;
m_info.sit.tVY = norm.x * oCar->_speed_X + norm.y * oCar->_speed_Y;
// work out track relative yaw of other car.
m_info.sit.tYaw = oCar->_yaw - Utils::VecAngle(norm) - PI * 0.5;
NORM_PI_PI(m_info.sit.tYaw);
// work out avg velocity of other car.
m_info.sit.agVX = m_info.sit.agVX * 0.75 + oCar->pub.DynGCg.vel.x * 0.25;
m_info.sit.agVY = m_info.sit.agVY * 0.75 + oCar->pub.DynGCg.vel.y * 0.25;
m_info.sit.ragVX = myDirX * m_info.sit.agVX + myDirY * m_info.sit.agVY;
m_info.sit.ragVY = myDirY * m_info.sit.agVX - myDirX * m_info.sit.agVY;
// work out avg acceleration of other car.
m_info.sit.agAX = m_info.sit.agAX * 0.75 + oCar->pub.DynGCg.acc.x * 0.25;
m_info.sit.agAY = m_info.sit.agAY * 0.75 + oCar->pub.DynGCg.acc.y * 0.25;
m_info.sit.ragAX = myDirX * m_info.sit.agAX + myDirY * m_info.sit.agAY;
m_info.sit.ragAY = myDirY * m_info.sit.agAX - myDirX * m_info.sit.agAY;
// work out lateral accelerations.
double rAX = myDirX * oCar->pub.DynGCg.acc.x + myDirY * oCar->pub.DynGCg.acc.y;
double rAY = myDirY * oCar->pub.DynGCg.acc.x - myDirX * oCar->pub.DynGCg.acc.y;
m_info.sit.arAX = m_info.sit.arAX * 0.75 + rAX * 0.25;
m_info.sit.arAY = m_info.sit.arAY * 0.75 + rAY * 0.25;
// offset from track centre line.
m_info.sit.offs = -oCar->_trkPos.toMiddle;
// the rest of the calcs are car-car relative, and make no sense if both
// cars are the same.
if( oCar == myCar )
return;
// calc other cars position, velocity relative to my car (global coords).
double dPX = oCar->pub.DynGCg.pos.x - myCar->pub.DynGCg.pos.x;
double dPY = oCar->pub.DynGCg.pos.y - myCar->pub.DynGCg.pos.y;
double dVX = oCar->_speed_X - myCar->_speed_X;
double dVY = oCar->_speed_Y - myCar->_speed_Y;
// work out relative position, velocity in local coords (coords of my car).
double rdPX = myDirX * dPX + myDirY * dPY;
double rdPY = myDirY * dPX - myDirX * dPY;
double rdVX = myDirX * dVX + myDirY * dVY;
double rdVY = myDirY * dVX - myDirX * dVY;
m_info.sit.rdPX = rdPX;
m_info.sit.rdPY = rdPY;
m_info.sit.rdVX = rdVX;
m_info.sit.rdVY = rdVY;
m_info.sit.minDX = (myCar->_dimension_x + oCar->_dimension_x) / 2;
m_info.sit.minDY = (myCar->_dimension_y + oCar->_dimension_y) / 2;
double myVelAng = atan2(myCar->_speed_Y, myCar->_speed_X);
double myYaw = myCar->_yaw - myVelAng;
NORM_PI_PI(myYaw);
double oYaw = oCar->_yaw - myVelAng;
NORM_PI_PI(oYaw);
double extSide = (m_info.sit.minDX - m_info.sit.minDY) *
(fabs(sin(myYaw)) + fabs(sin(oYaw)));
// m_info.sit.minDY += MX(0, MN((rdPX - m_info.sit.minDY) * 0.1, 4));
m_info.sit.minDY += extSide + 1;//0.5;
m_info.sit.minDX += 1.0;//pTeamInfo->IsTeamMate(myCar, oCar) ? 0.5 : 1.0;
// if( fabs(extSide) > 0.2 )
// GfOut( "****** angDiff %.3f extSide %.2f ******\n", angDiff, extSide );
// work out positions of car from start of track.
double myPos = RtGetDistFromStart((tCarElt*)myCar);
double hisPos = RtGetDistFromStart((tCarElt*)oCar);
double relPos = hisPos - myPos;
double trackLen = m_path.GetTrack()->GetLength();
if( relPos > trackLen / 2 )
relPos -= trackLen;
else if( relPos < -trackLen / 2 )
relPos += trackLen;
m_info.sit.relPos = relPos;
}
// Controls the car.
static void drive(int index, tCarElt* car, tSituation *situation)
{
tdble angle;
tdble brake;
tdble b1; // Brake value in case we are to fast HERE and NOW.
tdble b2; // Brake value for some brake point in front of us.
tdble b3; // Brake value for control (avoid loosing control).
tdble b4; // Brake value for avoiding high angle of attack.
tdble b5; // Brake for the pit;
tdble steer, targetAngle, shiftaccel;
MyCar* myc = mycar[index-1];
Pathfinder* mpf = myc->getPathfinderPtr();
b1 = b2 = b3 = b4 = b5 = 0.0;
shiftaccel = 0.0;
// Update some values needed.
myc->update(myTrackDesc, car, situation);
// Decide how we want to drive, choose a behaviour.
if ( car->_dammage < myc->undamaged/3 && myc->bmode != myc->NORMAL) {
myc->loadBehaviour(myc->NORMAL);
} else if (car->_dammage > myc->undamaged/3 && car->_dammage < (myc->undamaged*2)/3 && myc->bmode != myc->CAREFUL) {
myc->loadBehaviour(myc->CAREFUL);
} else if (car->_dammage > (myc->undamaged*2)/3 && myc->bmode != myc->SLOW) {
myc->loadBehaviour(myc->SLOW);
}
// Update the other cars just once.
if (currenttime != situation->currentTime) {
currenttime = situation->currentTime;
for (int i = 0; i < situation->_ncars; i++) ocar[i].update();
}
// Startmode.
if (myc->trtime < 5.0 && myc->bmode != myc->START) {
myc->loadBehaviour(myc->START);
myc->startmode = true;
}
if (myc->startmode && myc->trtime > 5.0) {
myc->startmode = false;
myc->loadBehaviour(myc->NORMAL);
}
// Compute path according to the situation.
mpf->plan(myc->getCurrentSegId(), car, situation, myc, ocar);
// Clear ctrl structure with zeros and set the current gear.
memset(&car->ctrl, 0, sizeof(tCarCtrl));
car->_gearCmd = car->_gear;
// Uncommenting the following line causes pitstop on every lap.
//if (!mpf->getPitStop()) mpf->setPitStop(true, myc->getCurrentSegId());
// Compute fuel consumption.
if (myc->getCurrentSegId() >= 0 && myc->getCurrentSegId() < 5 && !myc->fuelchecked) {
if (car->race.laps > 0) {
myc->fuelperlap = MAX(myc->fuelperlap, (myc->lastfuel+myc->lastpitfuel-car->priv.fuel));
}
myc->lastfuel = car->priv.fuel;
myc->lastpitfuel = 0.0;
myc->fuelchecked = true;
} else if (myc->getCurrentSegId() > 5) {
myc->fuelchecked = false;
}
// Decide if we need a pit stop.
if (!mpf->getPitStop() && (car->_remainingLaps-car->_lapsBehindLeader) > 0 && (car->_dammage > myc->MAXDAMMAGE ||
(car->priv.fuel < (myc->fuelperlap*(1.0+myc->FUEL_SAFETY_MARGIN)) &&
car->priv.fuel < (car->_remainingLaps-car->_lapsBehindLeader)*myc->fuelperlap)))
{
mpf->setPitStop(true, myc->getCurrentSegId());
}
if (mpf->getPitStop()) {
car->_raceCmd = RM_CMD_PIT_ASKED;
}
// Steer toward the next target point.
targetAngle = atan2(myc->dynpath->getLoc(myc->destpathsegid)->y - car->_pos_Y, myc->dynpath->getLoc(myc->destpathsegid)->x - car->_pos_X);
targetAngle -= car->_yaw;
NORM_PI_PI(targetAngle);
steer = targetAngle / car->_steerLock;
// Steer P (proportional) controller. We add a steer correction proportional to the distance error
// to the path.
// Steer angle has usual meaning, therefore + is to left (CCW) and - to right (CW).
// derror sign is + to right and - to left.
if (!mpf->getPitStop()) {
steer = steer + MIN(myc->STEER_P_CONTROLLER_MAX, myc->derror*myc->STEER_P_CONTROLLER_GAIN)*myc->getErrorSgn();
if (fabs(steer) > 1.0) {
steer/=fabs(steer);
}
} else {
// Check if we are almost in the pit to set brake to the max to avoid overrun.
tdble dl, dw;
RtDistToPit(car, myTrackDesc->getTorcsTrack(), &dl, &dw);
if (dl < 1.0f) {
b5 = 1.0f;
}
}
// Try to control angular velocity with a D (differential) controller.
double omega = myc->getSpeed()/myc->dynpath->getRadius(myc->currentpathsegid);
steer += myc->STEER_D_CONTROLLER_GAIN*(omega - myc->getCarPtr()->_yaw_rate);
// Brakes.
tdble brakecoeff = 1.0/(2.0*g*myc->currentseg->getKfriction()*myc->CFRICTION);
tdble brakespeed, brakedist;
tdble lookahead = 0.0;
int i = myc->getCurrentSegId();
brake = 0.0;
while (lookahead < brakecoeff * myc->getSpeedSqr()) {
lookahead += myc->dynpath->getLength(i);
brakespeed = myc->getSpeedSqr() - myc->dynpath->getSpeedsqr(i);
if (brakespeed > 0.0) {
tdble gm, qb, qs;
gm = myTrackDesc->getSegmentPtr(myc->getCurrentSegId())->getKfriction()*myc->CFRICTION*myTrackDesc->getSegmentPtr(myc->getCurrentSegId())->getKalpha();
qs = myc->dynpath->getSpeedsqr(i);
brakedist = brakespeed*(myc->mass/(2.0*gm*g*myc->mass + qs*(gm*myc->ca + myc->cw)));
if (brakedist > lookahead - myc->getWheelTrack()) {
qb = brakespeed*brakecoeff/brakedist;
if (qb > b2) {
b2 = qb;
}
}
}
i = (i + 1 + mpf->getnPathSeg()) % mpf->getnPathSeg();
}
if (myc->getSpeedSqr() > myc->dynpath->getSpeedsqr(myc->currentpathsegid)) {
b1 = (myc->getSpeedSqr() - myc->dynpath->getSpeedsqr(myc->currentpathsegid)) / (myc->getSpeedSqr());
}
// Try to avoid flying.
if (myc->getDeltaPitch() > myc->MAXALLOWEDPITCH && myc->getSpeed() > myc->FLYSPEED) {
b4 = 1.0;
}
// Check if we are on the way.
if (myc->getSpeed() > myc->TURNSPEED && myc->tr_mode == 0) {
if (myc->derror > myc->PATHERR) {
vec2d *cd = myc->getDir();
vec2d *pd = myc->dynpath->getDir(myc->currentpathsegid);
float z = cd->x*pd->y - cd->y*pd->x;
// If the car points away from the path brake.
if (z*myc->getErrorSgn() >= 0.0) {
targetAngle = atan2(myc->dynpath->getDir(myc->currentpathsegid)->y, myc->dynpath->getDir(myc->currentpathsegid)->x);
targetAngle -= car->_yaw;
NORM_PI_PI(targetAngle);
double toborder = MAX(1.0, myc->currentseg->getWidth()/2.0 - fabs(myTrackDesc->distToMiddle(myc->getCurrentSegId(), myc->getCurrentPos())));
b3 = (myc->getSpeed()/myc->STABLESPEED)*(myc->derror-myc->PATHERR)/toborder;
}
}
}
// Anti wheel locking and brake code.
if (b1 > b2) brake = b1; else brake = b2;
if (brake < b3) brake = b3;
if (brake < b4) {
brake = MIN(1.0, b4);
tdble abs_mean;
abs_mean = (car->_wheelSpinVel(REAR_LFT) + car->_wheelSpinVel(REAR_RGT))*car->_wheelRadius(REAR_LFT)/myc->getSpeed();
abs_mean /= 2.0;
brake = brake * abs_mean;
} else {
brake = MIN(1.0, brake);
tdble abs_min = 1.0;
for (int i = 0; i < 4; i++) {
tdble slip = car->_wheelSpinVel(i) * car->_wheelRadius(i) / myc->getSpeed();
if (slip < abs_min) abs_min = slip;
}
brake = brake * abs_min;
}
// Reduce brake value to the approximate normal force available on the wheels.
float weight = myc->mass*G;
float maxForce = weight + myc->ca*myc->MAX_SPEED*myc->MAX_SPEED;
float force = weight + myc->ca*myc->getSpeedSqr();
brake = brake*MIN(1.0, force/maxForce);
if (b5 > 0.0f) {
brake = b5;
}
// Gear changing.
if (myc->tr_mode == 0) {
if (car->_gear <= 0) {
car->_gearCmd = 1;
} else {
float gr_up = car->_gearRatio[car->_gear + car->_gearOffset];
float omega = car->_enginerpmRedLine/gr_up;
float wr = car->_wheelRadius(2);
if (omega*wr*myc->SHIFT < car->_speed_x) {
car->_gearCmd++;
} else {
float gr_down = car->_gearRatio[car->_gear + car->_gearOffset - 1];
omega = car->_enginerpmRedLine/gr_down;
if (car->_gear > 1 && omega*wr*myc->SHIFT > car->_speed_x + myc->SHIFT_MARGIN) {
car->_gearCmd--;
}
}
}
}
// Acceleration / brake execution.
tdble cerror, cerrorh;
cerrorh = sqrt(car->_speed_x*car->_speed_x + car->_speed_y*car->_speed_y);
if (cerrorh > myc->TURNSPEED) {
cerror = fabs(car->_speed_x)/cerrorh;
} else {
cerror = 1.0;
}
if (myc->tr_mode == 0) {
if (brake > 0.0) {
myc->accel = 0.0;
car->_accelCmd = myc->accel;
car->_brakeCmd = brake*cerror;
} else {
if (myc->getSpeedSqr() < myc->dynpath->getSpeedsqr(myc->getCurrentSegId())) {
if (myc->accel < myc->ACCELLIMIT) {
myc->accel += myc->ACCELINC;
}
car->_accelCmd = myc->accel/cerror;
} else {
if (myc->accel > 0.0) {
myc->accel -= myc->ACCELINC;
}
// TODO: shiftaccel always 0 at the moment...
car->_accelCmd = myc->accel = MIN(myc->accel/cerror, shiftaccel/cerror);
}
tdble slipspeed = myc->querySlipSpeed(car);
if (slipspeed > myc->TCL_SLIP) {
car->_accelCmd = car->_accelCmd - MIN(car->_accelCmd, (slipspeed - myc->TCL_SLIP)/myc->TCL_RANGE);
}
}
}
// Check if we are stuck, try to get unstuck.
tdble bx = myc->getDir()->x, by = myc->getDir()->y;
tdble cx = myc->currentseg->getMiddle()->x - car->_pos_X, cy = myc->currentseg->getMiddle()->y - car->_pos_Y;
tdble parallel = (cx*bx + cy*by) / (sqrt(cx*cx + cy*cy)*sqrt(bx*bx + by*by));
if ((myc->getSpeed() < myc->TURNSPEED) && (parallel < cos(90.0*PI/180.0)) && (mpf->dist2D(myc->getCurrentPos(), myc->dynpath->getLoc(myc->getCurrentSegId())) > myc->TURNTOL)) {
myc->turnaround += situation->deltaTime;
} else myc->turnaround = 0.0;
if ((myc->turnaround >= waitToTurn) || (myc->tr_mode >= 1)) {
if (myc->tr_mode == 0) {
myc->tr_mode = 1;
}
if ((car->_gearCmd > -1) && (myc->tr_mode < 2)) {
car->_accelCmd = 0.0;
if (myc->tr_mode == 1) {
car->_gearCmd--;
}
car->_brakeCmd = 1.0;
} else {
myc->tr_mode = 2;
if (parallel < cos(90.0*PI/180.0) && (mpf->dist2D(myc->getCurrentPos(), myc->dynpath->getLoc(myc->getCurrentSegId())) > myc->TURNTOL)) {
angle = queryAngleToTrack(car);
car->_steerCmd = ( -angle > 0.0) ? 1.0 : -1.0;
car->_brakeCmd = 0.0;
if (myc->accel < 1.0) {
myc->accel += myc->ACCELINC;
}
car->_accelCmd = myc->accel;
tdble slipspeed = myc->querySlipSpeed(car);
if (slipspeed < -myc->TCL_SLIP) {
car->_accelCmd = car->_accelCmd - MIN(car->_accelCmd, (myc->TCL_SLIP - slipspeed)/myc->TCL_RANGE);
}
} else {
if (myc->getSpeed() < 1.0) {
myc->turnaround = 0;
myc->tr_mode = 0;
myc->loadBehaviour(myc->START);
myc->startmode = true;
myc->trtime = 0.0;
}
car->_brakeCmd = 1.0;
car->_steerCmd = 0.0;
car->_accelCmd = 0.0;
}
}
}
if (myc->tr_mode == 0) car->_steerCmd = steer;
car->_clutchCmd = getClutch(myc, car);
}
void SimAeroUpdate(tCar *car, tSituation *s)
{
tdble hm;
int i;
tCar *otherCar;
tdble x, y;
tdble yaw, otherYaw, airSpeed, tmpas, spdang, tmpsdpang, dyaw;
tdble dragK = 1.0;
x = car->DynGCg.pos.x;
y = car->DynGCg.pos.y;
yaw = car->DynGCg.pos.az;
airSpeed = car->DynGC.vel.x;
spdang = atan2(car->DynGCg.vel.y, car->DynGCg.vel.x);
if (airSpeed > 10.0f) {
for (i = 0; i < s->_ncars; i++) {
if (i == car->carElt->index) {
// skip myself
continue;
}
otherCar = &(SimCarTable[i]);
otherYaw = otherCar->DynGCg.pos.az;
tmpsdpang = spdang - atan2(y - otherCar->DynGCg.pos.y, x - otherCar->DynGCg.pos.x);
NORM_PI_PI(tmpsdpang);
dyaw = yaw - otherYaw;
NORM_PI_PI(dyaw);
if ((otherCar->DynGC.vel.x > 10.0f) && (fabs(dyaw) < 0.1396f)) {
if (fabs(tmpsdpang) > 2.9671f) { /* 10 degrees */
// behind another car
tmpas = 1.0f - exp(- 2.0f * DIST(x, y, otherCar->DynGCg.pos.x, otherCar->DynGCg.pos.y) /
(otherCar->aero.Cd * otherCar->DynGC.vel.x));
if (tmpas < dragK) {
dragK = tmpas;
}
} else if (fabs(tmpsdpang) < 0.1396f) { /* 8 degrees */
// before another car, lower drag by maximum 15% (this is just another guess)
tmpas = 1.0f - 0.15f * exp(- 8.0f * DIST(x, y, otherCar->DynGCg.pos.x, otherCar->DynGCg.pos.y) / (car->aero.Cd * car->DynGC.vel.x));
if (tmpas < dragK) {
dragK = tmpas;
}
}
}
}
}
car->airSpeed2 = airSpeed * airSpeed;
tdble v2 = car->airSpeed2;
// simulate ground effect drop off caused by non-frontal airflow (diffusor stops working etc.)
tdble cosa = 1.0f;
if (car->speed > 1.0f) {
cosa = car->DynGC.vel.x/car->speed;
}
if (cosa < 0.0f) {
cosa = 0.0f;
}
car->aero.drag = -SIGN(car->DynGC.vel.x) * car->aero.SCx2 * v2 * (1.0f + (tdble)car->dammage / 10000.0f) * dragK * dragK;
hm = 1.5f * (car->wheel[0].rideHeight + car->wheel[1].rideHeight + car->wheel[2].rideHeight + car->wheel[3].rideHeight);
hm = hm*hm;
hm = hm*hm;
hm = 2.0f * exp(-3.0f*hm);
car->aero.lift[0] = - car->aero.Clift[0] * v2 * hm * cosa;
car->aero.lift[1] = - car->aero.Clift[1] * v2 * hm * cosa;
}
/* controls the car */
static void drive(int index, tCarElt* car, tSituation *situation)
{
tdble angle;
tdble brake;
tdble b1; /* brake value in case we are to fast HERE and NOW */
tdble b2; /* brake value for some brake point in front of us */
tdble b3; /* brake value for control (avoid loosing control) */
tdble b4; /* brake value for avoiding high angle of attack */
tdble steer, targetAngle, shiftaccel;
MyCar* myc = mycar[index-1];
Pathfinder* mpf = myc->getPathfinderPtr();
b1 = b2 = b3 = b4 = 0.0;
shiftaccel = 0.0;
/* update some values needed */
myc->update(myTrackDesc, car, situation);
/* decide how we want to drive */
if ( car->_dammage < myc->undamaged/3 && myc->bmode != myc->NORMAL) {
myc->loadBehaviour(myc->NORMAL);
} else if (car->_dammage > myc->undamaged/3 && car->_dammage < (myc->undamaged*2)/3 && myc->bmode != myc->CAREFUL) {
myc->loadBehaviour(myc->CAREFUL);
} else if (car->_dammage > (myc->undamaged*2)/3 && myc->bmode != myc->SLOW) {
myc->loadBehaviour(myc->SLOW);
}
/* update the other cars just once */
if (currenttime != situation->currentTime) {
currenttime = situation->currentTime;
for (int i = 0; i < situation->_ncars; i++) ocar[i].update();
}
/* startmode */
if (myc->trtime < 5.0 && myc->bmode != myc->START) {
myc->loadBehaviour(myc->START);
myc->startmode = true;
}
if (myc->startmode && myc->trtime > 5.0) {
myc->startmode = false;
myc->loadBehaviour(myc->NORMAL);
}
/* compute path according to the situation */
mpf->plan(myc->getCurrentSegId(), car, situation, myc, ocar);
/* clear ctrl structure with zeros and set the current gear */
memset(&car->ctrl, 0, sizeof(tCarCtrl));
car->_gearCmd = car->_gear;
/* uncommenting the following line causes pitstop on every lap */
//if (!mpf->getPitStop()) mpf->setPitStop(true, myc->getCurrentSegId());
/* compute fuel consumption */
if (myc->getCurrentSegId() >= 0 && myc->getCurrentSegId() < 5 && !myc->fuelchecked) {
if (car->race.laps > 0) {
myc->fuelperlap = MAX(myc->fuelperlap, (myc->lastfuel+myc->lastpitfuel-car->priv.fuel));
}
myc->lastfuel = car->priv.fuel;
myc->lastpitfuel = 0.0;
myc->fuelchecked = true;
} else if (myc->getCurrentSegId() > 5) {
myc->fuelchecked = false;
}
/* decide if we need a pit stop */
if (!mpf->getPitStop() && (car->_remainingLaps-car->_lapsBehindLeader) > 0 && (car->_dammage > myc->MAXDAMMAGE ||
(car->priv.fuel < 1.5*myc->fuelperlap &&
car->priv.fuel < (car->_remainingLaps-car->_lapsBehindLeader)*myc->fuelperlap)))
{
mpf->setPitStop(true, myc->getCurrentSegId());
}
if (mpf->getPitStop()) {
car->_raceCmd = RM_CMD_PIT_ASKED;
}
/* steer to next target point */
targetAngle = atan2(myc->destpathseg->getLoc()->y - car->_pos_Y, myc->destpathseg->getLoc()->x - car->_pos_X);
targetAngle -= car->_yaw;
NORM_PI_PI(targetAngle);
steer = targetAngle / car->_steerLock;
/* brakes */
tdble brakecoeff = 1.0/(2.0*g*myc->currentseg->getKfriction()*myc->CFRICTION);
tdble brakespeed, brakedist;
tdble lookahead = 0.0;
int i = myc->getCurrentSegId();
brake = 0.0;
while (lookahead < brakecoeff * myc->getSpeedSqr()) {
lookahead += mpf->getPathSeg(i)->getLength();
brakespeed = myc->getSpeedSqr() - mpf->getPathSeg(i)->getSpeedsqr();
if (brakespeed > 0.0) {
tdble gm, qb, qs;
gm = myTrackDesc->getSegmentPtr(myc->getCurrentSegId())->getKfriction()*myc->CFRICTION*myTrackDesc->getSegmentPtr(myc->getCurrentSegId())->getKalpha();
qs = mpf->getPathSeg(i)->getSpeedsqr();
brakedist = brakespeed*(myc->mass/(2.0*gm*g*myc->mass + qs*(gm*myc->ca + myc->cw)));
if (brakedist > lookahead - myc->getWheelTrack()) {
qb = brakespeed*brakecoeff/brakedist;
if (qb > b2) {
b2 = qb;
}
}
}
i = (i + 1 + mpf->getnPathSeg()) % mpf->getnPathSeg();
}
if (myc->getSpeedSqr() > myc->currentpathseg->getSpeedsqr()) {
b1 = (myc->getSpeedSqr() - myc->currentpathseg->getSpeedsqr()) / (myc->getSpeedSqr());
}
/* try to avoid flying */
if (myc->getDeltaPitch() > myc->MAXALLOWEDPITCH && myc->getSpeed() > myc->FLYSPEED) {
b4 = 1.0;
}
if (!mpf->getPitStop()) {
steer = steer + MIN(0.1, myc->derror*0.02)*myc->getErrorSgn();
if (fabs(steer) > 1.0) steer/=fabs(steer);
}
/* check if we are on the way */
if (myc->getSpeed() > myc->TURNSPEED && myc->tr_mode == 0) {
if (myc->derror > myc->PATHERR) {
v3d r;
myc->getDir()->crossProduct(myc->currentpathseg->getDir(), &r);
if (r.z*myc->getErrorSgn() >= 0.0) {
targetAngle = atan2(myc->currentpathseg->getDir()->y, myc->currentpathseg->getDir()->x);
targetAngle -= car->_yaw;
NORM_PI_PI(targetAngle);
double toborder = MAX(1.0, myc->currentseg->getWidth()/2.0 - fabs(myTrackDesc->distToMiddle(myc->getCurrentSegId(), myc->getCurrentPos())));
b3 = (myc->getSpeed()/myc->STABLESPEED)*(myc->derror-myc->PATHERR)/toborder;
}
}
}
/* try to control angular velocity */
double omega = myc->getSpeed()/myc->currentpathseg->getRadius();
steer += 0.1*(omega - myc->getCarPtr()->_yaw_rate);
/* anti blocking and brake code */
if (b1 > b2) brake = b1; else brake = b2;
if (brake < b3) brake = b3;
if (brake < b4) {
brake = MIN(1.0, b4);
tdble abs_mean;
abs_mean = (car->_wheelSpinVel(REAR_LFT) + car->_wheelSpinVel(REAR_RGT))*car->_wheelRadius(REAR_LFT)/myc->getSpeed();
abs_mean /= 2.0;
brake = brake * abs_mean;
} else {
brake = MIN(1.0, brake);
tdble abs_min = 1.0;
for (int i = 0; i < 4; i++) {
tdble slip = car->_wheelSpinVel(i) * car->_wheelRadius(i) / myc->getSpeed();
if (slip < abs_min) abs_min = slip;
}
brake = brake * abs_min;
}
float weight = myc->mass*G;
float maxForce = weight + myc->ca*myc->MAX_SPEED*myc->MAX_SPEED;
float force = weight + myc->ca*myc->getSpeedSqr();
brake = brake*MIN(1.0, force/maxForce);
// Gear changing.
if (myc->tr_mode == 0) {
if (car->_gear <= 0) {
car->_gearCmd = 1;
} else {
float gr_up = car->_gearRatio[car->_gear + car->_gearOffset];
float omega = car->_enginerpmRedLine/gr_up;
float wr = car->_wheelRadius(2);
if (omega*wr*myc->SHIFT < car->_speed_x) {
car->_gearCmd++;
} else {
float gr_down = car->_gearRatio[car->_gear + car->_gearOffset - 1];
omega = car->_enginerpmRedLine/gr_down;
if (car->_gear > 1 && omega*wr*myc->SHIFT > car->_speed_x + myc->SHIFT_MARGIN) {
car->_gearCmd--;
}
}
}
}
tdble cerror, cerrorh;
cerrorh = sqrt(car->_speed_x*car->_speed_x + car->_speed_y*car->_speed_y);
if (cerrorh > myc->TURNSPEED) cerror = fabs(car->_speed_x)/cerrorh; else cerror = 1.0;
/* acceleration / brake execution */
if (myc->tr_mode == 0) {
if (brake > 0.0) {
myc->accel = 0.0;
car->_accelCmd = myc->accel;
car->_brakeCmd = brake*cerror;
} else {
if (myc->getSpeedSqr() < mpf->getPathSeg(myc->getCurrentSegId())->getSpeedsqr()) {
if (myc->accel < myc->ACCELLIMIT) {
myc->accel += myc->ACCELINC;
}
car->_accelCmd = myc->accel/cerror;
} else {
if (myc->accel > 0.0) {
myc->accel -= myc->ACCELINC;
}
car->_accelCmd = myc->accel = MIN(myc->accel/cerror, shiftaccel/cerror);
}
tdble slipspeed = myc->querySlipSpeed(car);
if (slipspeed > myc->TCL_SLIP) {
car->_accelCmd = car->_accelCmd - MIN(car->_accelCmd, (slipspeed - myc->TCL_SLIP)/myc->TCL_RANGE);
}
}
}
/* check if we are stuck, try to get unstuck */
tdble bx = myc->getDir()->x, by = myc->getDir()->y;
tdble cx = myc->currentseg->getMiddle()->x - car->_pos_X, cy = myc->currentseg->getMiddle()->y - car->_pos_Y;
tdble parallel = (cx*bx + cy*by) / (sqrt(cx*cx + cy*cy)*sqrt(bx*bx + by*by));
if ((myc->getSpeed() < myc->TURNSPEED) && (parallel < cos(90.0*PI/180.0)) && (mpf->dist2D(myc->getCurrentPos(), mpf->getPathSeg(myc->getCurrentSegId())->getLoc()) > myc->TURNTOL)) {
myc->turnaround += situation->deltaTime;
} else myc->turnaround = 0.0;
if ((myc->turnaround >= waitToTurn) || (myc->tr_mode >= 1)) {
if (myc->tr_mode == 0) {
myc->tr_mode = 1;
}
if ((car->_gearCmd > -1) && (myc->tr_mode < 2)) {
car->_accelCmd = 0.0;
if (myc->tr_mode == 1) {
car->_gearCmd--;
}
car->_brakeCmd = 1.0;
} else {
myc->tr_mode = 2;
if (parallel < cos(90.0*PI/180.0) && (mpf->dist2D(myc->getCurrentPos(), mpf->getPathSeg(myc->getCurrentSegId())->getLoc()) > myc->TURNTOL)) {
angle = queryAngleToTrack(car);
car->_steerCmd = ( -angle > 0.0) ? 1.0 : -1.0;
car->_brakeCmd = 0.0;
if (myc->accel < 1.0) {
myc->accel += myc->ACCELINC;
}
car->_accelCmd = myc->accel;
tdble slipspeed = myc->querySlipSpeed(car);
if (slipspeed < -myc->TCL_SLIP) {
car->_accelCmd = car->_accelCmd - MIN(car->_accelCmd, (myc->TCL_SLIP - slipspeed)/myc->TCL_RANGE);
}
} else {
if (myc->getSpeed() < 1.0) {
myc->turnaround = 0;
myc->tr_mode = 0;
myc->loadBehaviour(myc->START);
myc->startmode = true;
myc->trtime = 0.0;
}
car->_brakeCmd = 1.0;
car->_steerCmd = 0.0;
car->_accelCmd = 0.0;
}
}
}
if (myc->tr_mode == 0) car->_steerCmd = steer;
timeSinceLastUpdate[index - 1] += situation->deltaTime;
//Only update once per second
if(timeSinceLastUpdate[index - 1] >= 0.1)
{
/* steer to next target point */
float targetAngleOut = atan2(myc->destpathseg->getLoc()->y - car->_pos_Y, myc->destpathseg->getLoc()->x - car->_pos_X);
targetAngleOut -= car->_yaw;
NORM_PI_PI(targetAngleOut);
timeSinceLastUpdate[index - 1] = 0.0;
sensors[index - 1]->sensors_update();
//Write training data
outputFiles[index - 1]<<"DATA"<<std::endl;
//INPUT
outputFiles[index - 1]<<"speed "<<myc->getSpeed()<<std::endl;
outputFiles[index - 1]<<"angle "<<myc->getDeltaPitch()<<std::endl;
outputFiles[index - 1]<<"targetAngle "<<targetAngleOut<<std::endl;
//Distance sensors
outputFiles[index - 1]<<"distR "<<sensors[index - 1]->getSensorOut(0)<<std::endl;
outputFiles[index - 1]<<"distFR "<<sensors[index - 1]->getSensorOut(1)<<std::endl;
outputFiles[index - 1]<<"distFFR "<<sensors[index - 1]->getSensorOut(2)<<std::endl;
outputFiles[index - 1]<<"distF "<<sensors[index - 1]->getSensorOut(3)<<std::endl;
outputFiles[index - 1]<<"distFFL "<<sensors[index - 1]->getSensorOut(4)<<std::endl;
outputFiles[index - 1]<<"distFL "<<sensors[index - 1]->getSensorOut(5)<<std::endl;
outputFiles[index - 1]<<"distL "<<sensors[index - 1]->getSensorOut(6)<<std::endl;
//OUTPUT
outputFiles[index - 1]<<"steer "<<car->ctrl.steer<<std::endl;
outputFiles[index - 1]<<"accel "<<car->ctrl.accelCmd<<std::endl;
outputFiles[index - 1]<<"brake "<<car->ctrl.brakeCmd<<std::endl;
outputFiles[index - 1]<<"gear "<<car->ctrl.gear<<std::endl;
outputFiles[index - 1]<<"clutch "<<car->ctrl.clutchCmd<<std::endl;
//Write training data to console
printf_s("-----------------------------\n");
printf_s("Speed: %f\n", myc->getSpeed());
printf_s("Steering: %f\n", car->ctrl.steer);
printf_s("Acceleration: %f\n", car->ctrl.accelCmd);
printf_s("Brake: %f\n", car->ctrl.brakeCmd);
printf_s("Gear: %f\n", car->ctrl.gear);
printf_s("Clutch: %f\n", car->ctrl.clutchCmd);
printf_s("Angle: %f\n", myc->getDeltaPitch());
printf_s("Target Angle: %f\n\n", targetAngleOut);
printf_s("distR %f\n", sensors[index - 1]->getSensorOut(0));
printf_s("distFR %f\n", sensors[index - 1]->getSensorOut(1));
printf_s("distFFR %f\n", sensors[index - 1]->getSensorOut(2));
printf_s("distF %f\n", sensors[index - 1]->getSensorOut(3));
printf_s("distFFL %f\n", sensors[index - 1]->getSensorOut(4));
printf_s("distFL %f\n", sensors[index - 1]->getSensorOut(5));
printf_s("distL %f\n", sensors[index - 1]->getSensorOut(6));
}
}
void SimWheelUpdateForce(tCar *car, int index)
{
tWheel *wheel = &(car->wheel[index]);
tdble axleFz = wheel->axleFz;
tdble vt, v, v2, wrl; // wheel related velocity
tdble Fn, Ft;
tdble waz;
tdble CosA, SinA;
tdble s, sa, sx, sy; // slip vector
tdble stmp, F, Bx;
tdble mu;
tdble reaction_force = 0.0f;
wheel->state = 0;
// VERTICAL STUFF CONSIDERING SMALL PITCH AND ROLL ANGLES
// update suspension force
SimSuspUpdate(&(wheel->susp));
// check suspension state
wheel->state |= wheel->susp.state;
if ((wheel->state & SIM_SUSP_EXT) == 0) {
wheel->forces.z = axleFz + wheel->susp.force;
reaction_force = wheel->forces.z;
wheel->rel_vel -= SimDeltaTime * wheel->susp.force / wheel->mass;
if (wheel->forces.z < 0.0f) {
wheel->forces.z = 0.0f;
}
} 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;
}
// update wheel coord, center relative to GC
wheel->relPos.z = - wheel->susp.x / wheel->susp.spring.bellcrank + wheel->radius;
// HORIZONTAL FORCES
waz = wheel->steer + wheel->staticPos.az;
CosA = cos(waz);
SinA = sin(waz);
// tangent velocity.
vt = wheel->bodyVel.x * CosA + wheel->bodyVel.y * SinA;
v2 = wheel->bodyVel.x * wheel->bodyVel.x + wheel->bodyVel.y * wheel->bodyVel.y;
v = sqrt(v2);
// slip angle
if (v < 0.000001f) {
sa = 0.0f;
} else {
sa = atan2(wheel->bodyVel.y, wheel->bodyVel.x) - waz;
}
NORM_PI_PI(sa);
wrl = wheel->spinVel * wheel->radius;
if ((wheel->state & SIM_SUSP_EXT) != 0) {
sx = sy = 0.0f;
} else if (v < 0.000001f) {
sx = wrl;
sy = 0.0f;
} else {
sx = (vt - wrl) / fabs(vt);
sy = sin(sa);
}
Ft = 0.0f;
Fn = 0.0f;
s = sqrt(sx*sx+sy*sy);
{
// calculate _skid and _reaction for sound.
if (v < 2.0f) {
car->carElt->_skid[index] = 0.0f;
} else {
car->carElt->_skid[index] = MIN(1.0f, (s*reaction_force*0.0002f));
}
car->carElt->_reaction[index] = reaction_force;
}
stmp = MIN(s, 1.5f);
// MAGIC FORMULA
Bx = wheel->mfB * stmp;
F = sin(wheel->mfC * atan(Bx * (1.0f - wheel->mfE) + wheel->mfE * atan(Bx))) * (1.0f + stmp * simSkidFactor[car->carElt->_skillLevel]);
// load sensitivity
mu = wheel->mu * (wheel->lfMin + (wheel->lfMax - wheel->lfMin) * exp(wheel->lfK * wheel->forces.z / wheel->opLoad));
F *= wheel->forces.z * mu * wheel->trkPos.seg->surface->kFriction * (1.0f + 0.05f * sin(-wheel->staticPos.ax * 18.0f)); /* coeff */
wheel->rollRes = wheel->forces.z * wheel->trkPos.seg->surface->kRollRes;
car->carElt->priv.wheel[index].rollRes = wheel->rollRes;
if (s > 0.000001f) {
// wheel axis based
Ft -= F * sx / s;
Fn -= F * sy / s;
}
RELAXATION2(Fn, wheel->preFn, 50.0f);
RELAXATION2(Ft, wheel->preFt, 50.0f);
wheel->relPos.az = waz;
wheel->forces.x = Ft * CosA - Fn * SinA;
wheel->forces.y = Ft * SinA + Fn * CosA;
wheel->spinTq = Ft * wheel->radius;
wheel->sa = sa;
wheel->sx = sx;
wheel->feedBack.spinVel = wheel->spinVel;
wheel->feedBack.Tq = wheel->spinTq;
wheel->feedBack.brkTq = wheel->brake.Tq;
car->carElt->_wheelSlipSide(index) = sy*v;
car->carElt->_wheelSlipAccel(index) = sx*v;
car->carElt->_reaction[index] = reaction_force;
}
/* 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
}
}
void SegLearn::update(tSituation *s, tTrack *t, tCarElt *car, int alone, int avoiding, double outside, double *r)
{
// Still on the same segment, alone, offset near 0, check.
tTrackSeg *seg = car->_trkPos.seg;
tTrackSeg *tseg = seg;
if (seg->type == lastturn || seg->type == TR_STR)
{
if (check == true && alone > 0)
{
// + to left, - to right
double toleft = car->_trkPos.toLeft;
double dr = 0.0;
int inside_error = 0;
if (lastturn == TR_RGT) {
dr = toleft - MAX(MIN(outside, 1.5), outside - 1.5);
if (car->_trkPos.toRight < car->_dimension_y/4)
inside_error = 1;
} else if (lastturn == TR_LFT) {
dr = MIN(MAX(outside, seg->width-1.5), outside + 1.5) - toleft;
if (car->_trkPos.toLeft < car->_dimension_y/4)
inside_error = 1;
}
// decrease speed further if we're spinning out or hit a wall
int spindmg_error = 0;
double angle = RtTrackSideTgAngleL(&(car->_trkPos));
angle -= car->_yaw;
NORM_PI_PI(angle);
if (!avoiding)
{
if (fabs(angle) > 1.3 && fabs(car->_yaw_rate) > 1.5)
{
dr = MIN(0.0, dr) - (fabs(angle) + fabs(car->_yaw_rate))*2;
spindmg_error = 1;
}
else if (car->_dammage > last_dammage + 500)
{
dr = MIN(dr, -((double)(car->_dammage-last_dammage)/500.0));
spindmg_error = 1;
}
}
// this stops it 'learning' to over-accelerate though the first
// corner of a chicane.
if ((!avoiding && seg->radius <= 50.0 &&
(seg->type == TR_RGT && car->_trkPos.toRight < car->_dimension_y/4)) ||
(seg->type == TR_LFT && car->_trkPos.toLeft < car->_dimension_y/4))
{
inside_error = 1;
tTrackSeg *cs = seg->prev;
double len = 0.0;
while (cs->type == seg->type && len < 100.0)
{
len += cs->length;
cs = cs->prev;
}
if (cs->type != TR_STR && cs->type != seg->type && cs->radius < 400.0 && !last_inside_error)
{
int thisturn = cs->type;
double redux = (seg->type == TR_RGT ? (car->_dimension_y/2 - car->_trkPos.toRight) : (car->_dimension_y/2 - car->_trkPos.toLeft));
while (cs->type == thisturn)
{
if (radius[updateid[cs->id]] > 0.0 && learncount[updateid[cs->id]] < learnlimit)
{
minradius[updateid[cs->id]] = MIN(minradius[updateid[cs->id]], radius[updateid[cs->id]] - (redux/2));
radius[updateid[cs->id]] -= redux;
radius[updateid[cs->id]] = MIN(radius[updateid[cs->id]], 1000.0);
radius[updateid[cs->id]] = MAX(radius[updateid[cs->id]], -cs->radius+1.0);
learncount[updateid[cs->id]]++;
}
cs = cs->prev;
}
return;
}
else if (!last_inside_error)
inside_error = 0;
}
if (dr < rmin && !inside_error) {
rmin = dr;
}
if (rmin >= 0.0)
rmin = MAX(rmin, 0.1);
last_inside_error = inside_error;
} else {
check = false;
}
}
if (seg->type != prevtype || seg != lastseg) {
prevtype = seg->type;
if (lastseg != seg)
{
lastseg = seg;
}
lastrmin = rmin;
if (seg->type != TR_STR) {
if (check == true) {
tTrackSeg *cs = tseg->prev;
// Skip straights.
while (cs->type == TR_STR) {
cs = cs->prev;
}
while (cs->type == lastturn) {
if (rmin < 0.0)
{
rmin *= learnfactor;
rmin /= MAX(1, learncount[updateid[cs->id]]);
}
if (!avoiding && learncount[updateid[cs->id]] < learnlimit)
{
if (radius[updateid[cs->id]] + rmin < 0.0) {
rmin = MAX(cs->radius - r[cs->id], rmin);
}
double thisrmin = (rmin > 0.0 ? rmin : (minradius[updateid[cs->id]] < 10000.0 ? rmin * 0.5 : rmin * 1.1));
if (rmin < 0.0)
{
minradius[updateid[cs->id]] = MIN(minradius[updateid[cs->id]], radius[updateid[cs->id]] + rmin * 0.2);
radius[updateid[cs->id]] = (radius[updateid[cs->id]] + thisrmin) / 2;
}
else
radius[updateid[cs->id]] += thisrmin;
radius[updateid[cs->id]] = MIN(radius[updateid[cs->id]], 1000.0);
radius[updateid[cs->id]] = MIN(radius[updateid[cs->id]], minradius[updateid[cs->id]]);
avoidradius[updateid[cs->id]] = MIN(avoidradius[updateid[cs->id]], radius[updateid[cs->id]]);
avoidminradius[updateid[cs->id]] = MIN(avoidminradius[updateid[cs->id]], minradius[updateid[cs->id]]);
learncount[updateid[cs->id]]++;
}
else if (!avoiding)
{
if (avoidradius[updateid[cs->id]] + rmin < 0.0) {
rmin = MAX(cs->radius - r[cs->id], rmin);
}
double thisrmin = (rmin < 0.0 ? rmin : (avoidminradius[updateid[cs->id]] < 10000.0 ? rmin * 0.5 : rmin * 1.5));
if (rmin < 0.0)
{
avoidminradius[updateid[cs->id]] = MIN(avoidradius[updateid[cs->id]], avoidradius[updateid[cs->id]] + rmin * 0.2);
avoidradius[updateid[cs->id]] = (avoidradius[updateid[cs->id]]+thisrmin) / 2;
}
else
avoidradius[updateid[cs->id]] += thisrmin;
avoidradius[updateid[cs->id]] = MIN(avoidradius[updateid[cs->id]], 1000.0);
avoidradius[updateid[cs->id]] = MIN(avoidradius[updateid[cs->id]], avoidminradius[updateid[cs->id]]);
}
cs = cs->prev;
}
}
check = true;
rmin = MIN(seg->width/2.0, seg->radius/10.0);
lastturn = seg->type;
}
}
last_dammage = car->_dammage;
}
void
SimWingUpdate(tCar *car, int index, tSituation* s)
{
tWing *wing = &(car->wing[index]);
tdble vt2 = car->DynGC.vel.x;
tdble i_flow = 1.0;
// rear wing should not get any flow.
// compute angle of attack
// we don't add ay anymore since DynGC.vel.x,z are now in the correct frame of reference (see car.cpp)
tdble aoa = atan2(car->DynGC.vel.z, car->DynGC.vel.x); //+ car->DynGC.pos.ay;
// The flow to the rear wing can get cut off at large negative
// angles of attack. (so it won't produce lift because it will be
// completely shielded by the car's bottom)
// The value -0.4 should depend on the positioning of the wing.
// we also make this be like that.
if (index==1) {
i_flow = PartialFlowSmooth (-0.4, aoa);
}
// Flow to the wings gets cut off by other cars.
tdble airSpeed = car->DynGC.vel.x;
if (airSpeed > 10.0) {
tdble yaw = car->DynGC.pos.az;
tdble x = car->DynGC.pos.x + cos(yaw)*wing->staticPos.x;
tdble y = car->DynGC.pos.y + sin(yaw)*wing->staticPos.x;
tdble spdang = atan2(car->DynGCg.vel.y, car->DynGCg.vel.x);
int i;
for (i = 0; i < s->_ncars; i++) {
if (i == car->carElt->index) {
continue;
}
tdble tmpas = 1.00;
tCar* otherCar = &(SimCarTable[i]);
tdble otherYaw = otherCar->DynGC.pos.az;
tdble tmpsdpang = spdang - atan2(y - otherCar->DynGC.pos.y, x - otherCar->DynGC.pos.x);
NORM_PI_PI(tmpsdpang);
tdble dyaw = yaw - otherYaw;
NORM_PI_PI(dyaw);
if ((otherCar->DynGC.vel.x > 10.0) &&
(fabs(dyaw) < 0.1396)) {
if (fabs(tmpsdpang) > 2.9671) { /* 10 degrees */
/* behind another car - reduce overall airflow */
tdble factor = (fabs(tmpsdpang)-2.9671)/(M_PI-2.9671);
tmpas = 1.0 - factor*exp(- 2.0 * DIST(x, y, otherCar->DynGC.pos.x, otherCar->DynGC.pos.y) /
(otherCar->aero.Cd * otherCar->DynGC.vel.x));
i_flow = i_flow * tmpas;
}
}
}
}
//if (index==1) { -- thrown away so that we have different downforce
// reduction for front and rear parts.
if (1) {
// downforce due to body and ground effect.
tdble alpha = 0.0f;
tdble vt2b = vt2 * (alpha+(1-alpha)*i_flow);
vt2b = vt2b * vt2b;
tdble hm = 1.5 * (car->wheel[0].rideHeight + car->wheel[1].rideHeight + car->wheel[2].rideHeight + car->wheel[3].rideHeight);
hm = hm*hm;
hm = hm*hm;
hm = 1.0 + exp(-3.0*hm);
car->aero.lift[index] = - car->aero.Clift[index] * vt2b * hm;
//car->aero.lift[1] = - car->aero.Clift[1] * vt2b * hm;
//printf ("%f\n", car->aero.lift[0]+car->aero.lift[1]);
}
vt2=vt2*i_flow;
vt2=vt2*vt2;
aoa += wing->angle;
// the sinus of the angle of attack
tdble sinaoa = sin(aoa);
tdble cosaoa = cos(aoa);
if (car->DynGC.vel.x > 0.0f) {
switch (car->options->aeroflow_model) {
case SIMPLE:
wing->forces.x = wing->Kx * vt2 * (1.0f + (tdble)car->dammage / 10000.0f) * sinaoa;
wing->forces.z = wing->Kz * vt2 * sinaoa;
break;
case PLANAR:
wing->forces.x = wing->Kx * vt2 * (1.0f + (tdble)car->dammage / 10000.0f) * sinaoa * sinaoa * sinaoa;
wing->forces.z = wing->Kz * vt2 * sinaoa * sinaoa * cosaoa;
break;
case OPTIMAL:
wing->forces.x = wing->Kx * vt2 * (1.0f + (tdble)car->dammage / 10000.0f) * (1.0f - cosaoa);
wing->forces.x = wing->Kx * vt2 * (1.0f + (tdble)car->dammage / 10000.0f) * sinaoa;
break;
default:
fprintf (stderr, "Unimplemented option %d for aeroflow model\n", car->options->aeroflow_model);
}
} else {
wing->forces.x = wing->forces.z = 0.0f;
}
}
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;
}
static void common_drive(int index, tCarElt* car, tSituation *s)
{
tdble slip;
tdble ax0;
tdble brake;
tdble clutch;
tdble throttle;
tdble leftSteer;
tdble rightSteer;
int scrw, scrh, dummy;
int idx = index - 1;
tControlCmd *cmd = HCtx[idx]->CmdControl;
const int BUFSIZE = 1024;
char sstring[BUFSIZE];
static int firstTime = 1;
if (firstTime) {
if (HCtx[idx]->MouseControlUsed) {
GfuiMouseShow();
GfctrlMouseInitCenter();
}
GfuiKeyEventRegisterCurrent(onKeyAction);
GfuiSKeyEventRegisterCurrent(onSKeyAction);
firstTime = 0;
}
HCtx[idx]->distToStart = RtGetDistFromStart(car);
HCtx[idx]->Gear = (tdble)car->_gear; /* telemetry */
GfScrGetSize(&scrw, &scrh, &dummy, &dummy);
memset(&(car->ctrl), 0, sizeof(tCarCtrl));
car->_lightCmd = HCtx[idx]->lightCmd;
if (car->_laps != HCtx[idx]->LastPitStopLap) {
car->_raceCmd = RM_CMD_PIT_ASKED;
}
if (lastKeyUpdate != s->currentTime) {
/* Update the controls only once for all the players */
updateKeys();
if (joyPresent) {
GfctrlJoyGetCurrent(joyInfo);
}
GfctrlMouseGetCurrent(mouseInfo);
lastKeyUpdate = s->currentTime;
}
if (((cmd[CMD_ABS].type == GFCTRL_TYPE_JOY_BUT) && joyInfo->edgeup[cmd[CMD_ABS].val]) ||
((cmd[CMD_ABS].type == GFCTRL_TYPE_KEYBOARD) && keyInfo[cmd[CMD_ABS].val].edgeUp) ||
((cmd[CMD_ABS].type == GFCTRL_TYPE_SKEYBOARD) && skeyInfo[cmd[CMD_ABS].val].edgeUp))
{
HCtx[idx]->ParamAbs = 1 - HCtx[idx]->ParamAbs;
snprintf(sstring, BUFSIZE, "%s/%s/%d", HM_SECT_PREF, HM_LIST_DRV, index);
GfParmSetStr(PrefHdle, sstring, HM_ATT_ABS, Yn[1 - HCtx[idx]->ParamAbs]);
GfParmWriteFile(NULL, PrefHdle, "Human");
}
if (((cmd[CMD_ASR].type == GFCTRL_TYPE_JOY_BUT) && joyInfo->edgeup[cmd[CMD_ASR].val]) ||
((cmd[CMD_ASR].type == GFCTRL_TYPE_KEYBOARD) && keyInfo[cmd[CMD_ASR].val].edgeUp) ||
((cmd[CMD_ASR].type == GFCTRL_TYPE_SKEYBOARD) && skeyInfo[cmd[CMD_ASR].val].edgeUp))
{
HCtx[idx]->ParamAsr = 1 - HCtx[idx]->ParamAsr;
snprintf(sstring, BUFSIZE, "%s/%s/%d", HM_SECT_PREF, HM_LIST_DRV, index);
GfParmSetStr(PrefHdle, sstring, HM_ATT_ASR, Yn[1 - HCtx[idx]->ParamAsr]);
GfParmWriteFile(NULL, PrefHdle, "Human");
}
const int bufsize = sizeof(car->_msgCmd[0]);
snprintf(car->_msgCmd[0], bufsize, "%s %s", (HCtx[idx]->ParamAbs ? "ABS" : ""), (HCtx[idx]->ParamAsr ? "ASR" : ""));
memcpy(car->_msgColorCmd, color, sizeof(car->_msgColorCmd));
if (((cmd[CMD_SPDLIM].type == GFCTRL_TYPE_JOY_BUT) && (joyInfo->levelup[cmd[CMD_SPDLIM].val] == 1)) ||
((cmd[CMD_SPDLIM].type == GFCTRL_TYPE_KEYBOARD) && (keyInfo[cmd[CMD_SPDLIM].val].state == GFUI_KEY_DOWN)) ||
((cmd[CMD_SPDLIM].type == GFCTRL_TYPE_SKEYBOARD) && (skeyInfo[cmd[CMD_SPDLIM].val].state == GFUI_KEY_DOWN)))
{
speedLimiter = 1;
snprintf(car->_msgCmd[1], bufsize, "Speed Limiter On");
} else {
speedLimiter = 0;
snprintf(car->_msgCmd[1], bufsize, "Speed Limiter Off");
}
if (((cmd[CMD_LIGHT1].type == GFCTRL_TYPE_JOY_BUT) && joyInfo->edgeup[cmd[CMD_LIGHT1].val]) ||
((cmd[CMD_LIGHT1].type == GFCTRL_TYPE_KEYBOARD) && keyInfo[cmd[CMD_LIGHT1].val].edgeUp) ||
((cmd[CMD_LIGHT1].type == GFCTRL_TYPE_SKEYBOARD) && skeyInfo[cmd[CMD_LIGHT1].val].edgeUp))
{
if (HCtx[idx]->lightCmd & RM_LIGHT_HEAD1) {
HCtx[idx]->lightCmd &= ~(RM_LIGHT_HEAD1 | RM_LIGHT_HEAD2);
} else {
HCtx[idx]->lightCmd |= RM_LIGHT_HEAD1 | RM_LIGHT_HEAD2;
}
}
switch (cmd[CMD_LEFTSTEER].type) {
case GFCTRL_TYPE_JOY_AXIS:
ax0 = joyInfo->ax[cmd[CMD_LEFTSTEER].val] + cmd[CMD_LEFTSTEER].deadZone;
if (ax0 > cmd[CMD_LEFTSTEER].max) {
ax0 = cmd[CMD_LEFTSTEER].max;
} else if (ax0 < cmd[CMD_LEFTSTEER].min) {
ax0 = cmd[CMD_LEFTSTEER].min;
}
// normalize ax0 to -1..0
ax0 = (ax0 - cmd[CMD_LEFTSTEER].max) / (cmd[CMD_LEFTSTEER].max - cmd[CMD_LEFTSTEER].min);
leftSteer = -SIGN(ax0) * cmd[CMD_LEFTSTEER].pow * pow(fabs(ax0), cmd[CMD_LEFTSTEER].sens) / (1.0 + cmd[CMD_LEFTSTEER].spdSens * car->pub.speed);
break;
case GFCTRL_TYPE_MOUSE_AXIS:
ax0 = mouseInfo->ax[cmd[CMD_LEFTSTEER].val] - cmd[CMD_LEFTSTEER].deadZone; //FIXME: correct?
if (ax0 > cmd[CMD_LEFTSTEER].max) {
ax0 = cmd[CMD_LEFTSTEER].max;
} else if (ax0 < cmd[CMD_LEFTSTEER].min) {
ax0 = cmd[CMD_LEFTSTEER].min;
}
ax0 = ax0 * cmd[CMD_LEFTSTEER].pow;
leftSteer = pow(fabs(ax0), cmd[CMD_LEFTSTEER].sens) / (1.0 + cmd[CMD_LEFTSTEER].spdSens * car->pub.speed / 10.0);
break;
case GFCTRL_TYPE_KEYBOARD:
case GFCTRL_TYPE_SKEYBOARD:
case GFCTRL_TYPE_JOY_BUT:
if (cmd[CMD_LEFTSTEER].type == GFCTRL_TYPE_KEYBOARD) {
ax0 = keyInfo[cmd[CMD_LEFTSTEER].val].state;
} else if (cmd[CMD_LEFTSTEER].type == GFCTRL_TYPE_SKEYBOARD) {
ax0 = skeyInfo[cmd[CMD_LEFTSTEER].val].state;
} else {
ax0 = joyInfo->levelup[cmd[CMD_LEFTSTEER].val];
}
if (ax0 == 0) {
HCtx[idx]->prevLeftSteer = leftSteer = 0;
} else {
ax0 = 2 * ax0 - 1;
leftSteer = HCtx[idx]->prevLeftSteer + ax0 * cmd[CMD_LEFTSTEER].sens * s->deltaTime / (1.0 + cmd[CMD_LEFTSTEER].spdSens * car->pub.speed / 10.0);
if (leftSteer > 1.0) leftSteer = 1.0;
if (leftSteer < 0.0) leftSteer = 0.0;
HCtx[idx]->prevLeftSteer = leftSteer;
}
break;
default:
leftSteer = 0;
break;
}
switch (cmd[CMD_RIGHTSTEER].type) {
case GFCTRL_TYPE_JOY_AXIS:
ax0 = joyInfo->ax[cmd[CMD_RIGHTSTEER].val] - cmd[CMD_RIGHTSTEER].deadZone;
if (ax0 > cmd[CMD_RIGHTSTEER].max) {
ax0 = cmd[CMD_RIGHTSTEER].max;
} else if (ax0 < cmd[CMD_RIGHTSTEER].min) {
ax0 = cmd[CMD_RIGHTSTEER].min;
}
// normalize ax to 0..1
ax0 = (ax0 - cmd[CMD_RIGHTSTEER].min) / (cmd[CMD_RIGHTSTEER].max - cmd[CMD_RIGHTSTEER].min);
rightSteer = -SIGN(ax0) * cmd[CMD_RIGHTSTEER].pow * pow(fabs(ax0), cmd[CMD_RIGHTSTEER].sens) / (1.0 + cmd[CMD_RIGHTSTEER].spdSens * car->pub.speed);
break;
case GFCTRL_TYPE_MOUSE_AXIS:
ax0 = mouseInfo->ax[cmd[CMD_RIGHTSTEER].val] - cmd[CMD_RIGHTSTEER].deadZone;
if (ax0 > cmd[CMD_RIGHTSTEER].max) {
ax0 = cmd[CMD_RIGHTSTEER].max;
} else if (ax0 < cmd[CMD_RIGHTSTEER].min) {
ax0 = cmd[CMD_RIGHTSTEER].min;
}
ax0 = ax0 * cmd[CMD_RIGHTSTEER].pow;
rightSteer = - pow(fabs(ax0), cmd[CMD_RIGHTSTEER].sens) / (1.0 + cmd[CMD_RIGHTSTEER].spdSens * car->pub.speed / 10.0);
break;
case GFCTRL_TYPE_KEYBOARD:
case GFCTRL_TYPE_SKEYBOARD:
case GFCTRL_TYPE_JOY_BUT:
if (cmd[CMD_RIGHTSTEER].type == GFCTRL_TYPE_KEYBOARD) {
ax0 = keyInfo[cmd[CMD_RIGHTSTEER].val].state;
} else if (cmd[CMD_RIGHTSTEER].type == GFCTRL_TYPE_SKEYBOARD) {
ax0 = skeyInfo[cmd[CMD_RIGHTSTEER].val].state;
} else {
ax0 = joyInfo->levelup[cmd[CMD_RIGHTSTEER].val];
}
if (ax0 == 0) {
HCtx[idx]->prevRightSteer = rightSteer = 0;
} else {
ax0 = 2 * ax0 - 1;
rightSteer = HCtx[idx]->prevRightSteer - ax0 * cmd[CMD_RIGHTSTEER].sens * s->deltaTime/ (1.0 + cmd[CMD_RIGHTSTEER].spdSens * car->pub.speed / 10.0);
if (rightSteer > 0.0) rightSteer = 0.0;
if (rightSteer < -1.0) rightSteer = -1.0;
HCtx[idx]->prevRightSteer = rightSteer;
}
break;
default:
rightSteer = 0;
break;
}
car->_steerCmd = leftSteer + rightSteer;
switch (cmd[CMD_BRAKE].type) {
case GFCTRL_TYPE_JOY_AXIS:
brake = joyInfo->ax[cmd[CMD_BRAKE].val];
if (brake > cmd[CMD_BRAKE].max) {
brake = cmd[CMD_BRAKE].max;
} else if (brake < cmd[CMD_BRAKE].min) {
brake = cmd[CMD_BRAKE].min;
}
car->_brakeCmd = fabs(cmd[CMD_BRAKE].pow *
pow(fabs((brake - cmd[CMD_BRAKE].minVal) /
(cmd[CMD_BRAKE].max - cmd[CMD_BRAKE].min)),
cmd[CMD_BRAKE].sens));
break;
case GFCTRL_TYPE_MOUSE_AXIS:
ax0 = mouseInfo->ax[cmd[CMD_BRAKE].val] - cmd[CMD_BRAKE].deadZone;
if (ax0 > cmd[CMD_BRAKE].max) {
ax0 = cmd[CMD_BRAKE].max;
} else if (ax0 < cmd[CMD_BRAKE].min) {
ax0 = cmd[CMD_BRAKE].min;
}
ax0 = ax0 * cmd[CMD_BRAKE].pow;
car->_brakeCmd = pow(fabs(ax0), cmd[CMD_BRAKE].sens) / (1.0 + cmd[CMD_BRAKE].spdSens * car->_speed_x / 10.0);
break;
case GFCTRL_TYPE_JOY_BUT:
car->_brakeCmd = joyInfo->levelup[cmd[CMD_BRAKE].val];
break;
case GFCTRL_TYPE_MOUSE_BUT:
car->_brakeCmd = mouseInfo->button[cmd[CMD_BRAKE].val];
break;
case GFCTRL_TYPE_KEYBOARD:
car->_brakeCmd = keyInfo[cmd[CMD_BRAKE].val].state;
break;
case GFCTRL_TYPE_SKEYBOARD:
car->_brakeCmd = skeyInfo[cmd[CMD_BRAKE].val].state;
break;
default:
car->_brakeCmd = 0;
break;
}
switch (cmd[CMD_CLUTCH].type) {
case GFCTRL_TYPE_JOY_AXIS:
clutch = joyInfo->ax[cmd[CMD_CLUTCH].val];
if (clutch > cmd[CMD_CLUTCH].max) {
clutch = cmd[CMD_CLUTCH].max;
} else if (clutch < cmd[CMD_CLUTCH].min) {
clutch = cmd[CMD_CLUTCH].min;
}
car->_clutchCmd = fabs(cmd[CMD_CLUTCH].pow *
pow(fabs((clutch - cmd[CMD_CLUTCH].minVal) /
(cmd[CMD_CLUTCH].max - cmd[CMD_CLUTCH].min)),
cmd[CMD_CLUTCH].sens));
break;
case GFCTRL_TYPE_MOUSE_AXIS:
ax0 = mouseInfo->ax[cmd[CMD_CLUTCH].val] - cmd[CMD_CLUTCH].deadZone;
if (ax0 > cmd[CMD_CLUTCH].max) {
ax0 = cmd[CMD_CLUTCH].max;
} else if (ax0 < cmd[CMD_CLUTCH].min) {
ax0 = cmd[CMD_CLUTCH].min;
}
ax0 = ax0 * cmd[CMD_CLUTCH].pow;
car->_clutchCmd = pow(fabs(ax0), cmd[CMD_CLUTCH].sens) / (1.0 + cmd[CMD_CLUTCH].spdSens * car->_speed_x / 10.0);
break;
case GFCTRL_TYPE_JOY_BUT:
car->_clutchCmd = joyInfo->levelup[cmd[CMD_CLUTCH].val];
break;
case GFCTRL_TYPE_MOUSE_BUT:
car->_clutchCmd = mouseInfo->button[cmd[CMD_CLUTCH].val];
break;
case GFCTRL_TYPE_KEYBOARD:
car->_clutchCmd = keyInfo[cmd[CMD_CLUTCH].val].state;
break;
case GFCTRL_TYPE_SKEYBOARD:
car->_clutchCmd = skeyInfo[cmd[CMD_CLUTCH].val].state;
break;
default:
car->_clutchCmd = 0;
break;
}
// if player's used the clutch manually then we dispense with autoClutch
if (car->_clutchCmd != 0.0f)
HCtx[idx]->autoClutch = 0;
switch (cmd[CMD_THROTTLE].type) {
case GFCTRL_TYPE_JOY_AXIS:
throttle = joyInfo->ax[cmd[CMD_THROTTLE].val];
if (throttle > cmd[CMD_THROTTLE].max) {
throttle = cmd[CMD_THROTTLE].max;
} else if (throttle < cmd[CMD_THROTTLE].min) {
throttle = cmd[CMD_THROTTLE].min;
}
car->_accelCmd = fabs(cmd[CMD_THROTTLE].pow *
pow(fabs((throttle - cmd[CMD_THROTTLE].minVal) /
(cmd[CMD_THROTTLE].max - cmd[CMD_THROTTLE].min)),
cmd[CMD_THROTTLE].sens));
break;
case GFCTRL_TYPE_MOUSE_AXIS:
ax0 = mouseInfo->ax[cmd[CMD_THROTTLE].val] - cmd[CMD_THROTTLE].deadZone;
if (ax0 > cmd[CMD_THROTTLE].max) {
ax0 = cmd[CMD_THROTTLE].max;
} else if (ax0 < cmd[CMD_THROTTLE].min) {
ax0 = cmd[CMD_THROTTLE].min;
}
ax0 = ax0 * cmd[CMD_THROTTLE].pow;
car->_accelCmd = pow(fabs(ax0), cmd[CMD_THROTTLE].sens) / (1.0 + cmd[CMD_THROTTLE].spdSens * car->_speed_x / 10.0);
if (isnan (car->_accelCmd)) {
car->_accelCmd = 0;
}
/* printf(" axO:%f accelCmd:%f\n", ax0, car->_accelCmd); */
break;
case GFCTRL_TYPE_JOY_BUT:
car->_accelCmd = joyInfo->levelup[cmd[CMD_THROTTLE].val];
break;
case GFCTRL_TYPE_MOUSE_BUT:
car->_accelCmd = mouseInfo->button[cmd[CMD_THROTTLE].val];
break;
case GFCTRL_TYPE_KEYBOARD:
car->_accelCmd = keyInfo[cmd[CMD_THROTTLE].val].state;
break;
case GFCTRL_TYPE_SKEYBOARD:
car->_accelCmd = skeyInfo[cmd[CMD_THROTTLE].val].state;
break;
default:
car->_accelCmd = 0;
break;
}
if (s->currentTime > 1.0) {
// thanks Christos for the following: gradual accel/brake changes for on/off controls.
const tdble inc_rate = 0.2f;
if (cmd[CMD_BRAKE].type == GFCTRL_TYPE_JOY_BUT ||
cmd[CMD_BRAKE].type == GFCTRL_TYPE_MOUSE_BUT ||
cmd[CMD_BRAKE].type == GFCTRL_TYPE_KEYBOARD ||
cmd[CMD_BRAKE].type == GFCTRL_TYPE_SKEYBOARD)
{
tdble d_brake = car->_brakeCmd - HCtx[idx]->pbrake;
if (fabs(d_brake) > inc_rate && car->_brakeCmd > HCtx[idx]->pbrake) {
car->_brakeCmd = MIN(car->_brakeCmd, HCtx[idx]->pbrake + inc_rate*d_brake/fabs(d_brake));
}
HCtx[idx]->pbrake = car->_brakeCmd;
}
if (cmd[CMD_THROTTLE].type == GFCTRL_TYPE_JOY_BUT ||
cmd[CMD_THROTTLE].type == GFCTRL_TYPE_MOUSE_BUT ||
cmd[CMD_THROTTLE].type == GFCTRL_TYPE_KEYBOARD ||
cmd[CMD_THROTTLE].type == GFCTRL_TYPE_SKEYBOARD)
{
tdble d_accel = car->_accelCmd - HCtx[idx]->paccel;
if (fabs(d_accel) > inc_rate && car->_accelCmd > HCtx[idx]->paccel) {
car->_accelCmd = MIN(car->_accelCmd, HCtx[idx]->paccel + inc_rate*d_accel/fabs(d_accel));
}
HCtx[idx]->paccel = car->_accelCmd;
}
}
if (HCtx[idx]->AutoReverseEngaged) {
/* swap brake and throttle */
brake = car->_brakeCmd;
car->_brakeCmd = car->_accelCmd;
car->_accelCmd = brake;
}
if (HCtx[idx]->ParamAbs)
{
if (fabs(car->_speed_x) > 10.0)
{
int i;
tdble skidAng = atan2(car->_speed_Y, car->_speed_X) - car->_yaw;
NORM_PI_PI(skidAng);
if (car->_speed_x > 5 && fabs(skidAng) > 0.2)
car->_brakeCmd = MIN(car->_brakeCmd, 0.10 + 0.70 * cos(skidAng));
if (fabs(car->_steerCmd) > 0.1)
{
tdble decel = ((fabs(car->_steerCmd)-0.1) * (1.0 + fabs(car->_steerCmd)) * 0.6);
car->_brakeCmd = MIN(car->_brakeCmd, MAX(0.35, 1.0 - decel));
}
const tdble abs_slip = 2.5;
const tdble abs_range = 5.0;
slip = 0;
for (i = 0; i < 4; i++) {
slip += car->_wheelSpinVel(i) * car->_wheelRadius(i);
}
slip = car->_speed_x - slip/4.0f;
if (slip > abs_slip)
car->_brakeCmd = car->_brakeCmd - MIN(car->_brakeCmd*0.8, (slip - abs_slip) / abs_range);
}
}
if (HCtx[idx]->ParamAsr)
{
tdble trackangle = RtTrackSideTgAngleL(&(car->_trkPos));
tdble angle = trackangle - car->_yaw;
NORM_PI_PI(angle);
tdble maxaccel = 0.0;
if (car->_trkPos.seg->type == TR_STR)
maxaccel = MIN(car->_accelCmd, 0.2);
else if (car->_trkPos.seg->type == TR_LFT && angle < 0.0)
maxaccel = MIN(car->_accelCmd, MIN(0.6, -angle));
else if (car->_trkPos.seg->type == TR_RGT && angle > 0.0)
maxaccel = MIN(car->_accelCmd, MIN(0.6, angle));
tdble origaccel = car->_accelCmd;
tdble skidAng = atan2(car->_speed_Y, car->_speed_X) - car->_yaw;
NORM_PI_PI(skidAng);
if (car->_speed_x > 5 && fabs(skidAng) > 0.2)
{
car->_accelCmd = MIN(car->_accelCmd, 0.15 + 0.70 * cos(skidAng));
car->_accelCmd = MAX(car->_accelCmd, maxaccel);
}
if (fabs(car->_steerCmd) > 0.1)
{
tdble decel = ((fabs(car->_steerCmd)-0.1) * (1.0 + fabs(car->_steerCmd)) * 0.8);
car->_accelCmd = MIN(car->_accelCmd, MAX(0.35, 1.0 - decel));
}
tdble drivespeed = 0.0;
switch (HCtx[idx]->drivetrain)
{
case D4WD:
drivespeed = ((car->_wheelSpinVel(FRNT_RGT) + car->_wheelSpinVel(FRNT_LFT)) *
car->_wheelRadius(FRNT_LFT) +
(car->_wheelSpinVel(REAR_RGT) + car->_wheelSpinVel(REAR_LFT)) *
car->_wheelRadius(REAR_LFT)) / 4.0;
break;
case DFWD:
drivespeed = (car->_wheelSpinVel(FRNT_RGT) + car->_wheelSpinVel(FRNT_LFT)) *
car->_wheelRadius(FRNT_LFT) / 2.0;
break;
default:
drivespeed = (car->_wheelSpinVel(REAR_RGT) + car->_wheelSpinVel(REAR_LFT)) *
car->_wheelRadius(REAR_LFT) / 2.0;
break;
}
tdble slip = drivespeed - fabs(car->_speed_x);
if (slip > 2.0)
car->_accelCmd = MIN(car->_accelCmd, origaccel - MIN(origaccel-0.1, ((slip - 2.0)/10.0)));
}
if (speedLimiter) {
tdble Dv;
if (Vtarget != 0) {
Dv = Vtarget - car->_speed_x;
if (Dv > 0.0) {
car->_accelCmd = MIN(car->_accelCmd, fabs(Dv/6.0));
} else {
car->_brakeCmd = MAX(car->_brakeCmd, fabs(Dv/5.0));
car->_accelCmd = 0;
}
}
}
#ifndef WIN32
#ifdef TELEMETRY
if ((car->_laps > 1) && (car->_laps < 5)) {
if (HCtx[idx]->lap == 1) {
RtTelemStartMonitoring("Player");
}
RtTelemUpdate(car->_curLapTime);
}
if (car->_laps == 5) {
if (HCtx[idx]->lap == 4) {
RtTelemShutdown();
}
}
#endif
#endif
HCtx[idx]->lap = car->_laps;
}
