/** Used to get the normal vector of the road (pointing upward). Local coordinates are used to locate the point where to get the road normal vector. The vector is normalized. @ingroup tracktools @param p Local position @param norm Returned normalized road normal vector */ void RtTrackSurfaceNormalL(tTrkLocPos *p, t3Dd *norm) { tTrkLocPos p1; t3Dd px1, px2, py1, py2; t3Dd v1, v2; tdble lg; p1.seg = p->seg; p1.toStart = 0; p1.toRight = p->toRight; RtTrackLocal2Global(&p1, &px1.x, &px1.y, TR_TORIGHT); px1.z = RtTrackHeightL(&p1); if (p1.seg->type == TR_STR) { p1.toStart = p1.seg->length; } else { p1.toStart = p1.seg->arc; } RtTrackLocal2Global(&p1, &px2.x, &px2.y, TR_TORIGHT); px2.z = RtTrackHeightL(&p1); p1.toRight = 0; p1.toStart = p->toStart; RtTrackLocal2Global(&p1, &py1.x, &py1.y, TR_TORIGHT); py1.z = RtTrackHeightL(&p1); p1.toRight = p1.seg->width; RtTrackLocal2Global(&p1, &py2.x, &py2.y, TR_TORIGHT); py2.z = RtTrackHeightL(&p1); v1.x = px2.x - px1.x; v1.y = px2.y - px1.y; v1.z = px2.z - px1.z; v2.x = py2.x - py1.x; v2.y = py2.y - py1.y; v2.z = py2.z - py1.z; norm->x = v1.y * v2.z - v2.y * v1.z; norm->y = v2.x * v1.z - v1.x * v2.z; norm->z = v1.x * v2.y - v2.x * v1.y; lg = sqrt(norm->x * norm->x + norm->y * norm->y + norm->z * norm->z); if (lg == 0.0) { lg = 1.0f; } else { lg = 1.0f / lg; } norm->x *= lg; norm->y *= lg; norm->z *= lg; }
void SimWheelUpdateRide(tCar *car, int index) { tWheel *wheel = &(car->wheel[index]); tdble Zroad; // compute suspension travel RtTrackGlobal2Local(car->trkPos.seg, wheel->pos.x, wheel->pos.y, &(wheel->trkPos), TR_LPOS_SEGMENT); wheel->zRoad = Zroad = RtTrackHeightL(&(wheel->trkPos)); // Wheel susp.x is not the wheel movement, look at SimSuspCheckIn, it becomes there scaled with // susp->spring.bellcrank, so we invert this here. tdble prexwheel = wheel->susp.x / wheel->susp.spring.bellcrank; tdble new_susp_x= prexwheel - wheel->rel_vel * SimDeltaTime; tdble max_extend = wheel->pos.z - Zroad; wheel->rideHeight = max_extend; if (max_extend < new_susp_x) { new_susp_x = max_extend; wheel->rel_vel = 0.0f; } else if (new_susp_x < wheel->susp.spring.packers) { wheel->rel_vel = 0.0f; } tdble prex = wheel->susp.x; wheel->susp.x = new_susp_x; // verify the suspension travel, beware, wheel->susp.x will be scaled by SimSuspCheckIn SimSuspCheckIn(&(wheel->susp)); wheel->susp.v = (prex - wheel->susp.x) / SimDeltaTime; // update wheel brake SimBrakeUpdate(car, wheel, &(wheel->brake)); }
void SimWheelUpdateRide(tCar *car, int index) { tWheel *wheel = &(car->wheel[index]); tdble Zroad; // compute suspension travel RtTrackGlobal2Local(car->trkPos.seg, wheel->pos.x, wheel->pos.y, &(wheel->trkPos), TR_LPOS_SEGMENT); wheel->zRoad = Zroad = RtTrackHeightL(&(wheel->trkPos)); tdble prexwheel = wheel->susp.x / wheel->susp.spring.bellcrank; tdble new_susp_x= prexwheel - wheel->rel_vel * SimDeltaTime; tdble max_extend = wheel->pos.z - Zroad; wheel->rideHeight = max_extend; if (max_extend < new_susp_x) { new_susp_x = max_extend; wheel->rel_vel = 0.0f; } else if (new_susp_x < wheel->susp.spring.packers) { wheel->rel_vel = 0.0f; } tdble prex = wheel->susp.x; wheel->susp.x = new_susp_x; // verify the suspension travel SimSuspCheckIn(&(wheel->susp)); wheel->susp.v = (prex - wheel->susp.x) / SimDeltaTime; // update wheel brake SimBrakeUpdate(car, wheel, &(wheel->brake)); }
/** Returns the absolute height in meters of the road at the Global position (segment, X, Y) @ingroup tracktools @param seg Segment @param X Global X position @param Y Global Y position @return Height in meters */ tdble RtTrackHeightG(tTrackSeg *seg, tdble X, tdble Y) { tTrkLocPos p; RtTrackGlobal2Local(seg, X, Y, &p, TR_LPOS_SEGMENT); return RtTrackHeightL(&p); }
void SimWheelUpdateRide(tCar *car, int index) { tWheel *wheel = &(car->wheel[index]); tdble Zroad; /* compute suspension travel */ RtTrackGlobal2Local(car->trkPos.seg, wheel->pos.x, wheel->pos.y, &(wheel->trkPos), TR_LPOS_SEGMENT); wheel->zRoad = Zroad = RtTrackHeightL(&(wheel->trkPos)); tdble prexwheel = wheel->susp.x / wheel->susp.spring.bellcrank; tdble new_susp_x= prexwheel - wheel->rel_vel * SimDeltaTime; tdble max_extend; t3Dd normal; t3Dd rel_normal; // Find normal of track. RtTrackSurfaceNormalL(&(wheel->trkPos), &normal); wheel->normal = normal; { sgQuat Q; sgCopyQuat (Q, car->posQuat); sgPreRotQuat (Q, FLOAT_RAD2DEG(wheel->relPos.ax), 1.0f, 0.0f, 0.0f); sgVec3 P = {normal.x, normal.y, normal.z}; sgRotateVecQuat (P, Q); sg2t3 (P, rel_normal); } tdble dZ = wheel->pos.z - Zroad; //NaiveRotate (d, angles, &d); #ifdef USE_THICKNESS int seg_id = ((int) ((tdble) N_THICKNESS_SEGMENTS * (wheel->relPos.ay/(2*M_PI)))) % N_THICKNESS_SEGMENTS; if (seg_id<0) seg_id += N_THICKNESS_SEGMENTS; tdble adjRadius = wheel->radius + wheel->thickness[seg_id]; #else tdble adjRadius = wheel->radius; #endif if (rel_normal.z > MIN_NORMAL_Z) { wheel->susp.fx = adjRadius - adjRadius/rel_normal.z; wheel->susp.fy = 0.0; //wheel->susp.x = wheel->rideHeight = max_extend = adjRadius + ((dZ)*normal.z - adjRadius)/rel_normal.z; } else { //wheel->susp.x = wheel->rideHeight = (wheel->pos.z - Zroad); // wheel->susp.x = wheel->rideHeight = wheel->susp.spring.packers; wheel->susp.fx = 0.0; wheel->state = wheel->state | SIM_SUSP_COMP; max_extend = 0.0; } /* Note from Christos about the 'max_extend' variable set right above : This variable's name a left-over from simuv2. It has a slightly extended use. Perhaps the name should be changed. In simuv2, when the suspension was fully compressed then the suspension reaction was ignored, the car speed was set to be tangential to the track, and the car height was set to be just above the track. Now it basically signifies that the car is not a 'normal' regime, meaning either: a) The suspension is maxed out, and perhaps the car is touching the ground. b) The car has rolled over more than a certain amount. In either case, the collision code is used to see how to react. In the collision code, we check if the car is upright : If the car is upright: * If the car is not very low, then the simuv2 approach is used. * If the car is under the track (beyond a margin) then damages are taken and there is some friction. If the car is not upright, then we use the collision code with the ground. Now, this collision code with the ground (collidez) seems to be somewhat problematic, but I do not know why. Virtually the same code is used for collisions with the walls, and there the car behaves nicely. Possibly there is a mixup with the frames of reference. */ wheel->rideHeight = max_extend; wheel->bump_force = 0.0; // force of the wheel bumping up into the frame if (max_extend < new_susp_x) { new_susp_x = max_extend; wheel->rel_vel = 0.0f; } else if (new_susp_x <= wheel->susp.spring.packers) { wheel->bump_force = wheel->mass * wheel->rel_vel / SimDeltaTime; wheel->susp.x = wheel->susp.spring.packers; wheel->rel_vel = 0.0f; } tdble prex = wheel->susp.x; wheel->susp.x = new_susp_x; wheel->relPos.az = wheel->steer + wheel->staticPos.az; // Transform from world to suspension FOR if (index % 2) { wheel->relPos.ax = -wheel->staticPos.ax; } else { wheel->relPos.ax = wheel->staticPos.ax; } wheel->relPos.ax += wheel->dynamic_camber*wheel->steer; if (car->options->alignment_damage && wheel->rotational_damage_x>0.0) { wheel->relPos.ax += wheel->rotational_damage_x*sin(wheel->relPos.ay + wheel->bent_damage_x); wheel->relPos.az += wheel->rotational_damage_z*cos(wheel->relPos.ay + wheel->bent_damage_z); } //wheel->relPos.z = - wheel->susp.x / wheel->susp.spring.bellcrank + wheel->radius; /* center relative to GC */ /* verify the suspension travel */ SimSuspCheckIn(&(wheel->susp)); //wheel->rideHeight = wheel->susp.x / wheel->susp.spring.bellcrank; if (index % 2) { wheel->relPos.ax -= wheel->susp.dynamic_angles.x; } else { wheel->relPos.ax += wheel->susp.dynamic_angles.x; } wheel->susp.v = (prex - wheel->susp.x) / SimDeltaTime; /* update wheel brake */ SimBrakeUpdate(car, wheel, &(wheel->brake)); }
/* * Function * initStartingGrid * * Description * Place the cars on the starting grid * * Parameters * Race Information structure initialized * * Return * none */ static void initStartingGrid(void) { char path[64]; int i; tTrackSeg *curseg; int rows; tdble a, b; //tdble wi2; // Never used. tdble d1, d2,d3; tdble startpos, tr, ts; tdble speedInit; tdble heightInit; tCarElt *car; const char *pole; void *trHdle = ReInfo->track->params; void *params = ReInfo->params; snprintf(path, sizeof(path), "%s/%s", ReInfo->_reRaceName, RM_SECT_STARTINGGRID); /* Search for the first turn for find the pole side */ curseg = ReInfo->track->seg->next; while (curseg->type == TR_STR) { /* skip the straight segments */ curseg = curseg->next; } /* Set the pole for the inside of the first turn */ if (curseg->type == TR_LFT) { pole = GfParmGetStr(params, path, RM_ATTR_POLE, "left"); } else { pole = GfParmGetStr(params, path, RM_ATTR_POLE, "right"); } /* Tracks definitions can force the pole side */ pole = GfParmGetStr(trHdle, RM_SECT_STARTINGGRID, RM_ATTR_POLE, pole); if (strcmp(pole, "left") == 0) { a = ReInfo->track->width; b = -a; } else { a = 0; b = ReInfo->track->width; } //wi2 = ReInfo->track->width * 0.5f; // Never used. rows = (int)GfParmGetNum(params, path, RM_ATTR_ROWS, (char*)NULL, 2); rows = (int)GfParmGetNum(trHdle, RM_SECT_STARTINGGRID, RM_ATTR_ROWS, (char*)NULL, (tdble)rows); d1 = GfParmGetNum(params, path, RM_ATTR_TOSTART, (char*)NULL, 10); d1 = GfParmGetNum(trHdle, RM_SECT_STARTINGGRID, RM_ATTR_TOSTART, (char*)NULL, d1); d2 = GfParmGetNum(params, path, RM_ATTR_COLDIST, (char*)NULL, 10); d2 = GfParmGetNum(trHdle, RM_SECT_STARTINGGRID, RM_ATTR_COLDIST, (char*)NULL, d2); d3 = GfParmGetNum(params, path, RM_ATTR_COLOFFSET, (char*)NULL, 5); d3 = GfParmGetNum(trHdle, RM_SECT_STARTINGGRID, RM_ATTR_COLOFFSET, (char*)NULL, d3); speedInit = GfParmGetNum(params, path, RM_ATTR_INITSPEED, (char*)NULL, 0.0); heightInit = GfParmGetNum(params, path, RM_ATTR_INITHEIGHT, (char*)NULL, 0.3f); heightInit = GfParmGetNum(trHdle, RM_SECT_STARTINGGRID, RM_ATTR_INITHEIGHT, (char*)NULL, heightInit); if (rows < 1) { rows = 1; } for (i = 0; i < ReInfo->s->_ncars; i++) { car = &(ReInfo->carList[i]); car->_speed_x = speedInit; startpos = ReInfo->track->length - (d1 + (i / rows) * d2 + (i % rows) * d3); tr = a + b * ((i % rows) + 1) / (rows + 1); curseg = ReInfo->track->seg; /* last segment */ while (startpos < curseg->lgfromstart) { curseg = curseg->prev; } ts = startpos - curseg->lgfromstart; car->_trkPos.seg = curseg; car->_trkPos.toRight = tr; switch (curseg->type) { case TR_STR: car->_trkPos.toStart = ts; RtTrackLocal2Global(&(car->_trkPos), &(car->_pos_X), &(car->_pos_Y), TR_TORIGHT); car->_yaw = curseg->angle[TR_ZS]; break; case TR_RGT: car->_trkPos.toStart = ts / curseg->radius; RtTrackLocal2Global(&(car->_trkPos), &(car->_pos_X), &(car->_pos_Y), TR_TORIGHT); car->_yaw = curseg->angle[TR_ZS] - car->_trkPos.toStart; break; case TR_LFT: car->_trkPos.toStart = ts / curseg->radius; RtTrackLocal2Global(&(car->_trkPos), &(car->_pos_X), &(car->_pos_Y), TR_TORIGHT); car->_yaw = curseg->angle[TR_ZS] + car->_trkPos.toStart; break; } car->_pos_Z = RtTrackHeightL(&(car->_trkPos)) + heightInit; FLOAT_NORM0_2PI(car->_yaw); RePhysicsEngine().configureCar(car); } }
static void RemoveCar(tCar *car, tSituation *s) { int i; tCarElt *carElt; tTrkLocPos trkPos; int trkFlag; tdble travelTime; tdble dang; #define PULL_Z_OFFSET 3.0 #define PULL_SPD 0.5 carElt = car->carElt; if (carElt->_state & RM_CAR_STATE_PULLUP) { carElt->_pos_Z += car->restPos.vel.z * SimDeltaTime; carElt->_yaw += car->restPos.vel.az * SimDeltaTime; carElt->_roll += car->restPos.vel.ax * SimDeltaTime; carElt->_pitch += car->restPos.vel.ay * SimDeltaTime; sgMakeCoordMat4(carElt->pub.posMat, carElt->_pos_X, carElt->_pos_Y, carElt->_pos_Z - carElt->_statGC_z, RAD2DEG(carElt->_yaw), RAD2DEG(carElt->_roll), RAD2DEG(carElt->_pitch)); if (carElt->_pos_Z > (car->restPos.pos.z + PULL_Z_OFFSET)) { carElt->_state &= ~RM_CAR_STATE_PULLUP; carElt->_state |= RM_CAR_STATE_PULLSIDE; travelTime = DIST(car->restPos.pos.x, car->restPos.pos.y, carElt->_pos_X, carElt->_pos_Y) / PULL_SPD; car->restPos.vel.x = (car->restPos.pos.x - carElt->_pos_X) / travelTime; car->restPos.vel.y = (car->restPos.pos.y - carElt->_pos_Y) / travelTime; } return; } if (carElt->_state & RM_CAR_STATE_PULLSIDE) { carElt->_pos_X += car->restPos.vel.x * SimDeltaTime; carElt->_pos_Y += car->restPos.vel.y * SimDeltaTime; sgMakeCoordMat4(carElt->pub.posMat, carElt->_pos_X, carElt->_pos_Y, carElt->_pos_Z - carElt->_statGC_z, RAD2DEG(carElt->_yaw), RAD2DEG(carElt->_roll), RAD2DEG(carElt->_pitch)); if ((fabs(car->restPos.pos.x - carElt->_pos_X) < 0.5) && (fabs(car->restPos.pos.y - carElt->_pos_Y) < 0.5)) { carElt->_state &= ~RM_CAR_STATE_PULLSIDE; carElt->_state |= RM_CAR_STATE_PULLDN; } return; } if (carElt->_state & RM_CAR_STATE_PULLDN) { carElt->_pos_Z -= car->restPos.vel.z * SimDeltaTime; sgMakeCoordMat4(carElt->pub.posMat, carElt->_pos_X, carElt->_pos_Y, carElt->_pos_Z - carElt->_statGC_z, RAD2DEG(carElt->_yaw), RAD2DEG(carElt->_roll), RAD2DEG(carElt->_pitch)); if (carElt->_pos_Z < car->restPos.pos.z) { carElt->_state &= ~RM_CAR_STATE_PULLDN; carElt->_state |= RM_CAR_STATE_OUT; } return; } if (carElt->_state & RM_CAR_STATE_NO_SIMU) { return; } if ((s->_maxDammage) && (car->dammage > s->_maxDammage)) { carElt->_state |= RM_CAR_STATE_BROKEN; } else { carElt->_state |= RM_CAR_STATE_OUTOFGAS; } carElt->_gear = car->transmission.gearbox.gear = 0; carElt->_enginerpm = car->engine.rads = 0; if (!(carElt->_state & RM_CAR_STATE_DNF)) { if (fabs(carElt->_speed_x) > 1.0) { return; } } carElt->_state |= RM_CAR_STATE_PULLUP; carElt->priv.collision = car->collision = 0; for(i = 0; i < 4; i++) { carElt->_skid[i] = 0; carElt->_wheelSpinVel(i) = 0; carElt->_brakeTemp(i) = 0; } carElt->pub.DynGC = car->DynGC; carElt->_speed_x = 0; /* compute the target zone for the wrecked car */ trkPos = car->trkPos; if (trkPos.toRight > trkPos.seg->width / 2.0) { while (trkPos.seg->lside != 0) { trkPos.seg = trkPos.seg->lside; } trkPos.toLeft = -3.0; trkFlag = TR_TOLEFT; } else { while (trkPos.seg->rside != 0) { trkPos.seg = trkPos.seg->rside; } trkPos.toRight = -3.0; trkFlag = TR_TORIGHT; } trkPos.type = TR_LPOS_SEGMENT; RtTrackLocal2Global(&trkPos, &(car->restPos.pos.x), &(car->restPos.pos.y), trkFlag); car->restPos.pos.z = RtTrackHeightL(&trkPos) + carElt->_statGC_z; car->restPos.pos.az = RtTrackSideTgAngleL(&trkPos); car->restPos.pos.ax = 0; car->restPos.pos.ay = 0; car->restPos.vel.z = PULL_SPD; travelTime = (car->restPos.pos.z + PULL_Z_OFFSET - carElt->_pos_Z) / car->restPos.vel.z; dang = car->restPos.pos.az - carElt->_yaw; NORM_PI_PI(dang); car->restPos.vel.az = dang / travelTime; dang = car->restPos.pos.ax - carElt->_roll; NORM_PI_PI(dang); car->restPos.vel.ax = dang / travelTime; dang = car->restPos.pos.ay - carElt->_pitch; NORM_PI_PI(dang); car->restPos.vel.ay = dang / travelTime; }
void SimCarCollideZ(tCar *car) { int i; t3Dd car_normal; t3Dd rel_car_normal; tdble dotProd; tWheel *wheel; tdble corner_factor = 0.9f; // how much to shrink the bounding box if (car->collide_timer < 10.0) { car->collide_timer += SimDeltaTime; } if (car->carElt->_state & RM_CAR_STATE_NO_SIMU) { return; } tdble energy_restitution = 0.99f; tdble E_prev = SimCarEnergy(car); bool collide = false; // Get normal N //RtTrackSurfaceNormalL(&(car->trkPos), &car_normal); car_normal = car->normal; // Use precalculated values // Get normal N_q in local coordinate system QuatInverseRotate(car_normal, car->posQuat, rel_car_normal); // Increment the upside down timer. This can be used later to // remove cars that have been upside down for too long. if (rel_car_normal.z > 0) { car->upside_down_timer = 0.0f; } else { car->upside_down_timer += (float)(0.01*SimDeltaTime); } tdble gc_height_difference = (float)MIN(0.0, car->DynGCg.pos.z - RtTrackHeightL(&(car->trkPos))); // Go through all corners and check for collision. tdble min_height_difference = (float)MIN(0.0, gc_height_difference); for (i = 0; i < 4; i++) { wheel = &(car->wheel[i]); // We only need to check for body collision with the floor if // the suspension is maximally compressed or the car is upside // down. for (int j=0; j<2; j++) { t3Dd orig; // corner position in local coordinates t3Dd delta; // corner position in global coordinates t3Dd normal; // normal at corner position t3Dd rel_normal; // normal at corner position (local coords) tDynPt *corner = &(car->corner[i]); //if (rel_car_normal.z <= 0) { if (j==0) { // check top of car orig.x = corner->pos.x; orig.y = corner->pos.y; orig.z = car->dimension.z - car->statGC.z; } else { // check bottom of car orig.x = corner->pos.x; orig.y = corner->pos.y; orig.z = - car->statGC.z; /*if (!(wheel->state & SIM_SUSP_COMP)) { continue; }*/ } orig.x*= corner_factor; orig.y*= corner_factor; orig.z*= corner_factor; // get relative coordinates in global frame QuatRotate (orig, car->posQuat, delta); tTrkLocPos trkPos; //RtTrackGlobal2Local(car->trkPos.seg, //car->DynGCg.pos.x + orig.x, //car->DynGCg.pos.y + orig.y, //&trkPos, TR_LPOS_SEGMENT); RtTrackGlobal2Local(car->trkPos.seg, corner->pos.ax, corner->pos.ay, &trkPos, TR_LPOS_SEGMENT); tdble height_difference = car->DynGCg.pos.z + delta.z - RtTrackHeightL(&trkPos); //printf ("%d %d %f %f %f\n", i, j, height_difference, car->statGC.z, car->DynGCg.pos.z - RtTrackHeightL(&trkPos)); if (height_difference > 0) { continue; } else if (height_difference < min_height_difference) { min_height_difference = height_difference; } // get the track normal n_g for the wheel //RtTrackSurfaceNormalL(&(wheel->trkPos), &normal); normal = wheel->normal; // transform it to local coordinates: n = q' n_g q QuatInverseRotate (normal, car->posQuat, rel_normal); // calculate the velocity of the corner #if 1 // option 1: just use the car velocity // this works fine when more than 1 corner hits at the same time tdble cvx = (car->DynGCg.vel.x); tdble cvy = (car->DynGCg.vel.y); tdble cvz = (car->DynGCg.vel.z); #else // option 2: add the hopefully correctly calculated corner velocity // to use this, the code must take special consideration // of multiple corner hits, or we can just update corner // velocity and position after every hit tdble cvx = corner->vel.x; tdble cvy = corner->vel.y; tdble cvz = corner->vel.z; // NOTE: this last one is an approximation, sadly #endif // option 3: recalculate the velocity // TODO // c = K n'v, v|n = cn dotProd = (cvx * normal.x + cvy * normal.y + cvz * normal.z); if (dotProd < 0) { //tdble dotProd2 = 0.25*dotProd *car->mass / SimDeltaTime; tdble mu = 0.5; // normal tdble nx = normal.x; tdble ny = normal.y; tdble nz = normal.z; #ifdef DEBUG_COLLIDE_Z printf("CollideZ: %d %d %f\n N = (%f %f %f)\n", i, j, dotProd, nx, ny, nz); #endif // veolcity projected to normal tdble vPx = nx * cvx; tdble vPy = ny * cvy; tdble vPz = nz * cvz; //tdble vP = sqrt(vPx*vPx + vPy*vPy + vPz*vPz); #ifdef DEBUG_COLLIDE_Z printf(" V = (%.2f %.2f %.2f) -(P)-> (%.2f %.2f %.2f)\n", cvx, cvy, cvz, vPx, vPy, vPz); #endif // veolcity projected to tangent plane tdble vQx = cvx - vPx; tdble vQy = cvy - vPy; tdble vQz = cvz - vPz; tdble vQ = sqrt(vQx*vQx + vQy*vQy + vQz*vQz); // v|n = n'v'n = cn // reaction force - by definition has the // same direction as the normal t3Dd forces; forces.x = - dotProd * nx; forces.y = - dotProd * ny; forces.z = - dotProd * nz; tdble dP3 = (float)(dotProd * mu / (0.001 + vQ)); t3Dd friction; friction.x = vQx * dP3; friction.y = vQy * dP3; friction.z = vQz * dP3; #ifdef DEBUG_COLLIDE_Z printf (" Fn= %.2f %.2f %.2f\n", forces.x, forces.y, forces.z); printf (" Ff= %.2f %.2f %.2f\n", friction.x, friction.y, friction.z); #endif // propagate damage to deformation //car->normal.x = nx * dmg; //car->normal.y = ny * dmg; //car->normal.z = nz * dmg; // change car physical state // TODO: duplicate code: fix // Calculate change in rotational momentum. // ---------------------------------------- // Put the impulse in a 3d vector sgVec3 impulse = {(forces.x + friction.x), (forces.y + friction.y), forces.z + friction.z}; #ifdef DEBUG_COLLIDE_Z printf (" F = %.2f %.2f %.2f\n", impulse[SG_X], impulse[SG_Y], impulse[SG_Z]); #endif // rotate it to the car's frame sgRotateVecQuat (impulse, car->posQuat); //tdble E_prev = SimCarEnergy(car); // add to local-frame speed car->DynGC.vel.x += impulse[SG_X]; car->DynGC.vel.y += impulse[SG_Y]; car->DynGC.vel.z += impulse[SG_Z]; // Put the point of impact in a 3d vector sgVec3 v = {orig.x, orig.y, orig.z}; #ifdef DEBUG_COLLIDE_Z printf (" F' = %.2f %.2f %.2f @ %.2f %.2f %.2f (%.2f %.2f)\n", impulse[SG_X], impulse[SG_Y], impulse[SG_Z], v[SG_X], v[SG_Y], v[SG_Z], orig.x, orig.y); #endif // Calculate moments tdble Mx = + impulse[SG_Z] * v[SG_Y] - impulse[SG_Y] * v[SG_Z]; tdble My = - impulse[SG_Z] * v[SG_X] + impulse[SG_X] * v[SG_Z]; tdble Mz = - impulse[SG_X] * v[SG_Y] + impulse[SG_Y] * v[SG_X]; // Add moments to rotational inertia tdble rot_mom_scale = 0.25f*car->mass; #ifdef DEBUG_COLLIDE_Z printf (" J = (%f %f %f)\n", car->rot_mom[SG_X], car->rot_mom[SG_Y], car->rot_mom[SG_Z]); #endif car->rot_mom[SG_X] -= rot_mom_scale * Mx;// * car->Iinv.x; car->rot_mom[SG_Y] -= rot_mom_scale * My;// * car->Iinv.y; car->rot_mom[SG_Z] -= rot_mom_scale * Mz;// * car->Iinv.z; for (int i=0; i<3; i++) { if (fabs(car->rot_mom[i]) >500.0) { //printf ("rot_mom: %f\n", (car->rot_mom[i])); car->rot_mom[i] = (float)(250*SIGN(car->rot_mom[i])); } } #ifdef DEBUG_COLLIDE_Z printf (" M = (%f %f %f), s = %f\n", Mx, My, Mz, rot_mom_scale); printf (" -> J = (%f %f %f)\n", car->rot_mom[SG_X], car->rot_mom[SG_Y], car->rot_mom[SG_Z]); #endif // transform velocity to global frame if (1) { t3Dd original; t3Dd updated; original.x = car->DynGC.vel.x; original.y = car->DynGC.vel.y; original.z = car->DynGC.vel.z; QuatRotate(original, car->posQuat, updated); car->DynGCg.vel.x = updated.x; car->DynGCg.vel.y = updated.y; car->DynGCg.vel.z = updated.z; // Translate angular momentum to angular velocity // NOTE: This translation is done again in SimCarAddAngularVelocity() car->DynGCg.vel.ax = car->DynGC.vel.ax = -2.0f*car->rot_mom[SG_X] * car->Iinv.x; car->DynGCg.vel.ay = car->DynGC.vel.ay = -2.0f*car->rot_mom[SG_Y] * car->Iinv.y; car->DynGCg.vel.az = car->DynGC.vel.az = -2.0f*car->rot_mom[SG_Z] * car->Iinv.z; } SimCarUpdateCornerPos(car); SimCarLimitEnergy(car, E_prev); collide = true; } if (dotProd < 0) { // should be this way.. if (dotProd <-5.0) { // if it's hard, do a damage thing static tdble WHEEL_ROT_DAMAGE = 0.001f; static tdble WHEEL_BENT_DAMAGE = 0.01f; static tdble WHEEL_DAMAGE_LIMIT = 0.25f; static tdble SUSP_DAMAGE_CONST = 1.0f; static tdble SUSP_DAMAGE = 0.1f; car->collision |= 16; wheel->rotational_damage_x -= dotProd*WHEEL_ROT_DAMAGE*urandom(); wheel->rotational_damage_z -= dotProd*WHEEL_ROT_DAMAGE*urandom(); wheel->bent_damage_x += (float)(WHEEL_BENT_DAMAGE*(urandom()-0.5)); wheel->bent_damage_z += (float)(WHEEL_BENT_DAMAGE*(urandom()-0.5)); if (wheel->rotational_damage_x > WHEEL_DAMAGE_LIMIT) { wheel->rotational_damage_x = WHEEL_DAMAGE_LIMIT; } if (wheel->rotational_damage_z > WHEEL_DAMAGE_LIMIT) { wheel->rotational_damage_z = WHEEL_DAMAGE_LIMIT; } if (car->options->suspension_damage) { SimSuspDamage (&wheel->susp, SUSP_DAMAGE*dotProd + SUSP_DAMAGE_CONST); } car->collision |= 1; } car->collision |= 1; car->collision |= 8; if (wheel->susp.state & SIM_SUSP_OVERCOMP) { car->collision |= 1; } if ((car->carElt->_state & RM_CAR_STATE_FINISH) == 0) { car->dammage += (int)(wheel->trkPos.seg->surface->kDammage * fabs(dotProd) * simDammageFactor[car->carElt->_skillLevel]); } } } // for j //if (wheel->state & SIM_SUSP_COMP) { //car->DynGCg.pos.z += wheel->susp.spring.packers - wheel->rideHeight; //} } // for i car->DynGCg.pos.z -= MIN(gc_height_difference, min_height_difference); gc_height_difference = car->DynGCg.pos.z - RtTrackHeightL(&(car->trkPos)); if (gc_height_difference < 0) { car->DynGCg.pos.z -= gc_height_difference; } else if (gc_height_difference > 100) { car->DynGCg.pos.z = RtTrackHeightL(&(car->trkPos)) + 100; car->DynGCg.vel.x = car->DynGCg.vel.y = car->DynGCg.vel.z = car->DynGC.vel.x = car->DynGC.vel.y = car->DynGC.vel.z = 0.0; // Translate angular momentum to angular velocity // NOTE: This translation is done again in SimCarAddAngularVelocity() car->DynGCg.vel.ax = car->DynGC.vel.ax = car->DynGCg.vel.ay = car->DynGC.vel.ay = car->DynGCg.vel.az = car->DynGC.vel.az = 0.0; car->rot_mom[0] = car->rot_mom[1] = car->rot_mom[2] = 0.0; } car->DynGC.pos.z = car->DynGCg.pos.z; if (collide) { SimCarLimitEnergy(car, energy_restitution * E_prev); car->collide_timer = 0.0; } }
/* Drive during race. */ static void drive(int index, tCarElt* car, tSituation *s) { total_tics[index]++; char line[UDP_MSGLEN]; #ifdef __PRINT_RACE_RESULTS__ bestLap[index]=car->_bestLapTime; damages[index]=car->_dammage; totalTime[index]=car->_timeBehindLeader; #endif #ifdef __DISABLE_RESTART__ if (RESTARTING[index]==1) { clientAddressLength[index] = sizeof(clientAddress[index]); // Set line to all zeroes memset(line, 0x0, 101); if (recvfrom(listenSocket[index], line, 100, 0, (struct sockaddr *) &clientAddress[index], &clientAddressLength[index]) < 0) { std::cerr << "Error: problem in receiving from the listen socket"; exit(1); } #ifdef __UDP_SERVER_VERBOSE__ // show the client's IP address std::cout << " from " << inet_ntoa(clientAddress[index].sin_addr); // show the client's port number. std::cout << ":" << ntohs(clientAddress[index].sin_port) << "\n"; // Show the line std::cout << " Received: " << line << "\n"; #endif // compare received string with the ID if (strncmp(line,UDP_ID,3)==0) { #ifdef __UDP_SERVER_VERBOSE__ std::cout << "IDENTIFIED" << std::endl; #endif // char line[UDP_MSGLEN]; sprintf(line,"***identified***"); // Sending the car state to the client if (sendto(listenSocket[index], line, strlen(line) + 1, 0, (struct sockaddr *) &clientAddress[index], sizeof(clientAddress[index])) < 0) std::cerr << "Error: cannot send identification message"; RESTARTING[index]=0; } } #endif // local variables for UDP struct timeval timeVal; fd_set readSet; // computing distance to middle float dist_to_middle = 2*car->_trkPos.toMiddle/(car->_trkPos.seg->width); // computing the car angle wrt the track axis float angle = RtTrackSideTgAngleL(&(car->_trkPos)) - car->_yaw; NORM_PI_PI(angle); // normalize the angle between -PI and + PI //Update focus sensors' angle for (int i = 0; i < 5; ++i) { focusSens[index]->setSensor(i,(car->_focusCmd)+i-2,200); } // update the value of track sensors only as long as the car is inside the track float trackSensorOut[19]; float focusSensorOut[5];//ML if (dist_to_middle<=1.0 && dist_to_middle >=-1.0 ) { trackSens[index]->sensors_update(); for (int i = 0; i < 19; ++i) { trackSensorOut[i] = trackSens[index]->getSensorOut(i); if (getNoisy()) trackSensorOut[i] *= normRand(1,__NOISE_STD__); } focusSens[index]->sensors_update();//ML if ((car->_focusCD <= car->_curLapTime + car->_curTime)//ML Only send focus sensor reading if cooldown is over && (car->_focusCmd != 360))//ML Only send focus reading if requested by client {//ML for (int i = 0; i < 5; ++i) { focusSensorOut[i] = focusSens[index]->getSensorOut(i); if (getNoisy()) focusSensorOut[i] *= normRand(1,__FOCUS_NOISE_STD__); } car->_focusCD = car->_curLapTime + car->_curTime + 1.0;//ML Add cooldown [seconds] }//ML else//ML {//ML for (int i = 0; i < 5; ++i)//ML focusSensorOut[i] = -1;//ML During cooldown send invalid focus reading }//ML } else { for (int i = 0; i < 19; ++i) { trackSensorOut[i] = -1; } for (int i = 0; i < 5; ++i) { focusSensorOut[i] = -1; } } // update the value of opponent sensors float oppSensorOut[36]; oppSens[index]->sensors_update(s); for (int i = 0; i < 36; ++i) { oppSensorOut[i] = oppSens[index]->getObstacleSensorOut(i); if (getNoisy()) oppSensorOut[i] *= normRand(1,__OPP_NOISE_STD__); } float wheelSpinVel[4]; for (int i=0; i<4; ++i) { wheelSpinVel[i] = car->_wheelSpinVel(i); } if (prevDist[index]<0) { prevDist[index] = car->race.distFromStartLine; } float curDistRaced = car->race.distFromStartLine - prevDist[index]; prevDist[index] = car->race.distFromStartLine; if (curDistRaced>100) { curDistRaced -= curTrack->length; } if (curDistRaced<-100) { curDistRaced += curTrack->length; } distRaced[index] += curDistRaced; float totdist = curTrack->length * (car->race.laps -1) + car->race.distFromStartLine; // std::cerr << "totraced: " << totdist << std::endl; /********************************************************************** ****************** Building state string ***************************** **********************************************************************/ string stateString; stateString = SimpleParser::stringify("angle", angle); stateString += SimpleParser::stringify("curLapTime", float(car->_curLapTime)); stateString += SimpleParser::stringify("damage", ( getDamageLimit() ? car->_dammage : car->_fakeDammage ) ); stateString += SimpleParser::stringify("distFromStart", car->race.distFromStartLine); stateString += SimpleParser::stringify("totalDistFromStart", totdist); stateString += SimpleParser::stringify("distRaced", distRaced[index]); stateString += SimpleParser::stringify("fuel", car->_fuel); stateString += SimpleParser::stringify("gear", car->_gear); stateString += SimpleParser::stringify("lastLapTime", float(car->_lastLapTime)); stateString += SimpleParser::stringify("opponents", oppSensorOut, 36); stateString += SimpleParser::stringify("racePos", car->race.pos); stateString += SimpleParser::stringify("rpm", car->_enginerpm*10); stateString += SimpleParser::stringify("speedX", float(car->_speed_x * 3.6)); stateString += SimpleParser::stringify("speedY", float(car->_speed_y * 3.6)); stateString += SimpleParser::stringify("speedZ", float(car->_speed_z * 3.6)); stateString += SimpleParser::stringify("track", trackSensorOut, 19); stateString += SimpleParser::stringify("trackPos", dist_to_middle); stateString += SimpleParser::stringify("wheelSpinVel", wheelSpinVel, 4); stateString += SimpleParser::stringify("z", car->_pos_Z - RtTrackHeightL(&(car->_trkPos))); stateString += SimpleParser::stringify("focus", focusSensorOut, 5);//ML //GIUSE - VISION HERE! // printf("size: %d\n",car->vision->imgsize); if( getVision() ){ // std::cout << car->vision->imgsize << std::endl; stateString += SimpleParser::stringify("img", car->vision->img, car->vision->imgsize); } // for(int i=0; i < car->vision->imgsize; i++){ // std::cout << (int)car->vision->img[i] << " "; // } // GIUSE - that's UGLY, can we stay coherent with either char* or string?? // char line[UDP_MSGLEN]; // memset(line, 0x0,UDP_MSGLEN ); // // sprintf(line,"%s",stateString.c_str()); if (RESTARTING[index]==0) { #ifdef __UDP_SERVER_VERBOSE__ std::cout << "Sending: " << stateString.c_str() << std::endl; std::cout << "Sending: " << stateString.c_str() << std::endl; #endif #ifdef __STEP_LIMIT__ if (total_tics[index]>__STEP_LIMIT__) { RESTARTING[index] = 1; car->RESTART=1; char fileName[200]; sprintf(fileName,"%s.txt",trackName); printf("%s.txt\n",trackName); FILE *f = fopen (fileName,"a"); printf("Dist_raced %lf\n",distRaced[index]); fprintf(f,"Dist_raced %lf\n",distRaced[index]); fclose(f); return; } #endif // Sending the car state to the client // if (sendto(listenSocket[index], line, strlen(line) + 1, 0, if (sendto(listenSocket[index], stateString.c_str(), stateString.length() + 1, 0, (struct sockaddr *) &clientAddress[index], sizeof(clientAddress[index])) < 0) std::cerr << "Error: cannot send car state"; // Set timeout for client answer FD_ZERO(&readSet); FD_SET(listenSocket[index], &readSet); timeVal.tv_sec = 0; timeVal.tv_usec = UDP_TIMEOUT; memset(line, 0x0,UDP_MSGLEN ); // GIUSE - BUG, THERE WAS A 1000 HARDCODED if (select(listenSocket[index]+1, &readSet, NULL, NULL, &timeVal)) { // Read the client controller action // memset(line, 0x0,UDP_MSGLEN ); // Zero out the buffer. GIUSE - already done int numRead = recv(listenSocket[index], line, UDP_MSGLEN, 0); if (numRead < 0) { std::cerr << "Error, cannot get any response from the client!"; CLOSE(listenSocket[index]); exit(1); } #ifdef __UDP_SERVER_VERBOSE__ std::cout << "Received: " << line << std::endl; #endif std::string lineStr(line); CarControl carCtrl(lineStr); if (carCtrl.getMeta()==RACE_RESTART) { RESTARTING[index] = 1; #ifdef __DISABLE_RESTART__ // char line[UDP_MSGLEN]; memset(line, 0x0,UDP_MSGLEN ); sprintf(line,"***restart***"); // Sending the car state to the client if (sendto(listenSocket[index], line, strlen(line) + 1, 0, (struct sockaddr *) &clientAddress[index], sizeof(clientAddress[index])) < 0) std::cerr << "Error: cannot send restart message"; #else car->RESTART=1; #endif } // Set controls command and store them in variables oldAccel[index] = car->_accelCmd = carCtrl.getAccel(); oldBrake[index] = car->_brakeCmd = carCtrl.getBrake(); oldGear[index] = car->_gearCmd = carCtrl.getGear(); oldSteer[index] = car->_steerCmd = carCtrl.getSteer(); oldClutch[index] = car->_clutchCmd = carCtrl.getClutch(); oldFocus[index] = car->_focusCmd = carCtrl.getFocus();//ML } else { //#ifdef __UDP_SERVER_VERBOSE__ std::cout << "Timeout for client answer\n"; //#endif // If no new controls are availables uses old ones... car->_accelCmd = oldAccel[index]; car->_brakeCmd = oldBrake[index]; car->_gearCmd = oldGear[index]; car->_steerCmd = oldSteer[index]; car->_clutchCmd = oldClutch[index]; car->_focusCmd = oldFocus[index];//ML } } else { car->_accelCmd = oldAccel[index]; car->_brakeCmd = oldBrake[index]; car->_gearCmd = oldGear[index]; car->_steerCmd = oldSteer[index]; car->_clutchCmd = oldClutch[index]; car->_focusCmd = oldFocus[index];//ML } }
static void RemoveCar(tCar *car, tSituation *s) { int i; tCarElt *carElt; tTrkLocPos trkPos; int trkFlag; tdble travelTime; tdble dang; static tdble PULL_Z_OFFSET = 3.0; static tdble PULL_SPD = 0.5; carElt = car->carElt; if (carElt->_state & RM_CAR_STATE_PULLUP) { carElt->_pos_Z += car->restPos.vel.z * SimDeltaTime; carElt->_yaw += car->restPos.vel.az * SimDeltaTime; carElt->_roll += car->restPos.vel.ax * SimDeltaTime; carElt->_pitch += car->restPos.vel.ay * SimDeltaTime; sgMakeCoordMat4(carElt->pub.posMat, carElt->_pos_X, carElt->_pos_Y, carElt->_pos_Z - carElt->_statGC_z, (float) RAD2DEG(carElt->_yaw), (float) RAD2DEG(carElt->_roll), (float) RAD2DEG(carElt->_pitch)); if (carElt->_pos_Z > (car->restPos.pos.z + PULL_Z_OFFSET)) { carElt->_state &= ~RM_CAR_STATE_PULLUP; carElt->_state |= RM_CAR_STATE_PULLSIDE; // Moved pullside velocity computation down due to floating point error accumulation. } return; } if (carElt->_state & RM_CAR_STATE_PULLSIDE) { // Recompute speed to avoid missing the parking point due to error accumulation (the pos might be // in the 0-10000 range, depending on the track and vel*dt is around 0-0.001, so basically all // but the most significant digits are lost under bad conditions, happens e.g on e-track-4). // Should not lead to a division by zero because the pullside process stops if the car is within // [0.5, 0.5]. Do not move it back. travelTime = DIST(car->restPos.pos.x, car->restPos.pos.y, carElt->_pos_X, carElt->_pos_Y) / PULL_SPD; car->restPos.vel.x = (car->restPos.pos.x - carElt->_pos_X) / travelTime; car->restPos.vel.y = (car->restPos.pos.y - carElt->_pos_Y) / travelTime; carElt->_pos_X += car->restPos.vel.x * SimDeltaTime; carElt->_pos_Y += car->restPos.vel.y * SimDeltaTime; sgMakeCoordMat4(carElt->pub.posMat, carElt->_pos_X, carElt->_pos_Y, carElt->_pos_Z - carElt->_statGC_z, (float) RAD2DEG(carElt->_yaw), (float) RAD2DEG(carElt->_roll), (float) RAD2DEG(carElt->_pitch)); if ((fabs(car->restPos.pos.x - carElt->_pos_X) < 0.5) && (fabs(car->restPos.pos.y - carElt->_pos_Y) < 0.5)) { carElt->_state &= ~RM_CAR_STATE_PULLSIDE; carElt->_state |= RM_CAR_STATE_PULLDN; } return; } if (carElt->_state & RM_CAR_STATE_PULLDN) { carElt->_pos_Z -= car->restPos.vel.z * SimDeltaTime; sgMakeCoordMat4(carElt->pub.posMat, carElt->_pos_X, carElt->_pos_Y, carElt->_pos_Z - carElt->_statGC_z, (float) RAD2DEG(carElt->_yaw), (float) RAD2DEG(carElt->_roll), (float) RAD2DEG(carElt->_pitch)); if (carElt->_pos_Z < car->restPos.pos.z) { carElt->_state &= ~RM_CAR_STATE_PULLDN; carElt->_state |= RM_CAR_STATE_OUT; } return; } if (carElt->_state & (RM_CAR_STATE_NO_SIMU & ~RM_CAR_STATE_PIT)) { return; } if (carElt->_state & RM_CAR_STATE_PIT) { if ((s->_maxDammage) && (car->dammage > s->_maxDammage)) { // Broken during pit stop. carElt->_state &= ~RM_CAR_STATE_PIT; carElt->_pit->pitCarIndex = TR_PIT_STATE_FREE; } else { return; } } if ((s->_maxDammage) && (car->dammage > s->_maxDammage)) { carElt->_state |= RM_CAR_STATE_BROKEN; } else { carElt->_state |= RM_CAR_STATE_OUTOFGAS; } carElt->_gear = car->transmission.gearbox.gear = 0; carElt->_enginerpm = car->engine.rads = 0; if (!(carElt->_state & RM_CAR_STATE_DNF)) { if (fabs(carElt->_speed_x) > 1.0) { return; } } carElt->_state |= RM_CAR_STATE_PULLUP; // RM_CAR_STATE_NO_SIMU evaluates to > 0 from here, so we remove the car from the // collision detection. SimCollideRemoveCar(car, s->_ncars); carElt->priv.collision = car->collision = 0; for(i = 0; i < 4; i++) { carElt->_skid[i] = 0; carElt->_wheelSpinVel(i) = 0; carElt->_brakeTemp(i) = 0; } carElt->pub.DynGC = car->DynGC; carElt->_speed_x = 0; // Compute the target zone for the wrecked car. trkPos = car->trkPos; if (trkPos.toRight > trkPos.seg->width / 2.0) { while (trkPos.seg->lside != 0) { trkPos.seg = trkPos.seg->lside; } trkPos.toLeft = -3.0; trkFlag = TR_TOLEFT; } else { while (trkPos.seg->rside != 0) { trkPos.seg = trkPos.seg->rside; } trkPos.toRight = -3.0; trkFlag = TR_TORIGHT; } trkPos.type = TR_LPOS_SEGMENT; RtTrackLocal2Global(&trkPos, &(car->restPos.pos.x), &(car->restPos.pos.y), trkFlag); car->restPos.pos.z = RtTrackHeightL(&trkPos) + carElt->_statGC_z; car->restPos.pos.az = RtTrackSideTgAngleL(&trkPos); car->restPos.pos.ax = 0; car->restPos.pos.ay = 0; car->restPos.vel.z = PULL_SPD; travelTime = (car->restPos.pos.z + PULL_Z_OFFSET - carElt->_pos_Z) / car->restPos.vel.z; dang = car->restPos.pos.az - carElt->_yaw; FLOAT_NORM_PI_PI(dang); car->restPos.vel.az = dang / travelTime; dang = car->restPos.pos.ax - carElt->_roll; FLOAT_NORM_PI_PI(dang); car->restPos.vel.ax = dang / travelTime; dang = car->restPos.pos.ay - carElt->_pitch; FLOAT_NORM_PI_PI(dang); car->restPos.vel.ay = dang / travelTime; }