1 #include "pch.h"
2 #include "par.h"
3 #include "cardynamics.h"
4 #include "tobullet.h"
5 #include "../ogre/common/Def_Str.h"
6
7
GetWheelPosition(WHEEL_POSITION wp) const8 MATHVECTOR<Dbl,3> CARDYNAMICS::GetWheelPosition(WHEEL_POSITION wp) const
9 {
10 MATHVECTOR<Dbl,3> pos = GetLocalWheelPosition(wp, suspension[wp].GetDisplacementPercent());
11 chassisRotation.RotateVector(pos);
12 return pos + chassisPosition;
13 }
14
GetWheelPosition(WHEEL_POSITION wp,Dbl displacement_percent) const15 MATHVECTOR<Dbl,3> CARDYNAMICS::GetWheelPosition(WHEEL_POSITION wp, Dbl displacement_percent) const
16 {
17 MATHVECTOR<Dbl,3> pos = GetLocalWheelPosition(wp, displacement_percent);
18 chassisRotation.RotateVector(pos);
19 return pos + chassisPosition;
20 }
21
GetWheelOrientation(WHEEL_POSITION wp) const22 QUATERNION<Dbl> CARDYNAMICS::GetWheelOrientation(WHEEL_POSITION wp) const
23 {
24 QUATERNION<Dbl> siderot;
25 if(wp == FRONT_RIGHT || wp == REAR_RIGHT)
26 {
27 siderot.Rotate(PI_d, 0, 0, 1);
28 }
29 return chassisRotation * GetWheelSteeringAndSuspensionOrientation(wp) * wheel[wp].GetOrientation() * siderot;
30 }
31
GetUprightOrientation(WHEEL_POSITION wp) const32 QUATERNION<Dbl> CARDYNAMICS::GetUprightOrientation(WHEEL_POSITION wp) const
33 {
34 return chassisRotation * GetWheelSteeringAndSuspensionOrientation(wp);
35 }
36
GetMass() const37 float CARDYNAMICS::GetMass() const
38 {
39 return body.GetMass();
40 }
41
GetSpeed() const42 Dbl CARDYNAMICS::GetSpeed() const
43 {
44 return body.GetVelocity().Magnitude();
45 //return chassis->getLinearVelocity().length();
46 }
47
GetSpeedDir() const48 Dbl CARDYNAMICS::GetSpeedDir() const
49 {
50 MATHVECTOR<Dbl,3> v(1, 0, 0);
51 Orientation().RotateVector(v);
52
53 Dbl vel = body.GetVelocity().dot(v); // car body vel in local car direction
54 return sqrt(vel*vel);
55 }
56
GetVelocity() const57 MATHVECTOR<Dbl,3> CARDYNAMICS::GetVelocity() const
58 {
59 return body.GetVelocity();
60 //return ToMathVector<Dbl>(chassis->getLinearVelocity());
61 }
62
GetAngularVelocity() const63 MATHVECTOR<Dbl,3> CARDYNAMICS::GetAngularVelocity() const
64 {
65 return body.GetAngularVelocity();
66 //return ToMathVector<Dbl>(chassis->getAngularVelocity());
67 }
68
GetEnginePosition() const69 MATHVECTOR<Dbl,3> CARDYNAMICS::GetEnginePosition() const
70 {
71 MATHVECTOR<Dbl,3> offset = engine.GetPosition();
72 Orientation().RotateVector(offset);
73 return offset + chassisPosition;
74 }
75
StartEngine()76 void CARDYNAMICS::StartEngine()
77 {
78 engine.StartEngine();
79 }
SetClutch(float value)80 void CARDYNAMICS::SetClutch(float value)
81 {
82 clutch.SetClutch(value);
83 }
84
GetThrottle() const85 float CARDYNAMICS::GetThrottle() const
86 {
87 if (vtype == V_Spaceship) return hov_throttle; //
88 return engine.GetThrottle();
89 }
90
SetThrottle(float value)91 void CARDYNAMICS::SetThrottle(float value)
92 {
93 /// <><> damage reduce from 50 %
94 float dmg = fDamage >= 100.f ? 0.f : (1.f - 0.6f * std::max(0.f, fDamage-50.f)/50.f);
95
96 if (vtype != V_Car)
97 hov_throttle = value * dmg;
98 else
99 engine.SetThrottle(value * dmg);
100 }
101
SetBrake(float value)102 void CARDYNAMICS::SetBrake(float value)
103 {
104 /// <><> damage reduce from 50 %
105 float dmg = 1.f - std::max(0.f, fDamage-50.f)/50.f * 0.6f;
106
107 for (size_t i = 0; i < brake.size(); ++i)
108 brake[i].SetBrakeFactor(fDamage >= 100.f ? 0.1f : value * dmg);
109 }
110
SetHandBrake(float value)111 void CARDYNAMICS::SetHandBrake(float value)
112 {
113 /// <><> damage reduce from 50 %
114 float dmg = fDamage >= 100.f ? 0.f : (1.f - std::max(0.f, fDamage-50.f)/50.f * 0.6f);
115
116 for (size_t i = 0; i < brake.size(); ++i)
117 brake[i].SetHandbrakeFactor(value * dmg);
118 }
119
SetAutoClutch(bool value)120 void CARDYNAMICS::SetAutoClutch(bool value) { autoclutch = value; }
SetAutoShift(bool value)121 void CARDYNAMICS::SetAutoShift(bool value) { autoshift = value; }
SetAutoRear(bool value)122 void CARDYNAMICS::SetAutoRear(bool value) { autorear = value; }
123
GetTachoRPM() const124 Dbl CARDYNAMICS::GetTachoRPM() const { return tacho_rpm; }
125
SetABS(const bool newabs)126 void CARDYNAMICS::SetABS(const bool newabs) { abs = newabs; }
GetABSEnabled() const127 bool CARDYNAMICS::GetABSEnabled() const { return abs; }
GetABSActive() const128 bool CARDYNAMICS::GetABSActive() const
129 {
130 if (numWheels < 4)
131 return abs && ( abs_active[0]||abs_active[1] );
132 else
133 return abs && ( abs_active[0]||abs_active[1]||abs_active[2]||abs_active[3] );
134 }
SetTCS(const bool newtcs)135 void CARDYNAMICS::SetTCS(const bool newtcs) { tcs = newtcs; }
GetTCSEnabled() const136 bool CARDYNAMICS::GetTCSEnabled() const { return tcs; }
GetTCSActive() const137 bool CARDYNAMICS::GetTCSActive() const
138 {
139 if (numWheels < 4)
140 return tcs && ( tcs_active[0]||tcs_active[1] );
141 else
142 return tcs && ( tcs_active[0]||tcs_active[1]||tcs_active[2]||tcs_active[3] );
143 }
144
145
SetPosition(const MATHVECTOR<Dbl,3> & position)146 void CARDYNAMICS::SetPosition(const MATHVECTOR<Dbl,3> & position)
147 {
148 body.SetPosition(position);
149 if (chassis)
150 chassis->translate(ToBulletVector(position) - chassis->getCenterOfMassPosition());
151 }
152
153 //find the precise starting position for the car (trim out the extra space)
AlignWithGround()154 void CARDYNAMICS::AlignWithGround()
155 {
156 UpdateWheelTransform();
157 UpdateWheelContacts();
158
159 /*Dbl min_height = 0;
160 bool no_min_height = true;
161 for (int i = 0; i < WHEEL_POSITION_SIZE; ++i)
162 {
163 Dbl height = wheel_contact[i].GetDepth() - 2 * tire[i].GetRadius();
164 if (height < min_height || no_min_height)
165 {
166 min_height = height;
167 no_min_height = false;
168 }
169 }/**/ //--
170
171 //MATHVECTOR<Dbl,3> trimmed_position = Position() + GetDownVector() * min_height;
172 //SetPosition(Position()/*trimmed_position*/);
173 }
174
GetTotalAero() const175 MATHVECTOR<Dbl,3> CARDYNAMICS::GetTotalAero() const
176 {
177 MATHVECTOR<Dbl,3> downforce = 0;
178 for (std::vector <CARAERO>::const_iterator i = aerodynamics.begin(); i != aerodynamics.end(); ++i)
179 downforce = downforce + i->GetLiftVector() + i->GetDragVector();
180 return downforce;
181 }
182
GetAerodynamicDownforceCoefficient() const183 Dbl CARDYNAMICS::GetAerodynamicDownforceCoefficient() const
184 {
185 Dbl coeff = 0.0;
186 for (std::vector <CARAERO>::const_iterator i = aerodynamics.begin(); i != aerodynamics.end(); ++i)
187 coeff += i->GetAerodynamicDownforceCoefficient();
188 return coeff;
189 }
190
GetAeordynamicDragCoefficient() const191 Dbl CARDYNAMICS::GetAeordynamicDragCoefficient() const
192 {
193 Dbl coeff = 0.0;
194 for (std::vector <CARAERO>::const_iterator i = aerodynamics.begin(); i != aerodynamics.end(); ++i)
195 coeff += i->GetAeordynamicDragCoefficient();
196 return coeff;
197 }
198
199
GetDownVector() const200 MATHVECTOR<Dbl,3> CARDYNAMICS::GetDownVector() const
201 {
202 MATHVECTOR<Dbl,3> v(0, 0, -1);
203 Orientation().RotateVector(v);
204 return v;
205 }
206
Orientation() const207 QUATERNION<Dbl> CARDYNAMICS::Orientation() const
208 {
209 return body.GetOrientation();
210 //return ToMathQuaternion<Dbl>(chassis->getOrientation());
211 }
212
Position() const213 MATHVECTOR<Dbl,3> CARDYNAMICS::Position() const
214 {
215 return body.GetPosition();
216 //return ToMathVector<Dbl>(chassis->getCenterOfMassPosition());
217 }
218
LocalToWorld(const MATHVECTOR<Dbl,3> & local) const219 MATHVECTOR<Dbl,3> CARDYNAMICS::LocalToWorld(const MATHVECTOR<Dbl,3> & local) const
220 {
221 MATHVECTOR<Dbl,3> position = local - center_of_mass;
222 body.GetOrientation().RotateVector(position);
223 return position + body.GetPosition();
224 //btVector3 position = chassis->getCenterOfMassTransform().getBasis() * ToBulletVector(local - center_of_mass);
225 //position = position + chassis->getCenterOfMassTransform().getOrigin();
226 //return ToMathVector <Dbl> (position);
227 }
228
229 //simple hinge (arc) suspension displacement
GetLocalWheelPosition(WHEEL_POSITION wp,Dbl displacement_percent) const230 MATHVECTOR<Dbl,3> CARDYNAMICS::GetLocalWheelPosition(WHEEL_POSITION wp, Dbl displacement_percent) const
231 {
232 //const
233 const MATHVECTOR<Dbl,3> & wheelext = wheel[wp].GetExtendedPosition();
234 const MATHVECTOR<Dbl,3> & hinge = suspension[wp].GetHinge();
235 MATHVECTOR<Dbl,3> relwheelext = wheelext - hinge;
236 MATHVECTOR<Dbl,3> up(0,0,1);
237 MATHVECTOR<Dbl,3> rotaxis = up.cross ( relwheelext.Normalize() );
238 Dbl hingeradius = relwheelext.Magnitude();
239 Dbl travel = suspension[wp].GetTravel();
240 //const
241
242 Dbl displacement = displacement_percent * travel;
243 Dbl displacementradians = displacement / hingeradius;
244 QUATERNION<Dbl> hingerotate;
245 hingerotate.Rotate ( -displacementradians, rotaxis[0], rotaxis[1], rotaxis[2] );
246 MATHVECTOR<Dbl,3> localwheelpos = relwheelext;
247 hingerotate.RotateVector ( localwheelpos );
248 return localwheelpos + hinge;
249 }
250
251 ///returns the orientation of the wheel due only to steering and suspension
GetWheelSteeringAndSuspensionOrientation(WHEEL_POSITION wp) const252 QUATERNION<Dbl> CARDYNAMICS::GetWheelSteeringAndSuspensionOrientation(WHEEL_POSITION wp) const
253 {
254 QUATERNION<Dbl> steer;
255 steer.Rotate( -wheel[wp].GetSteerAngle() * PI_d/180.0, 0,0,1);
256
257 QUATERNION<Dbl> camber;
258 Dbl camber_rotation = -suspension[wp].GetCamber() * PI_d/180.0;
259 if (wp%2 == 1)
260 camber_rotation = -camber_rotation;
261 camber.Rotate( camber_rotation, 1,0,0);
262
263 QUATERNION<Dbl> toe;
264 Dbl toe_rotation = suspension[wp].GetToe() * PI_d/180.0;
265 if (wp%2 == 0)
266 toe_rotation = -toe_rotation;
267 toe.Rotate( toe_rotation, 0,0,1);
268
269 return camber * toe * steer;
270 }
271
272 /// worldspace position of the center of the wheel when the suspension is compressed
273 /// by the displacement_percent where 1.0 is fully compressed
GetWheelPositionAtDisplacement(WHEEL_POSITION wp,Dbl displacement_percent) const274 MATHVECTOR<Dbl,3> CARDYNAMICS::GetWheelPositionAtDisplacement(WHEEL_POSITION wp, Dbl displacement_percent) const
275 {
276 return LocalToWorld(GetLocalWheelPosition(wp, displacement_percent));
277 }
278
ApplyForce(const MATHVECTOR<Dbl,3> & force)279 void CARDYNAMICS::ApplyForce(const MATHVECTOR<Dbl,3> & force)
280 {
281 body.ApplyForce(force);
282 cam_body.ApplyForce(force * gPar.camBncF * fBncMass);
283 //chassis->applyCentralForce(ToBulletVector(force));
284 }
285
ApplyForce(const MATHVECTOR<Dbl,3> & force,const MATHVECTOR<Dbl,3> & offset)286 void CARDYNAMICS::ApplyForce(const MATHVECTOR<Dbl,3> & force, const MATHVECTOR<Dbl,3> & offset)
287 {
288 body.ApplyForce(force, offset);
289 MATHVECTOR<Dbl,3> fo = offset * force.Magnitude();
290 cam_body.ApplyForce((force * gPar.camBncFo + fo * gPar.camBncFof) * fBncMass);
291 //chassis->applyForce(ToBulletVector(force), ToBulletVector(offset));
292 }
293
ApplyTorque(const MATHVECTOR<Dbl,3> & torque)294 void CARDYNAMICS::ApplyTorque(const MATHVECTOR<Dbl,3> & torque)
295 {
296 body.ApplyTorque(torque);
297 //if(torque.MagnitudeSquared() > 1E-6)
298 // chassis->applyTorque(ToBulletVector(torque));
299 }
300
UpdateWheelVelocity()301 void CARDYNAMICS::UpdateWheelVelocity()
302 {
303 for(int i = 0; i < numWheels; ++i)
304 {
305 wheel_velocity[i] = body.GetVelocity(wheel_position[i] - body.GetPosition());
306 //btVector3 offset = ToBulletVector(wheel_position[i]) - chassis->getCenterOfMassPosition();
307 //wheel_velocity[i] = ToMathVector<Dbl>(chassis->getVelocityInLocalPoint(offset));
308 }
309 }
310
UpdateWheelTransform()311 void CARDYNAMICS::UpdateWheelTransform()
312 {
313 for(int i = 0; i < numWheels; ++i)
314 {
315 wheel_position[i] = GetWheelPositionAtDisplacement(WHEEL_POSITION(i), suspension[i].GetDisplacementPercent());
316 wheel_orientation[i] = Orientation() * GetWheelSteeringAndSuspensionOrientation(WHEEL_POSITION(i));
317 }
318 }
319
SetDrive(const std::string & newdrive)320 void CARDYNAMICS::SetDrive(const std::string & newdrive)
321 {
322 if (newdrive == "RWD") drive = RWD;
323 else if (newdrive == "FWD") drive = FWD;
324 else if (newdrive == "AWD") drive = AWD;
325 else assert(0);
326 }
327
AddMassParticle(Dbl newmass,MATHVECTOR<Dbl,3> newpos)328 void CARDYNAMICS::AddMassParticle( Dbl newmass, MATHVECTOR<Dbl,3> newpos )
329 {
330 newpos[0] += com_ofs_L; //|
331 newpos[2] += com_ofs_H; //|
332 mass_only_particles.push_back( std::pair <Dbl, MATHVECTOR<Dbl,3> > (newmass, newpos) );
333 //std::cout << "adding mass particle " << newmass << " at " << newpos << std::endl;
334 }
335
AddAerodynamicDevice(const MATHVECTOR<Dbl,3> & newpos,Dbl drag_frontal_area,Dbl drag_coefficient,Dbl lift_surface_area,Dbl lift_coefficient,Dbl lift_efficiency)336 void CARDYNAMICS::AddAerodynamicDevice( const MATHVECTOR<Dbl,3> & newpos,
337 Dbl drag_frontal_area, Dbl drag_coefficient,
338 Dbl lift_surface_area, Dbl lift_coefficient, Dbl lift_efficiency )
339 {
340 aerodynamics.push_back( CARAERO() );
341 aerodynamics.back().Set( newpos, drag_frontal_area, drag_coefficient, lift_surface_area,
342 lift_coefficient, lift_efficiency );
343 }
344
IsBraking() const345 char CARDYNAMICS::IsBraking() const
346 {
347 // true when any wheel is braking
348 if (fDamage < 100.f)
349 for (int w=0; w < numWheels; ++w)
350 {
351 WHEEL_POSITION wp = (WHEEL_POSITION)w;
352 if (GetBrake(wp).GetBrakeFactor() > 0
353 || GetBrake(wp).GetHandbrakeFactor() > 0)
354 return 1;
355 }
356 return 0;
357 }
358