1 /***************************************************************************
2 car.cpp - A car, being a moving object
3 -------------------
4 begin : Wed Dec 4 2002
5 copyright : (C) 2002 by CJP
6 email : cornware-cjp@users.sourceforge.net
7 ***************************************************************************/
8
9 /***************************************************************************
10 * *
11 * This program is free software; you can redistribute it and/or modify *
12 * it under the terms of the GNU General Public License as published by *
13 * the Free Software Foundation; either version 2 of the License, or *
14 * (at your option) any later version. *
15 * *
16 ***************************************************************************/
17
18 #include <cstdio>
19 #include <cmath>
20
21 #include "pi.h"
22 #define DBLPI (2.0*M_PI)
23
24 #include "bound.h"
25 #include "car.h"
26 #include "carinput.h"
27 #include "world.h"
28
29 #include "physics.h"
30
31 #include "datafile.h"
32 #include "lconfig.h"
33
CCar(CDataManager * manager)34 CCar::CCar(CDataManager *manager) : CMovingObject(manager)
35 {
36 m_Ground.nor = CVector(0,0,0);
37 m_SimState = eFlying;
38 }
39
~CCar()40 CCar::~CCar()
41 {
42 }
43
load(const CString & filename,const CParamList & list)44 bool CCar::load(const CString &filename, const CParamList &list)
45 {
46 CMovingObject::load(filename, list);
47
48 //initial state
49 m_DesiredSteering = 0.0;
50 //m_xAngle = 0.0;
51 //m_zAngle = 0.0;
52 //m_BodyHeight = 0.0;
53
54 CDataFile dfile(getFilename());
55 CLConfig cfile(dfile.useExtern());
56 //TODO: make a way to find out if this file exists
57 //and return false if it doesn't
58
59 //Description
60 m_CarName = cfile.getValue("description", "fullname");
61
62 //Mass + moment of inertia
63 m_InvMass = 1.0 / cfile.getValue("body", "mass").toFloat();
64 m_BodySize = cfile.getValue("body", "size").toVector();
65 m_InvInertia.setElement(0, 0, 1.0 / (m_BodySize.y*m_BodySize.y + m_BodySize.z*m_BodySize.z));
66 m_InvInertia.setElement(1, 1, 1.0 / (m_BodySize.x*m_BodySize.x + m_BodySize.z*m_BodySize.z));
67 m_InvInertia.setElement(2, 2, 1.0 / (m_BodySize.x*m_BodySize.x + m_BodySize.y*m_BodySize.y));
68 m_InvInertia *= 12.0 * m_InvMass;
69 m_CenterOfMass = cfile.getValue("body", "centerofmass").toVector();
70
71 //body
72 CString bodygeomfile = cfile.getValue("body", "geometry");
73 m_CameraPos = cfile.getValue("body", "camerapos").toVector();
74 m_cwA = cfile.getValue("body", "cwa").toFloat();
75 m_RotationDamping = cfile.getValue("body", "rotationdamping").toFloat();
76
77 //wheels
78 CString frontwheelgeomfile = cfile.getValue("frontwheels", "geometry");
79 CString rearwheelgeomfile = cfile.getValue("rearwheels", "geometry");
80 m_FrontSteerMax = cfile.getValue("frontwheels", "steermax").toFloat();
81 m_RearSteerMax = -cfile.getValue("rearwheels", "steermax").toFloat();
82 m_Engine.m_FrontTraction = cfile.getValue("frontwheels", "traction").toFloat();
83 m_Engine.m_RearTraction = cfile.getValue("rearwheels", "traction").toFloat();
84 m_FrontDownforce = cfile.getValue("frontwheels", "downforce").toFloat();
85 m_RearDownforce = cfile.getValue("rearwheels", "downforce").toFloat();
86
87 //Engine torque curve:
88 m_Engine.m_M0 = cfile.getValue("engine", "zerotorque").toFloat();
89 m_Engine.m_Mmax = cfile.getValue("engine", "maxtorque").toFloat();
90 m_Engine.m_w_Mmax = cfile.getValue("engine", "w_maxtorque").toFloat();
91 m_Engine.m_Pmax = cfile.getValue("engine", "maxpower").toFloat();
92 m_Engine.m_w_Pmax = cfile.getValue("engine", "w_maxpower").toFloat();
93 m_Engine.m_w_Zero = cfile.getValue("engine", "w_zero").toFloat();
94
95 //driving pipeline
96 float gear0 = cfile.getValue("engine", "gear0").toFloat();
97 float gear1 = cfile.getValue("engine", "gear1").toFloat();
98 float gear2 = cfile.getValue("engine", "gear2").toFloat();
99 float gear3 = cfile.getValue("engine", "gear3").toFloat();
100 float gear4 = cfile.getValue("engine", "gear4").toFloat();
101 float gear5 = cfile.getValue("engine", "gear5").toFloat();
102 float gear6 = cfile.getValue("engine", "gear6").toFloat();
103 m_Engine.m_GearRatios.push_back(gear0);
104 m_Engine.m_GearRatios.push_back(gear1);
105 if(gear2 > 0.00001) m_Engine.m_GearRatios.push_back(gear2);
106 if(gear3 > 0.00001) m_Engine.m_GearRatios.push_back(gear3);
107 if(gear4 > 0.00001) m_Engine.m_GearRatios.push_back(gear4);
108 if(gear5 > 0.00001) m_Engine.m_GearRatios.push_back(gear5);
109 if(gear6 > 0.00001) m_Engine.m_GearRatios.push_back(gear6);
110
111 m_Engine.m_DifferentialRatio = cfile.getValue("engine", "differentialratio").toFloat();
112
113 //The sounds:
114 m_Sounds.push_back(theWorld->loadObject(cfile.getValue("sound", "engine"), CParamList(), CDataObject::eSample));
115 {
116 CString baseRPS = cfile.getValue("sound", "enginerps");
117 m_EngineSoundBaseRPS = 388.0;
118 if(baseRPS != "")
119 m_EngineSoundBaseRPS = baseRPS.toFloat();
120 }
121
122 //One texture:
123 m_Textures.push_back(theWorld->loadObject(cfile.getValue("skin", "texture"), CParamList(), CDataObject::eMaterial));
124
125 //Dashboard info:
126 m_Dashboard.background_tex = cfile.getValue("dashboard", "background_tex");
127 m_Dashboard.background_hth = cfile.getValue("dashboard", "background_hth").toFloat();
128
129 m_Dashboard.crash_background_tex = cfile.getValue("dashboard", "crash_background_tex");
130 m_Dashboard.crash_tex = cfile.getValue("dashboard", "crash_tex");
131
132 m_Dashboard.steer_tex = cfile.getValue("dashboard", "steer_tex");
133 m_Dashboard.steer_pos = cfile.getValue("dashboard", "steer_pos").toVector();
134 m_Dashboard.steer_rad = cfile.getValue("dashboard", "steer_rad").toFloat();
135 m_Dashboard.steer_ang = cfile.getValue("dashboard", "steer_ang").toFloat();
136
137 m_Dashboard.analog_vel_tex = cfile.getValue("dashboard", "analog_vel_tex");
138 m_Dashboard.analog_vel_pos = cfile.getValue("dashboard", "analog_vel_pos").toVector();
139 m_Dashboard.analog_vel_rad = cfile.getValue("dashboard", "analog_vel_rad").toFloat();
140 m_Dashboard.analog_vel_an0 = cfile.getValue("dashboard", "analog_vel_an0").toFloat();
141 m_Dashboard.analog_vel_an1 = cfile.getValue("dashboard", "analog_vel_an1").toFloat();
142 m_Dashboard.analog_vel_max = cfile.getValue("dashboard", "analog_vel_max").toFloat();
143
144 m_Dashboard.analog_rpm_tex = cfile.getValue("dashboard", "analog_rpm_tex");
145 m_Dashboard.analog_rpm_pos = cfile.getValue("dashboard", "analog_rpm_pos").toVector();
146 m_Dashboard.analog_rpm_rad = cfile.getValue("dashboard", "analog_rpm_rad").toFloat();
147 m_Dashboard.analog_rpm_an0 = cfile.getValue("dashboard", "analog_rpm_an0").toFloat();
148 m_Dashboard.analog_rpm_an1 = cfile.getValue("dashboard", "analog_rpm_an1").toFloat();
149 m_Dashboard.analog_rpm_max = cfile.getValue("dashboard", "analog_rpm_max").toFloat();
150
151 m_Dashboard.digital_vel_pos = cfile.getValue("dashboard", "digital_vel_pos").toVector();
152 m_Dashboard.digital_vel_hth = cfile.getValue("dashboard", "digital_vel_hth").toFloat();
153 m_Dashboard.digital_vel_wth = cfile.getValue("dashboard", "digital_vel_wth").toFloat();
154 m_Dashboard.digital_rpm_pos = cfile.getValue("dashboard", "digital_rpm_pos").toVector();
155 m_Dashboard.digital_rpm_hth = cfile.getValue("dashboard", "digital_rpm_hth").toFloat();
156 m_Dashboard.digital_rpm_wth = cfile.getValue("dashboard", "digital_rpm_wth").toFloat();
157 m_Dashboard.songtitle_pos = cfile.getValue("dashboard", "songtitle_pos").toVector();
158 m_Dashboard.songtitle_hth = cfile.getValue("dashboard", "songtitle_hth").toFloat();
159 m_Dashboard.songtitle_wth = cfile.getValue("dashboard", "songtitle_wth").toFloat();
160
161 m_SteerSpeedOut = cfile.getValue("controls", "steerspeed_out").toFloat();
162 m_SteerSpeedIn = cfile.getValue("controls", "steerspeed_in").toFloat();
163 m_SteerSpeed_v_factor = cfile.getValue("controls", "steerspeed_v_factor").toFloat();
164
165 //The input object: CCarInput instead of CMovObjInput
166 delete m_InputData;
167 m_InputData = new CCarInput;
168 m_InputData->m_MovObjID = m_MovObjID;
169
170 //Five bodies:
171 CBody body, wheel1, wheel2, wheel3, wheel4;
172
173 //texture settings:
174 CParamList plist;
175 {
176 SParameter p;
177 p.name = "subset";
178 p.value = (int)m_Textures[0];
179 plist.push_back(p);
180
181 p.name = "color";
182 p.value = list.getValue("color", cfile.getValue("skin", "defaultcolor"));
183 plist.push_back(p);
184 }
185
186 //Set the indices to the body array
187 body.m_Body = theWorld->loadObject(bodygeomfile, plist, CDataObject::eBound);
188 if(body.m_Body < 0)
189 printf("Error: body geometry %s was not loaded\n", bodygeomfile.c_str());
190
191 wheel1.m_Body = theWorld->loadObject(frontwheelgeomfile, plist, CDataObject::eBound);
192 wheel2.m_Body = wheel1.m_Body;
193 if(wheel1.m_Body < 0)
194 printf("Error: frontwheel geometry %s was not loaded\n", frontwheelgeomfile.c_str());
195
196 wheel3.m_Body = theWorld->loadObject(rearwheelgeomfile, plist, CDataObject::eBound);
197 wheel4.m_Body = wheel3.m_Body;
198 if(wheel3.m_Body < 0)
199 printf("Error: rearwheel geometry %s was not loaded\n", rearwheelgeomfile.c_str());
200
201 m_Bodies.push_back(body);
202 m_Bodies.push_back(wheel1);
203 m_Bodies.push_back(wheel2);
204 m_Bodies.push_back(wheel3);
205 m_Bodies.push_back(wheel4);
206
207 //Size of the wheels
208 m_Wheel[0].m_Radius = ((CBound *)theWorld->getObject(CDataObject::eBound, wheel1.m_Body))->m_CylinderRadius;
209 m_Wheel[1].m_Radius = ((CBound *)theWorld->getObject(CDataObject::eBound, wheel2.m_Body))->m_CylinderRadius;
210 m_Wheel[2].m_Radius = ((CBound *)theWorld->getObject(CDataObject::eBound, wheel3.m_Body))->m_CylinderRadius;
211 m_Wheel[3].m_Radius = ((CBound *)theWorld->getObject(CDataObject::eBound, wheel4.m_Body))->m_CylinderRadius;
212
213 //Important: do this AFTER setting m_Radius:
214 m_Wheel[0].load(cfile, "frontwheels");
215 m_Wheel[1].load(cfile, "frontwheels");
216 m_Wheel[2].load(cfile, "rearwheels");
217 m_Wheel[3].load(cfile, "rearwheels");
218
219 //Correcting the neutral position
220 m_Wheel[0].m_NeutralPos.x = -m_Wheel[0].m_NeutralPos.x;
221 m_Wheel[2].m_NeutralPos.x = -m_Wheel[2].m_NeutralPos.x;
222
223 for(unsigned int i=0; i<4; i++)
224 m_Wheel[i].m_NeutralPos -= m_CenterOfMass; //save as relative to center of mass
225
226 //Calculate the position above the ground:
227 m_PositionAboveGround = 0.5 * (
228 m_Wheel[0].m_Radius - m_Wheel[0].m_NeutralPos.y +
229 m_Wheel[2].m_Radius - m_Wheel[2].m_NeutralPos.y);
230
231 //Setting the initial positions
232 m_Position = CVector(0,0,0);
233 m_OrientationMatrix = CMatrix();
234 resetBodyPositions();
235
236 return true;
237 }
238
unload()239 void CCar::unload()
240 {
241 //TODO
242 CMovingObject::unload();
243 }
244
resetBodyPositions()245 void CCar::resetBodyPositions()
246 {
247 placeBodies();
248 }
249
placeBodies()250 void CCar::placeBodies()
251 {
252 m_Bodies[0].m_Position = m_Position -
253 m_OrientationMatrix * m_CenterOfMass;
254
255 m_Bodies[0].m_OrientationMatrix = m_OrientationMatrix;
256
257 for(unsigned int i=0; i < 4; i++)
258 {
259 float steeringAngle = m_Wheel[i].m_DesiredSt;
260 float rotationAngle = m_Wheel[i].m_a;
261 if(i == 1 || i == 3) //the right wheels
262 {
263 steeringAngle += M_PI;
264 rotationAngle = -rotationAngle;
265 }
266
267 //s = wheel steering matrix
268 //r = wheel rotation matrix
269 CMatrix s, r;
270 s.rotY(steeringAngle);
271 r.rotX(rotationAngle);
272
273 //i+1 because body 0 is the car main body
274 m_Bodies[i+1].m_Position = m_Position + m_OrientationMatrix *
275 (m_Wheel[i].m_NeutralPos + CVector(0,m_Wheel[i].m_Height,0));
276 m_Bodies[i+1].m_OrientationMatrix = r * s * m_OrientationMatrix;
277 }
278 }
279
determineGroundPlane(CPhysics * simulator)280 void CCar::determineGroundPlane(CPhysics *simulator)
281 {
282 vector<CCollisionFace> wheelGround;
283 vector<CVector> contactPoint;
284
285 //Debugging:
286 //m_Ground.nor = CVector(0,1,0);
287 //m_Ground.d = 0.0;
288 //return;
289
290 //fprintf(stderr, "\n\ndetermineGroundPlane\n");
291
292 //get the ground faces for the wheels
293 for(unsigned int i=0; i < 4; i++)
294 {
295 CVector pos = m_OrientationMatrix * (m_Wheel[i].m_NeutralPos + CVector(0,m_Wheel[i].m_Radius,0));
296 //fprintf(stderr, "Wheel %d height: %.3f\n", i, (pos + m_Position).y);
297
298 const CCollisionFace * theFace = theWorld->m_Detector.getGroundFace(pos + m_Position);
299 if(theFace != NULL)
300 {
301 //In absolute coordinates:
302 m_Wheel[i].m_Ground = *theFace;
303
304 //relative to the car center:
305 CCollisionFace cf = *theFace;
306 cf.d -= m_Position.dotProduct(cf.nor);
307 float dpos = pos.dotProduct(cf.nor);
308
309 //check if it's close enough to the wheel
310 if(dpos - cf.d > 5.0 * m_Wheel[i].m_Radius)
311 continue;
312
313 //fprintf(stderr, " -> on the ground\n");
314
315 //contact position
316 pos += (cf.d - dpos) * cf.nor;
317
318 contactPoint.push_back(pos);
319 wheelGround.push_back(cf);
320 }
321 }
322
323 //printf("%d\n", wheelGround.size());
324
325 switch(wheelGround.size())
326 {
327 case 4:
328 {
329 //a plane through each combination of 3 wheels
330 CCollisionFace planes[4];
331
332 for(unsigned int i=0; i < 4; i++)
333 {
334 int indices[3]; //the 3 points
335
336 //only the points that are not i
337 int count = 0;
338 for(unsigned int j=0; j < 4; j++)
339 if(j != i)
340 {
341 indices[count] = j;
342 count++;
343 }
344
345 CVector line1 = contactPoint[indices[1]] - contactPoint[indices[0]];
346 CVector line2 = contactPoint[indices[2]] - contactPoint[indices[0]];
347
348 planes[i].nor = line1.crossProduct(line2).normal();
349
350 CVector norsum =
351 wheelGround[indices[0]].nor +
352 wheelGround[indices[1]].nor +
353 wheelGround[indices[2]].nor;
354
355 if(norsum.dotProduct(planes[i].nor) < 0.0) //the real normals point to the other side
356 planes[i].nor = -planes[i].nor;
357
358 planes[i].d = contactPoint[indices[0]].dotProduct(planes[i].nor);
359 }
360
361 //now choose a plane. The other wheel should be as much above its own plane as possible
362 //so its contact point should be as much below the chosen plane as possible
363
364 //to make the simulation more continuous, we don't choose, but
365 //we use the height as a weight
366
367 m_Ground.nor = CVector(0,0,0);
368 m_Ground.d = 0.0;
369
370 float weight[4];
371 for(unsigned int i=0; i < 4; i++)
372 {
373 float height = contactPoint[i].dotProduct(planes[i].nor) - planes[i].d;
374 if(height < 0.0)
375 {weight[i] = 1.0;}
376 else
377 {weight[i] = 0.1;}
378 //weight[i] = exp(-height); //always positive, >> 0 for height = 0
379
380 m_Ground.nor += weight[i] * planes[i].nor;
381 m_Ground.d += weight[i] * planes[i].d;
382 }
383 float weightsum = weight[0] + weight[1] + weight[2] + weight[3];
384 if(fabs(weightsum) > 0.01)
385 {
386 m_Ground.d /= weightsum;
387 }
388 else
389 {
390 m_Ground.d = 0.0;
391 }
392 m_Ground.nor.normalise();
393
394 float norcomp0, norcomp1, norcomp2, norcomp3;
395 norcomp0 = wheelGround[0].nor.dotProduct(m_Ground.nor);
396 norcomp1 = wheelGround[1].nor.dotProduct(m_Ground.nor);
397 norcomp2 = wheelGround[2].nor.dotProduct(m_Ground.nor);
398 norcomp3 = wheelGround[3].nor.dotProduct(m_Ground.nor);
399
400 if(norcomp0 > norcomp1 && norcomp0 > norcomp2 && norcomp0 > norcomp3)
401 {
402 m_Ground.material = wheelGround[0].material;
403 m_Ground.isSurface = wheelGround[0].isSurface;
404 m_Ground.isWater = wheelGround[0].isWater;
405 }
406 else if(norcomp1 > norcomp2 && norcomp1 > norcomp3)
407 {
408 m_Ground.material = wheelGround[1].material;
409 m_Ground.isSurface = wheelGround[1].isSurface;
410 m_Ground.isWater = wheelGround[1].isWater;
411 }
412 else if(norcomp2 > norcomp3)
413 {
414 m_Ground.material = wheelGround[2].material;
415 m_Ground.isSurface = wheelGround[2].isSurface;
416 m_Ground.isWater = wheelGround[2].isWater;
417 }
418 else
419 {
420 m_Ground.material = wheelGround[3].material;
421 m_Ground.isSurface = wheelGround[3].isSurface;
422 m_Ground.isWater = wheelGround[3].isWater;
423 }
424
425 break;
426 }
427 case 3:
428 {
429 CVector line1 = contactPoint[1] - contactPoint[0];
430 CVector line2 = contactPoint[2] - contactPoint[0];
431
432 m_Ground.nor = line1.crossProduct(line2).normal();
433
434 CVector norsum = wheelGround[0].nor + wheelGround[1].nor + wheelGround[2].nor;
435
436 if(norsum.dotProduct(m_Ground.nor) < 0.0) //the real normals point to the other side
437 m_Ground.nor = -m_Ground.nor;
438
439 m_Ground.d = contactPoint[0].dotProduct(m_Ground.nor);
440
441 float norcomp0, norcomp1, norcomp2;
442 norcomp0 = wheelGround[0].nor.dotProduct(m_Ground.nor);
443 norcomp1 = wheelGround[1].nor.dotProduct(m_Ground.nor);
444 norcomp2 = wheelGround[2].nor.dotProduct(m_Ground.nor);
445
446 if(norcomp0 > norcomp1 && norcomp0 > norcomp2)
447 {
448 m_Ground.material = wheelGround[0].material;
449 m_Ground.isSurface = wheelGround[0].isSurface;
450 m_Ground.isWater = wheelGround[0].isWater;
451 }
452 else if(norcomp1 > norcomp2)
453 {
454 m_Ground.material = wheelGround[1].material;
455 m_Ground.isSurface = wheelGround[1].isSurface;
456 m_Ground.isWater = wheelGround[1].isWater;
457 }
458 else
459 {
460 m_Ground.material = wheelGround[2].material;
461 m_Ground.isSurface = wheelGround[2].isSurface;
462 m_Ground.isWater = wheelGround[2].isWater;
463 }
464
465 break;
466 }
467 case 2:
468 {
469 CVector line = (contactPoint[1] - contactPoint[0]).normal();
470
471 CVector nor0_comp = wheelGround[0].nor - wheelGround[0].nor.component(line);
472 CVector nor1_comp = wheelGround[1].nor - wheelGround[1].nor.component(line);
473
474 m_Ground.nor = (nor0_comp + nor1_comp).normal();
475 m_Ground.d = contactPoint[0].dotProduct(m_Ground.nor);
476
477 if(nor0_comp.abs2() > nor1_comp.abs2())
478 {
479 m_Ground.material = wheelGround[0].material;
480 m_Ground.isSurface = wheelGround[0].isSurface;
481 m_Ground.isWater = wheelGround[0].isWater;
482 }
483 else
484 {
485 m_Ground.material = wheelGround[1].material;
486 m_Ground.isSurface = wheelGround[1].isSurface;
487 m_Ground.isWater = wheelGround[1].isWater;
488 }
489
490 break;
491 }
492 case 1:
493 {
494 m_Ground = wheelGround[0];
495 break;
496 }
497 case 0:
498 {
499 m_Ground.nor = CVector(0,0,0);
500 break;
501 }
502 }
503
504 //back to absolute coordinates:
505 m_Ground.d += m_Position.dotProduct(m_Ground.nor);
506 }
507
placeOnGround()508 void CCar::placeOnGround()
509 {
510 //Place wheels on ground
511 for(unsigned int i=0; i < 4; i++)
512 {
513 const CCollisionFace &ground = m_Wheel[i].m_Ground;
514 float ndoty = ground.nor.dotProduct(m_OrientationMatrix * CVector(0,1,0));
515
516 if(ndoty < 0.1)
517 {
518 m_Wheel[i].m_Height = 0.0;
519 m_Wheel[i].m_dHeight = 0.0;
520 continue;
521 }
522
523 float ndoty_inv = 1.0 / ndoty;
524
525 CVector neutral_rel = m_OrientationMatrix * m_Wheel[i].m_NeutralPos;
526 CVector neutral = m_Position + neutral_rel;
527 m_Wheel[i].m_Height = ndoty_inv * (ground.d + m_Wheel[i].m_Radius - ground.nor.dotProduct(neutral));
528
529 //Assuming that ndoty is more or less constant:
530 m_Wheel[i].m_dHeight = -ndoty_inv * ground.nor.dotProduct(
531 m_Velocity - m_AngularVelocity.crossProduct(neutral_rel));
532
533 if(m_Wheel[i].m_Height < 0.0)
534 {
535 m_Wheel[i].m_Height = 0.0;
536 m_Wheel[i].m_dHeight = 0.0;
537 }
538 }
539 }
540
landOnGround()541 void CCar::landOnGround()
542 {
543 /*
544 All models are wrong, but some are useful.
545 G.E.P. Box
546 */
547
548 if(m_Ground.nor.abs2() < 0.25) return;
549
550 //The right rotation:
551 CMatrix &Mnu = m_OrientationMatrix;
552 CVector ynu;
553 ynu.x = Mnu.Element(1,0);
554 ynu.y = Mnu.Element(1,1);
555 ynu.z = Mnu.Element(1,2);
556
557 CVector ystr = m_Ground.nor;
558 CVector cp = ystr.crossProduct(ynu);
559
560 float abscp = cp.abs();
561 if(abscp > 0.001) cp *= (asin(abscp) / abscp);
562
563 CMatrix rotate;
564 rotate.setRotation(cp);
565
566 Mnu *= rotate;
567
568 //align the angular velocity
569 m_AngularVelocity = m_AngularVelocity.component(m_Ground.nor);
570
571 //The right position:
572 CVector &pos = m_Position;
573 float dobj = pos.dotProduct(m_Ground.nor) - m_Ground.d;
574 pos -= (dobj - m_PositionAboveGround) * m_Ground.nor;
575
576 //align the linear velocity:
577 float vvert = m_Velocity.dotProduct(m_Ground.nor);
578 if(vvert < 0.0)
579 {
580 m_Velocity -= vvert * m_Ground.nor;
581 }
582
583 printf("Landed\n");
584 }
585
fixFlyingOrientation()586 void CCar::fixFlyingOrientation()
587 {
588 //Goal: align the nose more or less with the velocity vector
589 //More precise: the vertical y-axis is made perpendicular to
590 //the velocity vector
591
592 //don't do it at low speeds
593 if(m_Velocity.abs2() < 100.0) return;
594
595 //The Y-axis orientation:
596 CVector yNow = m_OrientationMatrix * CVector(0,1,0);
597
598 //The velocity vector, normalised:
599 CVector v = m_Velocity.normal();
600
601 //The component of the Y-axis perpendicular to v, normalised:
602 CVector yNew = (yNow - yNow.component(v)).normal();
603 if(yNew.dotProduct(yNow) < 0.0) yNew = -yNew;
604
605 //The rotation axis
606 CVector rAxis = yNew.crossProduct(yNow);
607 float rAxisSize = rAxis.abs();
608
609 if(rAxisSize < 0.001) return; //rotation is not needed; prevent division by zero
610
611 float angle = fabsf(asin(rAxisSize));
612
613 //Don't rotate too much:
614 angle *= 0.5; //some kind of under-compensation
615 if(angle > 0.01) angle = 0.01;
616
617 rAxis *= (angle / rAxisSize);
618
619 CMatrix rot;
620 rot.setRotation(rAxis);
621 m_OrientationMatrix *= rot;
622
623 m_AngularVelocity -= m_AngularVelocity.component(rAxis.normal());
624 }
625
update(CPhysics * simulator,float dt)626 void CCar::update(CPhysics *simulator, float dt)
627 {
628 //Reset the skid volume to 0.0
629 //Reset the torque on the wheel to 0.0
630 for(unsigned int i=0; i < 4; i++)
631 {
632 m_Wheel[i].m_SkidVolume = 0.0;
633 m_Wheel[i].m_M = 0.0;
634 }
635
636 //Override input on crash
637 if(m_RuleStatus.state == CCarRuleStatus::eCrashed)
638 {
639 m_InputData->m_Up = 0.0;
640 m_InputData->m_Forward = 0.0;
641 m_InputData->m_Backward = 1.0; //This is always the brake, even in reverse gear
642 m_InputData->m_Right = 0.0;
643 }
644
645 //Wheel stuff
646 doSteering(dt);
647
648 simulateGeneral(simulator, dt);
649
650 determineGroundPlane(simulator);
651 if(m_Ground.nor.abs2() < 0.25)
652 {
653 //fprintf(stderr, "ground plane NOT found\n");
654 simulateAir(simulator, dt);
655 }
656 else
657 {
658 float vvert = m_Velocity.dotProduct(m_Ground.nor);
659 float dobj = m_Bodies[0].m_Position.dotProduct(m_Ground.nor) - m_Ground.d;
660 unsigned numFloatingWheels = 0;
661 for(unsigned int i=0; i<4; i++)
662 if(m_Wheel[i].m_Height < 0.001) numFloatingWheels++;
663
664 if(vvert > 0.1 && numFloatingWheels == 4)
665 {
666 //fprintf(stderr, "vertical speed pointing away from gound\n");
667 //printf("Airborne!\n");
668 simulateAir(simulator, dt);
669 }
670 else if(dobj > 10.0)
671 {
672 //fprintf(stderr, "ground plane far below\n");
673 //printf("Airborne!\n");
674 simulateAir(simulator, dt);
675 }
676 else
677 {
678 //fprintf(stderr, "ground plane found\n");
679 simulateGround(simulator, dt);
680 }
681 }
682
683 //integration step for the wheels
684 for(unsigned int i=0; i < 4; i++)
685 {
686 m_Wheel[i].m_a += m_Wheel[i].m_w * dt;
687 if(m_Wheel[i].m_a > DBLPI) m_Wheel[i].m_a -= DBLPI;
688 if(m_Wheel[i].m_a < 0.0) m_Wheel[i].m_a += DBLPI;
689 m_Wheel[i].m_w += dt * m_Wheel[i].m_M * m_Wheel[i].m_Iinv_eff;
690 }
691
692 //----------------------
693 //Integration step
694 //----------------------
695 CMovingObject::update(simulator, dt);
696 }
697
correctCollisions()698 void CCar::correctCollisions()
699 {
700 for(unsigned int c=0; c < m_SimCollisions.size(); c++)
701 {
702 CCollisionData col = m_SimCollisions[c];
703
704 //First do a crash test
705 if(col.getRadVel() > 10.0) //10 m/s = 36 km/h
706 {
707 m_RuleStatus.crash();
708 m_SimCollisions[c].fatal = true;
709 }
710
711 //Then do position correction
712 CVector dr = col.nor * col.depth;
713
714 //no correction towards the ground
715 if(m_Ground.nor.abs2() > 0.25)
716 {
717 if(col.nor.dotProduct(m_Ground.nor) < -0.9) continue;
718
719 CVector newcolnor = (col.nor - col.nor.component(m_Ground.nor)).normal();
720
721 float dotpr = newcolnor.dotProduct(col.nor);
722 if(fabs(dotpr) < 0.001) continue;
723
724 dr = newcolnor * (col.depth / dotpr);
725 }
726
727 //correct the position
728 m_Position += dr;
729
730 //set the collision velocity (relative to vmean) to zero
731 m_Velocity -= col.vmean;
732 float radcomp = m_Velocity.dotProduct(col.nor);
733 if(radcomp < 0.0)
734 m_Velocity -= radcomp * col.nor;
735 m_Velocity += col.vmean;
736 }
737 }
738
simulateGeneral(CPhysics * simulator,float dt)739 void CCar::simulateGeneral(CPhysics *simulator, float dt)
740 {
741 /*
742 I know that this defies the law of gravity, but,
743 you see, I never studied law.
744
745 Bugs Bunny
746 */
747 addForce(CVector(0,-9.81 / m_InvMass, 0));
748
749 addDownforce();
750
751 //----------------------
752 //Air resistance
753 //----------------------
754 addForce(-m_cwA * m_Velocity.abs() * m_Velocity);
755
756 //----------------------
757 //Rotation damping
758 //----------------------
759 addTorque(-m_RotationDamping * m_AngularVelocity.abs() * m_AngularVelocity);
760
761 //----------------------
762 //Engine simulation
763 //----------------------
764 m_Engine.m_Gear = ((CCarInput *)m_InputData)->m_Gear;
765 if(m_Engine.m_Gear >= m_Engine.m_GearRatios.size())
766 m_Engine.m_Gear = m_Engine.m_GearRatios.size() - 1;
767 m_Engine.m_Gas = ((CCarInput *)m_InputData)->m_Forward;
768 m_Engine.update(dt, m_Wheel[0].m_w, m_Wheel[1].m_w, m_Wheel[2].m_w, m_Wheel[3].m_w);
769
770 //----------------------
771 //Wheel simulation
772 //----------------------
773 updateWheelOrientation();
774 updateWheelTorques();
775 }
776
simulateAir(CPhysics * simulator,float dt)777 void CCar::simulateAir(CPhysics *simulator, float dt)
778 {
779 //printf("Flying\n");
780 m_SimState = eFlying;
781 for(unsigned int i=0; i<4; i++)
782 {
783 m_Wheel[i].m_Fnormal = 0.0;
784 m_Wheel[i].m_Height = 0.0;
785 m_Wheel[i].m_dHeight = 0.0;
786 }
787
788 fixFlyingOrientation();
789 }
790
simulateGround(CPhysics * simulator,float dt)791 void CCar::simulateGround(CPhysics *simulator, float dt)
792 {
793 if(m_SimState == eFlying)
794 landOnGround();
795 m_SimState = eRiding;
796 //printf("Riding\n");
797
798 placeOnGround(); //Places wheels on ground
799
800 calculateNormalForces();
801 applyWheelForces();
802 }
803
addDownforce()804 void CCar::addDownforce()
805 {
806 const CMatrix &ori = m_Bodies[0].m_OrientationMatrix;
807
808 CVector v = m_Velocity;
809
810 v /= ori;
811
812 if(v.z < 0.0) //moving forward
813 {
814 CVector frontpos(0.0, 0.0, m_Wheel[0].m_NeutralPos.z);
815 CVector rearpos(0.0, 0.0, m_Wheel[2].m_NeutralPos.z);
816
817 CVector frontF(0.0, -m_FrontDownforce*v.z*v.z, 0.0);
818 CVector rearF(0.0, -m_RearDownforce*v.z*v.z, 0.0);
819
820 frontpos *= ori;
821 rearpos *= ori;
822 frontF *= ori;
823 rearF *= ori;
824
825 addForceAt(frontF, frontpos);
826 addForceAt(rearF, rearpos);
827 }
828 }
829
doSteering(float dt)830 void CCar::doSteering(float dt)
831 {
832 CCarInput *input = (CCarInput *)m_InputData;
833 float steer = input->m_Right;
834
835 //the speed of steering
836 float steerspeed = m_SteerSpeedOut;
837 if((m_DesiredSteering < 0.0 && steer > m_DesiredSteering) ||
838 (m_DesiredSteering > 0.0 && steer < m_DesiredSteering))
839 steerspeed = m_SteerSpeedIn; //steer back to neutral is faster
840
841 //faster velocity -> slower steering
842 steerspeed /= 1.0 + m_SteerSpeed_v_factor * m_Velocity.abs();
843
844
845 //linear function:
846 float steerchange = copysign(steerspeed * dt, steer - m_DesiredSteering);
847 m_DesiredSteering += steerchange;
848 if(m_DesiredSteering < -1.0) m_DesiredSteering = -1.0;
849 if(m_DesiredSteering > 1.0) m_DesiredSteering = 1.0;
850
851 //fixing straight road driving:
852 if(fabsf(steer) < 0.05 && fabsf(m_DesiredSteering) < 0.05)
853 m_DesiredSteering = 0.0;
854
855 //if the wheels were in the middle of the car
856 float desiredfront = m_FrontSteerMax * m_DesiredSteering;
857 float desiredrear = m_RearSteerMax * m_DesiredSteering;
858
859 //desired angles
860 //important: default to zero
861 for(unsigned int i=0; i < 4; i++)
862 m_Wheel[i].m_DesiredSt = 0.0;
863
864 if(fabs(desiredfront) > 0.0001 || fabs(desiredrear) > 0.0001)
865 {
866 //This correction is actually better than that
867 //is possible in a real car
868 float lth = m_Wheel[2].m_NeutralPos.z - m_Wheel[0].m_NeutralPos.z;
869 float halfwthf = m_Wheel[1].m_NeutralPos.x;
870 float halfwthr = m_Wheel[3].m_NeutralPos.x;
871 float tanaf = tanf(desiredfront);
872 float tanar = tanf(-desiredrear);
873 float yf = lth * tanaf / (tanar + tanaf);
874 float yr = lth - yf;
875 float radius = 0.0;
876 if(fabs(tanaf) > 0.0001)
877 {radius = 0.5*lth / tanaf;} //yf / tanaf}
878 else
879 {radius = yr / tanar;}
880
881 m_Wheel[0].m_DesiredSt = atanf(yf / (radius + halfwthf));
882 m_Wheel[1].m_DesiredSt = atanf(yf / (radius - halfwthf));
883 m_Wheel[2].m_DesiredSt = -atanf(yr / (radius + halfwthr));
884 m_Wheel[3].m_DesiredSt = -atanf(yr / (radius - halfwthr));
885 }
886 }
887
updateWheelOrientation()888 void CCar::updateWheelOrientation()
889 {
890 CVector
891 z1(-sin(m_Wheel[0].m_DesiredSt), 0.0, cos(m_Wheel[0].m_DesiredSt)),
892 z2(-sin(m_Wheel[1].m_DesiredSt), 0.0, cos(m_Wheel[1].m_DesiredSt)),
893 z3(-sin(m_Wheel[2].m_DesiredSt), 0.0, cos(m_Wheel[2].m_DesiredSt)),
894 z4(-sin(m_Wheel[3].m_DesiredSt), 0.0, cos(m_Wheel[3].m_DesiredSt));
895
896 m_Wheel[0].m_Z = m_OrientationMatrix * z1;
897 m_Wheel[1].m_Z = m_OrientationMatrix * z2;
898 m_Wheel[2].m_Z = m_OrientationMatrix * z3;
899 m_Wheel[3].m_Z = m_OrientationMatrix * z4;
900 }
901
updateWheelTorques()902 void CCar::updateWheelTorques() //engine + brakes
903 {
904 CCarInput *input = (CCarInput *)m_InputData;
905
906 m_Wheel[0].m_M += m_Engine.getWheelM(0) - m_Wheel[0].getBrakeTorque(input->m_Backward);
907 m_Wheel[1].m_M += m_Engine.getWheelM(1) - m_Wheel[1].getBrakeTorque(input->m_Backward);
908 m_Wheel[2].m_M += m_Engine.getWheelM(2) - m_Wheel[2].getBrakeTorque(input->m_Backward);
909 m_Wheel[3].m_M += m_Engine.getWheelM(3) - m_Wheel[3].getBrakeTorque(input->m_Backward);
910
911 //adding the opposite torques to the body
912 for(unsigned int i=0; i < 3; i++)
913 {
914 CVector wz = m_Wheel[i].m_Z;
915
916 //make sure that the z axis is not flipped
917 if(i == 1 || i == 3)
918 wz = -wz;
919
920 //x direction: (TODO: write out explicitly; this is inefficient)
921 CVector wx = wz.crossProduct(CVector(0,-1,0));
922
923 addTorque(-m_Wheel[i].m_M * wx);
924 }
925 }
926
calculateNormalForces()927 void CCar::calculateNormalForces()
928 {
929 for(unsigned int i=0; i < 4; i++)
930 {
931 //y difference:
932 float y = m_Wheel[i].m_Height, dy = m_Wheel[i].m_dHeight;
933
934 m_Wheel[i].m_Fnormal = m_Wheel[i].m_suspk * y + m_Wheel[i].m_suspd * dy;
935 if(m_Wheel[i].m_Fnormal < 0.0) m_Wheel[i].m_Fnormal = 0.0; //no "glue" forces
936 }
937 }
938
applyWheelForces()939 void CCar::applyWheelForces()
940 {
941 //This function mainly works in the car coordinate system
942 CMatrix Rmat_inv = m_OrientationMatrix.transpose();
943
944 CVector LinVel = Rmat_inv * m_Velocity;
945 CVector AngVel = Rmat_inv * m_AngularVelocity;
946
947 for(unsigned int i=0; i < 4; i++)
948 {
949 if(m_Wheel[i].m_Ground.nor.abs2() < 0.25 || m_Wheel[i].m_Ground.material == NULL)
950 continue;
951
952 float muGround = m_Wheel[i].m_Ground.material->m_Mu;
953 float rollGround = m_Wheel[i].m_Ground.material->m_Roll;
954
955 //position of the wheel:
956 CVector r = m_Wheel[i].m_NeutralPos;
957
958 //z axis:
959 CVector wz = m_Wheel[i].m_Z;
960
961 wz *= Rmat_inv;
962
963 //make sure that the z axis is not flipped
964 if(wz.z < 0.0)
965 wz = -wz;
966
967 //x direction:
968 //TODO: write out explicitly (this is inefficient)
969 CVector wx = wz.crossProduct(CVector(0,-1,0));
970
971 //velocity of the center of the wheel:
972 CVector Vcent = LinVel + r.crossProduct(AngVel);
973
974 float vlong = Vcent.dotProduct(wz); //longitudinal velocity. Positive is backward
975 float vlat = Vcent.dotProduct(wx); //lateral velocity. Positive is right
976
977 CVector Mwheel;
978 CVector Fwheel = m_Wheel[i].getGroundForce(
979 Mwheel, vlong, vlat, m_Wheel[i].m_Mu * muGround, m_Wheel[i].m_Roll * rollGround);
980
981 //Transform from wheel coordinates to car coordinates:
982 Fwheel = CVector(0,Fwheel.y,0) + Fwheel.x * wx + Fwheel.z * wz;
983
984 //feedback to the net torque on the wheel:
985 m_Wheel[i].m_M += Fwheel.z * m_Wheel[i].m_Radius + Mwheel.x;
986
987 //forces attach on surface, not on middle:
988 r += CVector(0.0, -m_Wheel[i].m_Radius, 0.0);
989
990 //fprintf(stderr, "%f\t%f\t%f\t%f\t%f\t",
991 // wz.x, wz.y, wz.z,
992 // vlong, vlat);
993
994 //Back to world orientation:
995 Fwheel *= m_OrientationMatrix; //TODO: transform to ground plane
996 r *= m_OrientationMatrix;
997
998 //add forces to the wheel
999 addForceAt(Fwheel, r);
1000 }
1001
1002 //fprintf(stderr, "\n");
1003 }
1004
getData(CBinBuffer & b) const1005 CBinBuffer &CCar::getData(CBinBuffer &b) const
1006 {
1007 CMovingObject::getData(b);
1008
1009 //general
1010 b.addFloat8(m_DesiredSteering, 0.008);
1011 //b.addFloat8(m_xAngle, 0.002);
1012 //b.addFloat8(m_zAngle, 0.002);
1013
1014 //engine
1015 b += Uint8(m_Engine.m_Gear);
1016 b.addFloat16(m_Engine.m_MainAxisW, 0.4);
1017 b.addFloat8(m_Engine.m_Gas, 0.008);
1018
1019 //wheels
1020 for(unsigned int i=0; i < 4; i++)
1021 {
1022 b.addFloat16(m_Wheel[i].m_w, 0.4);
1023 b.addFloat8(m_Wheel[i].m_a, 0.025);
1024 b.addFloat8(m_Wheel[i].m_DesiredSt, 0.008);
1025 b.addFloat8(m_Wheel[i].m_SkidVolume, 0.008);
1026 b.addFloat8(m_Wheel[i].m_Height, 0.008);
1027 }
1028
1029 //Flag data
1030 Uint8 flags = 0;
1031 if(m_AllCollisions.size() != 0)
1032 flags = flags | 0x1; //collision data will be added to the message
1033 if(m_RuleStatus.state == CCarRuleStatus::eCrashed)
1034 flags = flags | 0x2;
1035 b += flags;
1036
1037 //Add collision data
1038 if(m_AllCollisions.size() != 0)
1039 {
1040 bool fatal = false;
1041 float radMax = 0.0, tangMax = 0.0;
1042
1043 for(unsigned int i=0; i < m_AllCollisions.size(); i++)
1044 {
1045 float rad = m_AllCollisions[i].getRadVel();
1046 float tang = m_AllCollisions[i].getTangVel();
1047
1048 if(rad > radMax) radMax = rad;
1049 if(tang > tangMax) tangMax = tang;
1050 if(m_AllCollisions[i].fatal) fatal = true;
1051 }
1052
1053 if(radMax > 50.0) radMax = 50.0;
1054 if(tangMax > 50.0) tangMax = 50.0;
1055
1056 b.addFloat8(radMax, 0.40);
1057 b.addFloat8(tangMax, 0.40);
1058 b += (Uint8) fatal;
1059 }
1060
1061 return b;
1062 }
1063
setData(const CBinBuffer & b,unsigned int & pos)1064 bool CCar::setData(const CBinBuffer &b, unsigned int &pos)
1065 {
1066 if(!CMovingObject::setData(b, pos)) return false;
1067
1068 //general
1069 m_DesiredSteering = b.getFloat8(pos, 0.008);
1070 //m_xAngle = b.getFloat8(pos, 0.002);
1071 //m_zAngle = b.getFloat8(pos, 0.002);
1072
1073 //engine
1074 m_Engine.m_Gear = b.getUint8(pos);
1075 m_Engine.m_MainAxisW = b.getFloat16(pos, 0.4);
1076 m_Engine.m_Gas = b.getFloat8(pos, 0.008);
1077
1078 //wheels
1079 for(unsigned int i=0; i < 4; i++)
1080 {
1081 m_Wheel[i].m_w = b.getFloat16(pos, 0.4);
1082 m_Wheel[i].m_a = b.getFloat8(pos, 0.025);
1083 m_Wheel[i].m_DesiredSt = b.getFloat8(pos, 0.008);
1084 m_Wheel[i].m_SkidVolume = b.getFloat8(pos, 0.008);
1085 m_Wheel[i].m_Height = b.getFloat8(pos, 0.008);
1086 }
1087
1088 //Flag data
1089 Uint8 flags = b.getUint8(pos);
1090 bool hasCollisions = (flags & 0x1) != 0;
1091 if((flags & 0x2) != 0)
1092 m_RuleStatus.crash();
1093
1094 //Collision data
1095 if(hasCollisions)
1096 {
1097 float radMax = b.getFloat8(pos, 0.40);
1098 float tangMax = b.getFloat8(pos, 0.40);
1099 bool fatal = b.getUint8(pos);
1100
1101 //Make a virtual collision that mimics the collision sound
1102 CCollisionData collision;
1103 collision.fatal = fatal;
1104 collision.nor = CVector(1,0,0);
1105 collision.pos = m_Position;
1106 collision.vmean = CVector(0,0,0);
1107 collision.vdiff = CVector(radMax, 0.0, tangMax);
1108
1109 m_AllCollisions.push_back(collision);
1110 }
1111
1112 placeBodies();
1113
1114 return true;
1115 }
1116