1 /***************************************************************************
2
3 file : car.cpp
4 created : Sun Mar 19 00:05:43 CET 2000
5 copyright : (C) 2000-2014 by Eric Espie, Bernhard Wymann
6 email : torcs@free.fr
7 version : $Id: car.cpp,v 1.24.2.5 2016/05/16 22:53:07 berniw Exp $
8
9 ***************************************************************************/
10
11 /***************************************************************************
12 * *
13 * This program is free software; you can redistribute it and/or modify *
14 * it under the terms of the GNU General Public License as published by *
15 * the Free Software Foundation; either version 2 of the License, or *
16 * (at your option) any later version. *
17 * *
18 ***************************************************************************/
19
20
21
22 #include <stdio.h>
23
24 #include "sim.h"
25
26 const tdble aMax = 1.04f; // 60 degrees DOF limit
27
28 void
SimCarConfig(tCar * car)29 SimCarConfig(tCar *car)
30 {
31 void *hdle = car->params;
32 tdble k;
33 tdble w;
34 tdble gcfrl, gcrrl, gcfr;
35 tdble wf0, wr0;
36 tdble overallwidth;
37 int i;
38 tCarElt *carElt = car->carElt;
39
40 car->dimension.x = GfParmGetNum(hdle, SECT_CAR, PRM_LEN, (char*)NULL, 4.7f);
41 car->dimension.y = GfParmGetNum(hdle, SECT_CAR, PRM_WIDTH, (char*)NULL, 1.9f);
42 overallwidth = GfParmGetNum(hdle, SECT_CAR, PRM_OVERALLWIDTH, (char*)NULL, car->dimension.y);
43 car->dimension.z = GfParmGetNum(hdle, SECT_CAR, PRM_HEIGHT, (char*)NULL, 1.2f);
44 car->mass = GfParmGetNum(hdle, SECT_CAR, PRM_MASS, (char*)NULL, 1500);
45 car->Minv = 1.0 / car->mass;
46 gcfr = GfParmGetNum(hdle, SECT_CAR, PRM_FRWEIGHTREP, (char*)NULL, .5);
47 gcfrl = GfParmGetNum(hdle, SECT_CAR, PRM_FRLWEIGHTREP, (char*)NULL, .5);
48 gcrrl = GfParmGetNum(hdle, SECT_CAR, PRM_RRLWEIGHTREP, (char*)NULL, .5);
49 car->statGC.y = - (gcfr * gcfrl + (1 - gcfr) * gcrrl) * car->dimension.y + car->dimension.y / 2.0;
50 car->statGC.z = GfParmGetNum(hdle, SECT_CAR, PRM_GCHEIGHT, (char*)NULL, .5);
51
52 car->tank = GfParmGetNum(hdle, SECT_CAR, PRM_TANK, (char*)NULL, 80);
53 car->fuel = GfParmGetNum(hdle, SECT_CAR, PRM_FUEL, (char*)NULL, 80);
54 k = GfParmGetNum(hdle, SECT_CAR, PRM_CENTR, (char*)NULL, 1.0);
55 carElt->_drvPos_x = GfParmGetNum(hdle, SECT_DRIVER, PRM_XPOS, (char*)NULL, 0.0);
56 carElt->_drvPos_y = GfParmGetNum(hdle, SECT_DRIVER, PRM_YPOS, (char*)NULL, 0.0);
57 carElt->_drvPos_z = GfParmGetNum(hdle, SECT_DRIVER, PRM_ZPOS, (char*)NULL, 0.0);
58 carElt->_bonnetPos_x = GfParmGetNum(hdle, SECT_BONNET, PRM_XPOS, (char*)NULL, carElt->_drvPos_x);
59 carElt->_bonnetPos_y = GfParmGetNum(hdle, SECT_BONNET, PRM_YPOS, (char*)NULL, carElt->_drvPos_y);
60 carElt->_bonnetPos_z = GfParmGetNum(hdle, SECT_BONNET, PRM_ZPOS, (char*)NULL, carElt->_drvPos_z);
61
62 if (car->fuel > car->tank) {
63 car->fuel = car->tank;
64 }
65 k = k * k;
66 car->Iinv.x = 12.0 / (car->mass * (car->dimension.y * car->dimension.y + car->dimension.z * car->dimension.z));
67 car->Iinv.y = 12.0 / (car->mass * (car->dimension.x * car->dimension.x + car->dimension.z * car->dimension.z));
68 car->Iinv.z = 12.0 / (car->mass * (car->dimension.y * car->dimension.y + k * car->dimension.x * car->dimension.x));
69
70 /* configure components */
71 w = car->mass * G;
72
73 wf0 = w * gcfr;
74 wr0 = w * (1 - gcfr);
75
76 car->wheel[FRNT_RGT].weight0 = wf0 * gcfrl;
77 car->wheel[FRNT_LFT].weight0 = wf0 * (1 - gcfrl);
78 car->wheel[REAR_RGT].weight0 = wr0 * gcrrl;
79 car->wheel[REAR_LFT].weight0 = wr0 * (1 - gcrrl);
80
81 for (i = 0; i < 2; i++) {
82 SimAxleConfig(car, i);
83 }
84
85 for (i = 0; i < 4; i++) {
86 SimWheelConfig(car, i);
87 }
88
89 /* Set the origin to GC */
90 car->wheelbase = car->wheeltrack = 0;
91 car->statGC.x = car->wheel[FRNT_RGT].staticPos.x * gcfr + car->wheel[REAR_RGT].staticPos.x * (1 - gcfr);
92
93 SimEngineConfig(car);
94 SimTransmissionConfig(car);
95 SimSteerConfig(car);
96 SimBrakeSystemConfig(car);
97 SimAeroConfig(car);
98 for (i = 0; i < 2; i++) {
99 SimWingConfig(car, i);
100 }
101
102 carElt->_dimension = car->dimension;
103 carElt->_statGC = car->statGC;
104 carElt->_tank = car->tank;
105 for (i = 0; i < 4; i++) {
106 carElt->priv.wheel[i].relPos = car->wheel[i].relPos;
107 }
108
109 for (i = 0; i < 4; i++) {
110 car->wheel[i].staticPos.x -= car->statGC.x;
111 car->wheel[i].staticPos.y -= car->statGC.y;
112 }
113 car->wheelbase = (car->wheel[FRNT_RGT].staticPos.x
114 + car->wheel[FRNT_LFT].staticPos.x
115 - car->wheel[REAR_RGT].staticPos.x
116 - car->wheel[REAR_LFT].staticPos.x) / 2.0;
117 car->wheeltrack = (-car->wheel[REAR_LFT].staticPos.y
118 - car->wheel[FRNT_LFT].staticPos.y
119 + car->wheel[FRNT_RGT].staticPos.y
120 + car->wheel[REAR_RGT].staticPos.y) / 2.0;
121
122 /* set corners pos */
123 car->corner[FRNT_RGT].pos.x = car->dimension.x * .5 - car->statGC.x;
124 car->corner[FRNT_RGT].pos.y = - overallwidth * .5 - car->statGC.y;
125 car->corner[FRNT_RGT].pos.z = 0;
126
127 car->corner[FRNT_LFT].pos.x = car->dimension.x * .5 - car->statGC.x;
128 car->corner[FRNT_LFT].pos.y = overallwidth * .5 - car->statGC.y;
129 car->corner[FRNT_LFT].pos.z = 0;
130
131 car->corner[REAR_RGT].pos.x = - car->dimension.x * .5 - car->statGC.x;
132 car->corner[REAR_RGT].pos.y = - overallwidth * .5 - car->statGC.y;
133 car->corner[REAR_RGT].pos.z = 0;
134
135 car->corner[REAR_LFT].pos.x = - car->dimension.x * .5 - car->statGC.x;
136 car->corner[REAR_LFT].pos.y = overallwidth * .5 - car->statGC.y;
137 car->corner[REAR_LFT].pos.z = 0;
138 }
139
140
141 static void
SimCarUpdateForces(tCar * car)142 SimCarUpdateForces(tCar *car)
143 {
144 tForces F;
145 int i;
146 tdble m, w, minv;
147 tdble SinTheta;
148 tdble Cosz, Sinz;
149 tdble v, R, Rv, Rm, Rx, Ry;
150
151 Cosz = car->Cosz = cos(car->DynGCg.pos.az);
152 Sinz = car->Sinz = sin(car->DynGCg.pos.az);
153
154 car->preDynGC = car->DynGCg;
155
156 /* total mass */
157 m = car->mass + car->fuel;
158 minv = 1.0 / m;
159 w = -m * G;
160
161 /* Weight */
162 SinTheta = (-car->wheel[FRNT_RGT].zRoad - car->wheel[FRNT_LFT].zRoad
163 + car->wheel[REAR_RGT].zRoad + car->wheel[REAR_LFT].zRoad) / (2.0 * car->wheelbase);
164 F.F.x = -w * SinTheta;
165 SinTheta = (-car->wheel[FRNT_RGT].zRoad - car->wheel[REAR_RGT].zRoad
166 + car->wheel[FRNT_LFT].zRoad + car->wheel[REAR_LFT].zRoad) / (2.0 * car->wheeltrack);
167 F.F.y = -w * SinTheta;
168 F.F.z = w; /* not 3D */
169 F.M.x = F.M.y = F.M.z = 0;
170
171 /* Wheels */
172 for (i = 0; i < 4; i++) {
173 /* forces */
174 F.F.x += car->wheel[i].forces.x;
175 F.F.y += car->wheel[i].forces.y;
176 F.F.z += car->wheel[i].forces.z;
177
178 /* moments */
179 F.M.x += car->wheel[i].forces.z * car->wheel[i].staticPos.y +
180 car->wheel[i].forces.y * car->wheel[i].rollCenter;
181 // Eventually TODO: activate fix below and make all cars/robots fit.
182 //car->wheel[i].forces.y * (car->statGC.z + car->wheel[i].rideHeight);
183 F.M.y -= car->wheel[i].forces.z * car->wheel[i].staticPos.x +
184 car->wheel[i].forces.x * (car->statGC.z + car->wheel[i].rideHeight);
185 F.M.z += -car->wheel[i].forces.x * car->wheel[i].staticPos.y +
186 car->wheel[i].forces.y * car->wheel[i].staticPos.x;
187 }
188
189 /* Aero Drag */
190 F.F.x += car->aero.drag;
191
192 /* Wings & Aero Downforce */
193 for (i = 0; i < 2; i++) {
194 /* forces */
195 F.F.z += car->wing[i].forces.z + car->aero.lift[i];
196 F.F.x += car->wing[i].forces.x;
197 /* moments */
198 F.M.y -= car->wing[i].forces.z * car->wing[i].staticPos.x + car->wing[i].forces.x * car->wing[i].staticPos.z;
199 F.M.y -= car->aero.lift[i] * (car->axle[i].xpos - car->statGC.x);
200 }
201
202 /* Rolling Resistance */
203 v = sqrt(car->DynGCg.vel.x * car->DynGCg.vel.x + car->DynGCg.vel.y * car->DynGCg.vel.y);
204 R = 0;
205 for (i = 0; i < 4; i++) {
206 R += car->wheel[i].rollRes;
207 }
208 if (v > 0.00001) {
209 Rv = R / v;
210 if ((Rv * minv * SimDeltaTime) > v) {
211 Rv = v * m / SimDeltaTime;
212 }
213 } else {
214 Rv = 0;
215 }
216 Rx = Rv * car->DynGCg.vel.x;
217 Ry = Rv * car->DynGCg.vel.y;
218
219 if ((R * car->wheelbase / 2.0 * car->Iinv.z) > fabs(car->DynGCg.vel.az)) {
220 Rm = car->DynGCg.vel.az / car->Iinv.z;
221 } else {
222 Rm = SIGN(car->DynGCg.vel.az) * R * car->wheelbase / 2.0;
223 }
224
225 /* compute accelerations */
226 car->DynGC.acc.x = F.F.x * minv;
227 car->DynGC.acc.y = F.F.y * minv;
228 car->DynGC.acc.z = F.F.z * minv;
229
230 car->DynGCg.acc.x = (F.F.x * Cosz - F.F.y * Sinz - Rx) * minv;
231 car->DynGCg.acc.y = (F.F.x * Sinz + F.F.y * Cosz - Ry) * minv;
232 car->DynGCg.acc.z = car->DynGC.acc.z;
233
234 car->DynGCg.acc.ax = car->DynGC.acc.ax = F.M.x * car->Iinv.x;
235 car->DynGCg.acc.ay = car->DynGC.acc.ay = F.M.y * car->Iinv.y;
236 car->DynGCg.acc.az = car->DynGC.acc.az = (F.M.z - Rm) * car->Iinv.z;
237 }
238
239 static void
SimCarUpdateSpeed(tCar * car)240 SimCarUpdateSpeed(tCar *car)
241 {
242 tdble Cosz, Sinz;
243
244 Cosz = car->Cosz;
245 Sinz = car->Sinz;
246
247 car->DynGCg.vel.x += car->DynGCg.acc.x * SimDeltaTime;
248 car->DynGCg.vel.y += car->DynGCg.acc.y * SimDeltaTime;
249 car->DynGCg.vel.z += car->DynGCg.acc.z * SimDeltaTime;
250
251 car->DynGCg.vel.ax += car->DynGCg.acc.ax * SimDeltaTime;
252 car->DynGCg.vel.ay += car->DynGCg.acc.ay * SimDeltaTime;
253 car->DynGCg.vel.az += car->DynGCg.acc.az * SimDeltaTime;
254
255 /* spin limitation */
256 if (fabs(car->DynGCg.vel.az) > 9.0) {
257 car->DynGCg.vel.az = SIGN(car->DynGCg.vel.az) * 9.0;
258 }
259
260 car->DynGC.vel.ax = car->DynGCg.vel.ax;
261 car->DynGC.vel.ay = car->DynGCg.vel.ay;
262 car->DynGC.vel.az = car->DynGCg.vel.az;
263
264 car->DynGC.vel.x = car->DynGCg.vel.x * Cosz + car->DynGCg.vel.y * Sinz;
265 car->DynGC.vel.y = -car->DynGCg.vel.x * Sinz + car->DynGCg.vel.y * Cosz;
266 car->DynGC.vel.z = car->DynGCg.vel.z;
267 }
268
269 void
SimCarUpdateWheelPos(tCar * car)270 SimCarUpdateWheelPos(tCar *car)
271 {
272 int i;
273 tdble vx;
274 tdble vy;
275 tdble Cosz, Sinz;
276
277 Cosz = car->Cosz;
278 Sinz = car->Sinz;
279 vx = car->DynGC.vel.x;
280 vy = car->DynGC.vel.y;
281
282 /* Wheels data */
283 for (i = 0; i < 4; i++) {
284 tdble x = car->wheel[i].staticPos.x;
285 tdble y = car->wheel[i].staticPos.y;
286 tdble dx = x * Cosz - y * Sinz;
287 tdble dy = x * Sinz + y * Cosz;
288
289 car->wheel[i].pos.x = car->DynGCg.pos.x + dx;
290 car->wheel[i].pos.y = car->DynGCg.pos.y + dy;
291 car->wheel[i].pos.z = car->DynGCg.pos.z - car->statGC.z - x * sin(car->DynGCg.pos.ay) + y * sin(car->DynGCg.pos.ax);
292
293 car->wheel[i].bodyVel.x = vx - car->DynGC.vel.az * y;
294 car->wheel[i].bodyVel.y = vy + car->DynGC.vel.az * x;
295 }
296 }
297
298 static void
SimCarUpdatePos(tCar * car)299 SimCarUpdatePos(tCar *car)
300 {
301 tdble vx, vy;
302
303 vx = car->DynGCg.vel.x;
304 vy = car->DynGCg.vel.y;
305
306 car->DynGCg.pos.x += vx * SimDeltaTime;
307 car->DynGCg.pos.y += vy * SimDeltaTime;
308 car->DynGCg.pos.z += car->DynGCg.vel.z * SimDeltaTime;
309
310 car->DynGCg.pos.ax += car->DynGCg.vel.ax * SimDeltaTime;
311 car->DynGCg.pos.ay += car->DynGCg.vel.ay * SimDeltaTime;
312 car->DynGCg.pos.az += car->DynGCg.vel.az * SimDeltaTime;
313
314 NORM_PI_PI(car->DynGCg.pos.az);
315
316 if (car->DynGCg.pos.ax > aMax) car->DynGCg.pos.ax = aMax;
317 if (car->DynGCg.pos.ax < -aMax) car->DynGCg.pos.ax = -aMax;
318 if (car->DynGCg.pos.ay > aMax) car->DynGCg.pos.ay = aMax;
319 if (car->DynGCg.pos.ay < -aMax) car->DynGCg.pos.ay = -aMax;
320
321 car->DynGC.pos.x = car->DynGCg.pos.x;
322 car->DynGC.pos.y = car->DynGCg.pos.y;
323 car->DynGC.pos.z = car->DynGCg.pos.z;
324
325 car->DynGC.pos.ax = car->DynGCg.pos.ax;
326 car->DynGC.pos.ay = car->DynGCg.pos.ay;
327 car->DynGC.pos.az = car->DynGCg.pos.az;
328
329 RtTrackGlobal2Local(car->trkPos.seg, car->DynGCg.pos.x, car->DynGCg.pos.y, &(car->trkPos), TR_LPOS_MAIN);
330 }
331
332 static void
SimCarUpdateCornerPos(tCar * car)333 SimCarUpdateCornerPos(tCar *car)
334 {
335 tdble Cosz = car->Cosz;
336 tdble Sinz = car->Sinz;
337 tdble vx = car->DynGCg.vel.x;
338 tdble vy = car->DynGCg.vel.y;
339 int i;
340
341 for (i = 0; i < 4; i++) {
342 tdble x = car->corner[i].pos.x + car->statGC.x;
343 tdble y = car->corner[i].pos.y + car->statGC.y;
344 tdble dx = x * Cosz - y * Sinz;
345 tdble dy = x * Sinz + y * Cosz;
346
347 car->corner[i].pos.ax = car->DynGCg.pos.x + dx;
348 car->corner[i].pos.ay = car->DynGCg.pos.y + dy;
349 /*car->corner[i].pos.az = car->DynGC.pos.z - car->statGC.z + x * sin(car->DynGC.pos.ay) + y * sin(car->DynGC.pos.ax);*/
350
351 /* add the body rotation to the wheel */
352 /* the speed is vel.az * r */
353 /* where r = sqrt(x*x + y*y) */
354 /* the tangent vector is -y / r and x / r */
355 // compute corner velocity at local frame
356 car->corner[i].vel.ax = - car->DynGC.vel.az * y;
357 car->corner[i].vel.ay = car->DynGC.vel.az * x;
358
359 // rotate to global and add global center of mass velocity
360 // note: global to local.
361 car->corner[i].vel.x = vx
362 + car->corner[i].vel.ax * Cosz - car->corner[i].vel.ay * Sinz;
363 car->corner[i].vel.y = vy
364 + car->corner[i].vel.ax * Sinz + car->corner[i].vel.ay * Cosz;
365
366 // add local center of mass velocity
367 car->corner[i].vel.ax += car->DynGC.vel.x;
368 car->corner[i].vel.ay += car->DynGC.vel.y;
369 }
370 }
371
372 void
SimTelemetryOut(tCar * car)373 SimTelemetryOut(tCar *car)
374 {
375 int i;
376 tdble Fzf, Fzr;
377
378 printf("-----------------------------\nCar: %d %s ---\n", car->carElt->index, car->carElt->_name);
379 printf("Seg: %d (%s) Ts:%f Tr:%f\n",
380 car->trkPos.seg->id, car->trkPos.seg->name, car->trkPos.toStart, car->trkPos.toRight);
381 printf("---\nMx: %f My: %f Mz: %f (N/m)\n", car->DynGC.acc.ax, car->DynGC.acc.ay, car->DynGC.acc.az);
382 printf("Wx: %f Wy: %f Wz: %f (rad/s)\n", car->DynGC.vel.ax, car->DynGC.vel.ay, car->DynGC.vel.az);
383 printf("Ax: %f Ay: %f Az: %f (rad)\n", car->DynGCg.pos.ax, car->DynGCg.pos.ay, car->DynGCg.pos.az);
384 printf("---\nAx: %f Ay: %f Az: %f (Gs)\n", car->DynGC.acc.x/9.81, car->DynGC.acc.y/9.81, car->DynGC.acc.z/9.81);
385 printf("Vx: %f Vy: %f Vz: %f (m/s)\n", car->DynGC.vel.x, car->DynGC.vel.y, car->DynGC.vel.z);
386 printf("Px: %f Py: %f Pz: %f (m)\n---\n", car->DynGCg.pos.x, car->DynGCg.pos.y, car->DynGCg.pos.z);
387 printf("As: %f\n---\n", sqrt(car->airSpeed2));
388 for (i = 0; i < 4; i++) {
389 printf("wheel %d - RH:%f susp:%f zr:%.2f ", i, car->wheel[i].rideHeight, car->wheel[i].susp.x, car->wheel[i].zRoad);
390 printf("sx:%f sa:%f w:%f ", car->wheel[i].sx, car->wheel[i].sa, car->wheel[i].spinVel);
391 printf("fx:%f fy:%f fz:%f\n", car->wheel[i].forces.x, car->wheel[i].forces.y, car->wheel[i].forces.z);
392 }
393 Fzf = (car->aero.lift[0] + car->wing[0].forces.z) / 9.81;
394 Fzr = (car->aero.lift[1] + car->wing[1].forces.z) / 9.81;
395 printf("Aero Fx:%f Fz:%f Fzf=%f Fzr=%f ratio=%f\n", car->aero.drag / 9.81, Fzf + Fzr,
396 Fzf, Fzr, (Fzf + Fzr) / (car->aero.drag + 0.1) * 9.81);
397
398 }
399
400 void
SimCarUpdate(tCar * car,tSituation *)401 SimCarUpdate(tCar *car, tSituation * /* s */)
402 {
403 SimCarUpdateForces(car);
404 CHECK(car);
405 SimCarUpdateSpeed(car);
406 CHECK(car);
407 SimCarUpdateCornerPos(car);
408 CHECK(car);
409 SimCarUpdatePos(car);
410 CHECK(car);
411 SimCarCollideZ(car);
412 CHECK(car);
413 SimCarCollideXYScene(car);
414 CHECK(car);
415 car->speed = sqrt(car->DynGC.vel.x*car->DynGC.vel.x + car->DynGC.vel.y*car->DynGC.vel.y + car->DynGC.vel.z*car->DynGC.vel.z);
416 }
417
418 void
SimCarUpdate2(tCar * car,tSituation *)419 SimCarUpdate2(tCar *car, tSituation * /* s */)
420 {
421 if (SimTelemetry == car->carElt->index) SimTelemetryOut(car);
422 }
423
424