1 #include "pch.h"
2 #include "cartire.h"
3 #include "cardefs.h"
4 //#include "../ogre/common/Def_Str.h"
5 #include "isnan.h"
6
7
FindSigmaHatAlphaHat(Dbl load,Dbl & output_sigmahat,Dbl & output_alphahat,int iterations)8 void CARTIRE::FindSigmaHatAlphaHat(Dbl load, Dbl & output_sigmahat, Dbl & output_alphahat, int iterations)
9 {
10 Dbl x, y, ymax, junk, x4 = 4.0/iterations, x40 = 40.0/iterations;
11 ymax = 0;
12 for (x = -2; x < 2; x += x4)
13 {
14 y = Pacejka_Fx(x, load, 1.0, junk);
15 if (y > ymax)
16 {
17 output_sigmahat = x;
18 ymax = y;
19 }
20 }
21 ymax = 0;
22 for (x = -20; x < 20; x += x40)
23 {
24 y = Pacejka_Fy(x, load, 0, 1.0, junk);
25 if (y > ymax)
26 {
27 output_alphahat = x;
28 ymax = y;
29 }
30 }
31 }
32
LookupSigmaHatAlphaHat(Dbl normalforce,Dbl & sh,Dbl & ah) const33 void CARTIRE::LookupSigmaHatAlphaHat(Dbl normalforce, Dbl & sh, Dbl & ah) const
34 {
35 assert(!sigma_hat.empty());
36 assert(!alpha_hat.empty());
37 assert(sigma_hat.size() == alpha_hat.size());
38
39 int HAT_ITERATIONS = sigma_hat.size();
40
41 Dbl HAT_LOAD = 0.5;
42 Dbl nf = normalforce * 0.001;
43 if (nf < HAT_LOAD)
44 {
45 sh = sigma_hat[0];
46 ah = alpha_hat[0];
47 }
48 else if (nf >= HAT_LOAD*HAT_ITERATIONS)
49 {
50 sh = sigma_hat[HAT_ITERATIONS-1];
51 ah = alpha_hat[HAT_ITERATIONS-1];
52 }
53 else
54 {
55 int lbound;
56 Dbl blend;
57 lbound = (int)(nf/HAT_LOAD);
58 lbound--;
59 if (lbound < 0)
60 lbound = 0;
61 blend = (nf-HAT_LOAD*(lbound+1))/HAT_LOAD;
62 sh = sigma_hat[lbound]*(1.0-blend)+sigma_hat[lbound+1]*blend;
63 ah = alpha_hat[lbound]*(1.0-blend)+alpha_hat[lbound+1]*blend;
64 }
65 }
66
67
68 /// Return the friction vector calculated from the magic formula.
69 /// HUB_VELOCITY is the velocity vector of the wheel's reference
70 /// frame. PATCH_SPEED is the rearward speed of the contact patch
71 /// with respect to the wheel's frame.
72 /// current_camber is expected in radians.
73 /// normal_force is in units N.
74 //-------------------------------------------------------------------------------------------------------------------------------
GetForce(Dbl normal_force,Dbl friction_coeff,const MATHVECTOR<Dbl,3> & hub_velocity,Dbl patch_speed,Dbl current_camber,CARWHEEL::SlideSlip * slips) const75 MATHVECTOR<Dbl,3> CARTIRE::GetForce(
76 Dbl normal_force,
77 Dbl friction_coeff,
78 //Dbl roll_friction_coeff,
79 const MATHVECTOR<Dbl,3> & hub_velocity,
80 Dbl patch_speed,
81 Dbl current_camber,
82 CARWHEEL::SlideSlip* slips) const
83 {
84 assert(friction_coeff > 0);
85
86 Dbl sigma_hat(0);
87 Dbl alpha_hat(0);
88
89 LookupSigmaHatAlphaHat(normal_force, sigma_hat, alpha_hat);
90
91 //std::cout << hub_velocity << " -- " << patch_speed << std::endl;
92
93 Dbl Fz = normal_force * 0.001;
94
95 //cap Fz at a magic number to prevent explosions
96 if (Fz > 30)
97 Fz = 30;
98
99 const Dbl EPSILON = 1e-6;
100 if (Fz < EPSILON)
101 {
102 MATHVECTOR<Dbl,3> zero(0);
103 return zero;
104 }
105
106 Dbl sigma = 0.0;
107 Dbl tan_alpha = 0.0;
108 Dbl alpha = 0.0;
109
110 Dbl V = hub_velocity[0];
111 Dbl denom = std::max(std::abs(V), 0.01);
112
113 sigma = (patch_speed - V) / denom;
114 tan_alpha = hub_velocity[1] / denom;
115 alpha = -atan2(hub_velocity[1], denom) * 180.0/PI_d;
116
117 /*crash dyn obj--*/
118 if (isnan(alpha) || isnan(1.f/sigma_hat))
119 {
120 MATHVECTOR<Dbl,3> outvec(0, 0, 0);
121 return outvec;
122 }
123 assert(!isnan(alpha));
124
125 Dbl gamma = current_camber * 180.0/PI_d;
126
127 // beckman method for pre-combining longitudinal and lateral forces
128 Dbl s = sigma / sigma_hat; assert(!isnan(s));
129 Dbl a = alpha / alpha_hat; assert(!isnan(a));
130
131 Dbl rho = std::max( sqrt( s*s+a*a ), 0.0001); //avoid divide by zero
132 assert(!isnan(rho));
133
134 Dbl max_Fx(0), max_Fy(0), max_Mz(0);
135 Dbl Fx = (s / rho) * Pacejka_Fx( rho*sigma_hat, Fz, friction_coeff, max_Fx ); assert(!isnan(Fx));
136 Dbl Fy = (a / rho) * Pacejka_Fy( rho*alpha_hat, Fz, gamma, friction_coeff, max_Fy ); assert(!isnan(Fy));
137 Dbl Mz = Pacejka_Mz( sigma, alpha, Fz, gamma, friction_coeff, max_Mz );
138
139 if (slips) // out vis
140 { slips->preFx = Fx;
141 slips->preFy = Fy;
142 }
143 //Dbl slip_x = -sigma / ( 1.0 + generic_abs ( sigma ) );
144 //Dbl slip_y = tan_alpha / ( 1.0+generic_abs ( sigma-1.0 ) );
145 //Dbl total_slip = std::sqrt ( slip_x * slip_x + slip_y * slip_y );
146 //Dbl maxforce = longitudinal_parameters[2] * 7.0;
147
148 // combining method 0: no combining
149
150 // combining method 1: traction circle
151 // determine to what extent the tires are long (x) gripping vs lat (y) gripping
152 float longfactor = 1.0;
153 float combforce = std::abs(Fx)+std::abs(Fy);
154 if (combforce > 1) // avoid divide by zero (assume longfactor = 1 for this case)
155 longfactor = std::abs(Fx)/combforce; // 1.0 when Fy is zero, 0.0 when Fx is zero
156 // determine the maximum force for this amount of long vs lat grip
157 float maxforce = std::abs(max_Fx)*longfactor + (1.0-longfactor)*std::abs(max_Fy); //linear interpolation
158 if (combforce > maxforce) // cap forces
159 {
160 //scale down forces to fit into the maximum
161 Dbl sc = maxforce / combforce;
162 Fx *= sc; assert(!isnan(Fx)); max_Fx *= sc; //vis only
163 Fy *= sc; assert(!isnan(Fy)); max_Fy *= sc;
164 //std::cout << "Limiting " << combforce << " to " << maxforce << std::endl;
165 }/**/
166
167 // combining method 2: traction ellipse (prioritize Fx)
168 //std::cout << "Fy0=" << Fy << ", ";
169 /*if (Fx >= max_Fx)
170 {
171 Fx = max_Fx;
172 Fy = 0;
173 }else
174 Fy = Fy*sqrt(1.0-(Fx/max_Fx)*(Fx/max_Fx));/**/
175 //std::cout << "Fy=" << Fy << ", Fx=Fx0=" << Fx << ", Fxmax=" << max_Fx << ", Fymax=" << max_Fy << std::endl;
176
177 // combining method 3: traction ellipse (prioritize Fy)
178 /*if (Fy >= max_Fy)
179 {
180 Fy = max_Fy;
181 Fx = 0;
182 }else
183 { Dbl scale = sqrt(1.0-(Fy/max_Fy)*(Fy/max_Fy));
184 if (std::isnan(scale))
185 Fx = 0;
186 else
187 Fx = Fx*scale;
188 }/**/
189
190 assert(!isnan(Fx));
191 assert(!isnan(Fy));
192
193 /*if ( hub_velocity.Magnitude () < 0.1 )
194 {
195 slide = 0.0;
196 }else
197 { slide = total_slip;
198 if ( slide > 1.0 )
199 slide = 1.0;
200 }*/
201
202 if (slips) // out vis
203 {
204 slips->slide = sigma; slips->slideratio = s;
205 slips->slip = alpha; slips->slipratio = a;
206 slips->fx_sr = s / rho; slips->fx_rsr = rho*sigma_hat;
207 slips->fy_ar = a / rho; slips->fy_rar = rho*alpha_hat;
208 slips->frict = friction_coeff;
209 //Dbl Fx = (s / rho) * Pacejka_Fx( rho*sigma_hat, Fz, friction_coeff, max_Fx );
210 //Dbl Fy = (a / rho) * Pacejka_Fy( rho*alpha_hat, Fz, gamma, friction_coeff, max_Fy );
211 slips->Fx = Fx; slips->Fxm = max_Fx;
212 slips->Fy = Fy; slips->Fym = max_Fy;
213 slips->Fz = Fz;
214 }
215
216 //std::cout << slide << ", " << slip << std::endl;
217
218 MATHVECTOR<Dbl,3> outvec(Fx, Fy, Mz);
219 return outvec;
220 }
221 //-------------------------------------------------------------------------------------------------------------------------------
222
223
CalculateSigmaHatAlphaHat(int tablesize)224 void CARTIRE::CalculateSigmaHatAlphaHat(int tablesize)
225 {
226 Dbl HAT_LOAD = 0.5;
227 sigma_hat.resize(tablesize, 0);
228 alpha_hat.resize(tablesize, 0);
229 for (int i = 0; i < tablesize; ++i)
230 FindSigmaHatAlphaHat((Dbl)(i+1)*HAT_LOAD, sigma_hat[i], alpha_hat[i]);
231 }
232
233 //-------------------------------------------------------------------------------------------------------------------------------
234
235
236 /// load is the normal force in newtons
GetMaximumFx(Dbl load) const237 Dbl CARTIRE::GetMaximumFx(Dbl load) const
238 {
239 const std::vector <Dbl>& b = longitudinal;
240 Dbl Fz = load * 0.001;
241 return (b[1]*Fz + b[2]) * Fz;
242 }
243
GetMaximumFy(Dbl load,Dbl current_camber) const244 Dbl CARTIRE::GetMaximumFy(Dbl load, Dbl current_camber) const
245 {
246 const std::vector <Dbl>& a = lateral;
247 Dbl Fz = load * 0.001;
248 Dbl gamma = current_camber * 180.0/PI_d;
249
250 Dbl D = (a[1]*Fz + a[2]) *Fz;
251 Dbl Sv = ((a[11]*Fz + a[12]) * gamma + a[13]) *Fz + a[14];
252
253 return D+Sv;
254 }
255
GetMaximumMz(Dbl load,Dbl current_camber) const256 Dbl CARTIRE::GetMaximumMz(Dbl load, Dbl current_camber) const
257 {
258 const std::vector <Dbl>& c = aligning;
259 Dbl Fz = load * 0.001;
260 Dbl gamma = current_camber * 180.0/PI_d;
261
262 Dbl D = (c[1]*Fz + c[2]) *Fz;
263 Dbl Sv = (c[14]*Fz*Fz + c[15]*Fz) * gamma + c[16]*Fz + c[17];
264
265 return -(D+Sv);
266 }
267
268 /// pacejka magic formula function
269 /// longitudinal
Pacejka_Fx(Dbl sigma,Dbl Fz,Dbl friction_coeff,Dbl & maxforce_output) const270 Dbl CARTIRE::Pacejka_Fx (Dbl sigma, Dbl Fz, Dbl friction_coeff, Dbl & maxforce_output) const
271 {
272 const std::vector <Dbl>& b = longitudinal;
273
274 Dbl D = (b[1]*Fz + b[2]) *Fz *friction_coeff;
275 assert(b[0]* (b[1]*Fz + b[2]) != 0);
276 Dbl B = ( b[3]*Fz+b[4] ) *exp ( -b[5]*Fz ) / ( b[0]* ( b[1]*Fz+b[2] ) );
277 Dbl E = ( b[6]*Fz*Fz+b[7]*Fz+b[8] );
278 Dbl S = ( 100*sigma + b[9]*Fz+b[10] );
279 Dbl Fx = D*sin ( b[0] * atan ( S*B+E* ( atan ( S*B )-S*B ) ) );
280
281 maxforce_output = D;
282
283 assert(!isnan(Fx));
284 return Fx;
285 }
286
287 /// lateral
Pacejka_Fy(Dbl alpha,Dbl Fz,Dbl gamma,Dbl friction_coeff,Dbl & maxforce_output) const288 Dbl CARTIRE::Pacejka_Fy (Dbl alpha, Dbl Fz, Dbl gamma, Dbl friction_coeff, Dbl & maxforce_output) const
289 {
290 const std::vector <Dbl>& a = lateral;
291
292 Dbl D = ( a[1]*Fz+a[2] ) *Fz*friction_coeff;
293 Dbl B = a[3]*sin ( 2.0*atan ( Fz/a[4] ) ) * ( 1.0-a[5]*std::abs ( gamma ) ) / ( a[0]* ( a[1]*Fz+a[2] ) *Fz );
294 assert(!isnan(B));
295 Dbl E = a[6]*Fz+a[7];
296 Dbl S = alpha + a[8]*gamma+a[9]*Fz+a[10];
297 Dbl Sv = ( ( a[11]*Fz+a[12] ) *gamma + a[13] ) *Fz+a[14];
298 Dbl Fy = D*sin ( a[0]*atan ( S*B+E* ( atan ( S*B )-S*B ) ) ) +Sv;
299
300 maxforce_output = D+Sv;
301
302 //LogO("Fy: "+fToStr(alpha,4,6)+" "+fToStr(Fz,4,6)+" "+fToStr(gamma,4,6)+" "+fToStr(friction_coeff,4,6)+" "+fToStr(maxforce_output,4,6));
303
304 assert(!isnan(Fy));
305 return Fy;
306 }
307
308 /// aligning
Pacejka_Mz(Dbl sigma,Dbl alpha,Dbl Fz,Dbl gamma,Dbl friction_coeff,Dbl & maxforce_output) const309 Dbl CARTIRE::Pacejka_Mz (Dbl sigma, Dbl alpha, Dbl Fz, Dbl gamma, Dbl friction_coeff, Dbl & maxforce_output) const
310 {
311 const std::vector <Dbl>& c = aligning;
312
313 Dbl D = ( c[1]*Fz+c[2] ) *Fz*friction_coeff;
314 Dbl B = ( c[3]*Fz*Fz+c[4]*Fz ) * ( 1.0-c[6]*std::abs ( gamma ) ) *exp ( -c[5]*Fz ) / ( c[0]*D );
315 Dbl E = ( c[7]*Fz*Fz+c[8]*Fz+c[9] ) * ( 1.0-c[10]*std::abs ( gamma ) );
316 Dbl S = alpha + c[11]*gamma+c[12]*Fz+c[13];
317 Dbl Sv = ( c[14]*Fz*Fz+c[15]*Fz ) *gamma+c[16]*Fz + c[17];
318 Dbl Mz = D*sin ( c[0]*atan ( S*B+E* ( atan ( S*B )-S*B ) ) ) +Sv;
319
320 maxforce_output = D+Sv;
321
322 assert(!isnan(Mz));
323 return Mz;
324 }
325
326
327 /// optimum steering angle in degrees given load in newtons
GetOptimumSteeringAngle(Dbl load) const328 Dbl CARTIRE::GetOptimumSteeringAngle(Dbl load) const
329 {
330 Dbl sigma_hat(0);
331 Dbl alpha_hat(0);
332
333 LookupSigmaHatAlphaHat(load, sigma_hat, alpha_hat);
334
335 return alpha_hat;
336 }
337
338
339 //-------------------------------------------------------------------------------------------------------------------------------
None()340 /*const*/CARTIRE * CARTIRE::None() // default, inited, won't crash, not to be used in game
341 {
342 static CARTIRE s;
343 static bool init = true;
344 if (init)
345 { init = false;
346 int i=0;
347 s.lateral[i++] = 1.61;
348 s.lateral[i++] = -0;
349 s.lateral[i++] = 2775;
350 s.lateral[i++] = 2220;
351 s.lateral[i++] = 19.6;
352 s.lateral[i++] = 0.013;
353 s.lateral[i++] = -0.14;
354 s.lateral[i++] = 0.14;
355 s.lateral[i++] = 0.019;
356 s.lateral[i++] = -0.019;
357 s.lateral[i++] = -0.18;
358 s.lateral[i++] = 0;
359 s.lateral[i++] = 0;
360 s.lateral[i++] = 0;
361 s.lateral[i++] = 0;
362 i = 0;
363 s.longitudinal[i++] = 1.73;
364 s.longitudinal[i++] = -0.49;
365 s.longitudinal[i++] = 3439;
366 s.longitudinal[i++] = 279;
367 s.longitudinal[i++] = 470;
368 s.longitudinal[i++] = 0;
369 s.longitudinal[i++] = 0.0008;
370 s.longitudinal[i++] = 0.005;
371 s.longitudinal[i++] = -0.024;
372 s.longitudinal[i++] = 0;
373 s.longitudinal[i++] = 0;
374 i = 0;
375 s.aligning[i++] = 2.10;
376 s.aligning[i++] = -3.9;
377 s.aligning[i++] = -3.9;
378 s.aligning[i++] = -1.26;
379 s.aligning[i++] = -8.20;
380 s.aligning[i++] = 0.025;
381 s.aligning[i++] = 0;
382 s.aligning[i++] = 0.044;
383 s.aligning[i++] = -0.58;
384 s.aligning[i++] = 0.18;
385 s.aligning[i++] = 0.043;
386 s.aligning[i++] = 0.048;
387 s.aligning[i++] = -0.0035;
388 s.aligning[i++] = -0.18;
389 s.aligning[i++] = 0.14;
390 s.aligning[i++] = -1.029;
391 s.aligning[i++] = 0.27;
392 s.aligning[i++] = -1.1;
393 s.name = "None";
394 s.user = 0;
395
396 s.CalculateSigmaHatAlphaHat();
397 }
398 return &s;
399 }
400