/* 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
}
}
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;
}
}
void opponent::update(carData *myCar){
//TODO there we compute the path followed by the car
if(car->_state & RM_CAR_STATE_NO_SIMU)
return; /* we don't need to compute if we are the car of the car is no longer in the simulation */
pos.x = car->_pos_X;
pos.y = car->_pos_Y;
currentSeg = car->_trkPos.seg;
trackAngle = RtTrackSideTgAngleL(&(car->_trkPos));
if(car == myCar->getCarPnt())
distToStart = car->_distFromStartLine;
else
distToStart = currentSeg->lgfromstart + getDistToSegStart();
/* update speed in track direction */
speed = getSpeed();
float cosa = speed/sqrt(car->_speed_X*car->_speed_X + car->_speed_Y*car->_speed_Y);
float alpha = acos(cosa);
width = car->_dimension_x*sin(alpha) + car->_dimension_y*cosa;
}
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
/* 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));
}
/* 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();
}
/* compute speed component parallel to the track */
float opponent_get_speed(tCarElt *car)
{
point_t speed, dir;
float trackangle = RtTrackSideTgAngleL(&(car->_trkPos));
speed.x = car->_speed_X;
speed.y = car->_speed_Y;
dir.x = cos(trackangle);
dir.y = sin(trackangle);
return speed*dir;
}
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;
}
}
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;
}
}
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;
}
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;
}
/* 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;
}
/* Car position */
extern "C" float get_track_angle(tTrkLocPos *pos) {
return RtTrackSideTgAngleL(pos);
}
