/* 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); }