1 // Vamos Automotive Simulator
2 // Copyright (C) 2001--2004 Sam Varner
3 //
4 // This program is free software; you can redistribute it and/or modify
5 // it under the terms of the GNU General Public License as published by
6 // the Free Software Foundation; either version 2 of the License, or
7 // (at your option) any later version.
8 //
9 // This program is distributed in the hope that it will be useful,
10 // but WITHOUT ANY WARRANTY; without even the implied warranty of
11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 // GNU General Public License for more details.
13 //
14 // You should have received a copy of the GNU General Public License
15 // along with this program; if not, write to the Free Software
16 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
17
18 #include "World.h"
19 #include "Driver.h"
20 #include "Timing_Info.h"
21 #include "../body/Wheel.h"
22
23 #include <cassert>
24 #include <limits>
25
26 using namespace Vamos_Body;
27 using namespace Vamos_Geometry;
28 using namespace Vamos_World;
29 using namespace Vamos_Track;
30
31 const double slipstream_time_constant = 0.7;
32
33 //-----------------------------------------------------------------------------
34
Car_Information(Car * car_in,Driver * driver_in)35 Car_Information::Car_Information (Car* car_in, Driver* driver_in)
36 : road_index (0),
37 segment_index (0),
38 car (car_in),
39 driver (driver_in),
40 m_record (5000)
41 {
42 }
43
44 //-----------------------------------------------------------------------------
45 void
reset()46 Car_Information::reset ()
47 {
48 road_index = 0;
49 segment_index = 0;
50 if (driver != 0)
51 driver->reset ();
52 car->reset ();
53 }
54
55 //-----------------------------------------------------------------------------
56 void
propagate(double time_step,double total_time,const Three_Vector & track_position)57 Car_Information::propagate (double time_step,
58 double total_time,
59 const Three_Vector& track_position)
60 {
61 if (driver != 0)
62 driver->propagate (time_step);
63
64 car->propagate (time_step);
65 m_record.push_back (Record (total_time, car, track_position));
66 }
67
68 //-----------------------------------------------------------------------------
Record(double time,Car * car,const Three_Vector & track_position)69 Car_Information::Record::Record (double time,
70 Car* car,
71 const Three_Vector& track_position)
72 : m_time (time),
73 m_track_position (track_position),
74 m_position (car->chassis ().position ()),
75 m_orientation (car->chassis ().orientation())
76 {
77 }
78
79 //-----------------------------------------------------------------------------
World(Vamos_Track::Strip_Track * track,Atmosphere * atmosphere)80 World::World (Vamos_Track::Strip_Track* track, Atmosphere* atmosphere)
81 : mp_track (track),
82 mp_atmosphere (atmosphere),
83 m_gravity (9.8),
84 mp_timing (0),
85 m_focused_car_index (0),
86 m_cars_can_interact (true),
87 m_has_controlled_car (false),
88 m_controlled_car_index (0)
89 {
90 }
91
~World()92 World::~World ()
93 {
94 for (std::vector <Car_Information>::iterator it = m_cars.begin ();
95 it != m_cars.end ();
96 it++)
97 {
98 delete it->car;
99 delete it->driver;
100 }
101 delete mp_timing;
102 }
103
104 void
start(size_t laps)105 World::start (size_t laps)
106 {
107 mp_timing = new Timing_Info (m_cars.size (), mp_track->timing_lines (), laps);
108 }
109
110 inline Three_Vector
rotation_term(const Inertia_Tensor & I,const Three_Vector & r,const Three_Vector & n)111 rotation_term (const Inertia_Tensor& I,
112 const Three_Vector& r,
113 const Three_Vector& n)
114 {
115 return (I.inverse () * (r.cross (n))).cross (r);
116 }
117
118 Three_Vector
impulse(const Three_Vector & r1,double m1,const Inertia_Tensor & I1,const Three_Vector & r2,double m2,const Inertia_Tensor & I2,const Three_Vector & v,double restitution,double friction,const Three_Vector & normal)119 impulse (const Three_Vector& r1,
120 double m1,
121 const Inertia_Tensor& I1,
122 const Three_Vector& r2,
123 double m2,
124 const Inertia_Tensor& I2,
125 const Three_Vector& v,
126 double restitution,
127 double friction,
128 const Three_Vector& normal)
129 {
130 return -normal * (1.0 + restitution) * v.dot (normal)
131 / (normal.dot (normal) * (1.0 / m1 + 1.0 / m2)
132 + (rotation_term (I1, r1, normal)
133 + rotation_term (I2, r2, normal)).dot (normal))
134 + friction * (v.project (normal) - v);
135 }
136
137 Three_Vector
impulse(const Three_Vector & r,const Three_Vector & v,double m,const Inertia_Tensor & I,double restitution,double friction,const Three_Vector & normal)138 impulse (const Three_Vector& r,
139 const Three_Vector& v,
140 double m,
141 const Inertia_Tensor& I,
142 double restitution,
143 double friction,
144 const Three_Vector& normal)
145 {
146 return -normal * (1.0 + restitution) * v.dot (normal)
147 / (normal.dot (normal) / m + rotation_term (I, r, normal).dot (normal))
148 + friction * (v.project (normal) - v);
149 }
150
151 void
propagate_cars(double time_step)152 World::propagate_cars (double time_step)
153 {
154 for (size_t i = 0; i < m_cars.size (); i++)
155 {
156 Car_Information& info = m_cars [i];
157 info.propagate (time_step,
158 mp_timing->total_time (),
159 mp_track->track_coordinates (info.car->center_position (),
160 info.road_index,
161 info.segment_index));
162 interact (info.car, info.road_index, info.segment_index);
163
164 double air_density_factor = 1.0;
165 if (m_cars_can_interact)
166 {
167 for (size_t j = 0; j < m_cars.size (); j++)
168 {
169 if (j == i)
170 continue;
171
172 Car_Information& other = m_cars [j];
173 collide (&info, &other);
174 air_density_factor = std::min (air_density_factor,
175 slipstream_air_density_factor (info, other));
176 }
177 }
178
179 // Handle air resistance.
180 info.car->wind (mp_atmosphere->velocity (),
181 mp_atmosphere->density () * air_density_factor);
182 info.driver->set_air_density_factor (air_density_factor);
183 }
184 }
185
186 // Return the fraction of air density at car1 due to the slipstream of car2.
187 double
slipstream_air_density_factor(Car_Information & car1,Car_Information & car2)188 World::slipstream_air_density_factor (Car_Information& car1, Car_Information& car2)
189 {
190 double factor = 1.0;
191
192 if (car1.road_index != car2.road_index)
193 return factor;
194
195 Three_Vector p1 = mp_track->track_coordinates (car1.car->center_position (),
196 car1.road_index,
197 car1.segment_index);
198 Three_Vector p2 = mp_track->track_coordinates (car2.car->center_position (),
199 car2.road_index,
200 car2.segment_index);
201
202 const Vamos_Track::Road& road = mp_track->get_road (car1.road_index);
203
204 if (road.distance (p1.x, p2.x) > 0.0)
205 return factor;
206
207 for (size_t i = car2.m_record.size (); i > 0; i--)
208 {
209 const Three_Vector& p2 = car2.m_record [i - 1].m_track_position;
210 if (road.distance (p1.x, p2.x) > 0.0)
211 {
212 const double now = mp_timing->total_time ();
213 const double longitudinal
214 = std::exp ((car2.m_record [i - 1].m_time - now)
215 / slipstream_time_constant);
216 const double transverse = longitudinal
217 * std::max (1.0 - std::abs (p2.y - p1.y) / car2.car->width (),
218 0.0);
219 return 1.0 - longitudinal * transverse;
220 }
221 }
222
223 return factor;
224 }
225
226 void
interact(Car * car,size_t road_index,size_t segment_index)227 World::interact (Car* car,
228 size_t road_index,
229 size_t segment_index)
230 {
231 size_t i = 0;
232 for (std::vector <Particle*>::iterator
233 it = car->chassis ().particles ().begin ();
234 it != car->chassis ().particles ().end ();
235 it++, i++)
236 {
237 const Three_Vector& pos = car->chassis ().contact_position (*it);
238 double bump_parameter =
239 car->distance_traveled () + (*it)->position ().x;
240 const Contact_Info info = mp_track->test_for_contact (pos,
241 bump_parameter,
242 road_index,
243 segment_index);
244
245 const Three_Vector& velocity = car->chassis ().velocity (*it);
246 if (info.contact)
247 {
248 Three_Vector j = impulse (car->chassis ().world_moment (pos),
249 velocity,
250 car->chassis ().mass (),
251 car->chassis ().inertia (),
252 (*it)->material ().restitution_factor ()
253 * info.material.restitution_factor (),
254 (*it)->material ().friction_factor ()
255 * info.material.friction_factor (),
256 info.normal);
257
258 car->chassis ().contact (*it,
259 j,
260 velocity,
261 info.depth,
262 info.normal,
263 info.material);
264
265 Three_Vector v_perp = velocity.project (info.normal);
266 Three_Vector v_par = velocity - v_perp;
267 m_interaction_info.
268 push_back (Interaction_Info (car,
269 (*it)->material ().type (),
270 info.material.type (),
271 v_par.magnitude (),
272 v_perp.magnitude ()));
273 }
274 }
275
276 // Check for contact with track objects.
277 for (std::vector <Vamos_Track::Track_Object>::const_iterator
278 object = mp_track->objects ().begin ();
279 object != mp_track->objects ().end ();
280 object++)
281 {
282 const Contact_Info info = car->collision (object->position,
283 Three_Vector (),
284 true);
285
286 if (info.contact)
287 {
288 Three_Vector velocity = car->chassis ().velocity
289 (car->chassis ().transform_from_world (object->position));
290 Three_Vector j = impulse (car->chassis ().world_moment (object->position),
291 velocity,
292 car->chassis ().mass (),
293 car->chassis ().inertia (),
294 object->material.restitution_factor ()
295 * info.material.restitution_factor (),
296 object->material.friction_factor ()
297 * info.material.friction_factor (),
298 info.normal);
299
300 car->chassis ().temporary_contact
301 (object->position,
302 j,
303 velocity,
304 info.depth,
305 info.normal,
306 info.material);
307
308 Three_Vector v_perp = velocity.project (info.normal);
309 Three_Vector v_par = velocity - v_perp;
310 m_interaction_info.
311 push_back (Interaction_Info (car,
312 object->material.type (),
313 info.material.type (),
314 v_par.magnitude (),
315 v_perp.magnitude ()));
316 }
317 }
318 }
319
320 void
collide(Car_Information * car1_info,Car_Information * car2_info)321 World::collide (Car_Information* car1_info, Car_Information* car2_info)
322 {
323 Car* car1 = car1_info->car;
324 Car* car2 = car2_info->car;
325 assert (car1 != car2);
326
327 const Three_Vector delta_r = car1->chassis ().cm_position ()
328 - car2->chassis ().cm_position ();
329
330 // Ignore cars that are too far away to make contact.
331 if (delta_r.magnitude () > 1.5 * car2->length ())
332 return;
333
334 const Three_Vector delta_v = car1->chassis ().cm_velocity ()
335 - car2->chassis ().cm_velocity ();
336
337 // Handle collisions between the contact points of car 1 and the
338 // crash box of car 2.
339 for (std::vector <Particle*>::iterator
340 it = car1->chassis ().particles ().begin ();
341 it != car1->chassis ().particles ().end ();
342 it++)
343 {
344 const Three_Vector& pos = car1->chassis ().contact_position (*it);
345 const Contact_Info info = car2->collision (pos, car1->chassis ().velocity (*it));
346
347 if (info.contact)
348 {
349 const Three_Vector& velocity = car1->chassis ().velocity (*it)
350 - car2->chassis ().velocity (*it);
351 Three_Vector j = impulse (car1->chassis ().world_moment (pos),
352 car1->chassis ().mass (),
353 car1->chassis ().inertia (),
354 car2->chassis ().world_moment (pos),
355 car2->chassis ().mass (),
356 car2->chassis ().inertia (),
357 delta_v,
358 (*it)->material ().restitution_factor ()
359 * (*it)->material ().restitution_factor (),
360 (*it)->material ().friction_factor ()
361 * (*it)->material ().friction_factor (),
362 info.normal);
363
364 car1->chassis ().contact (*it,
365 j,
366 delta_v,
367 info.depth,
368 info.normal,
369 info.material);
370
371 car2->chassis ().temporary_contact (car1->chassis ().contact_position (*it),
372 -j,
373 -delta_v,
374 info.depth,
375 -info.normal,
376 info.material);
377
378 Three_Vector v_perp = velocity.project (info.normal);
379 Three_Vector v_par = velocity - v_perp;
380 m_interaction_info.
381 push_back (Interaction_Info (car1,
382 info.material.type (),
383 info.material.type (),
384 v_par.magnitude (),
385 v_perp.magnitude ()));
386 }
387 }
388 }
389
390 // Place the car back on the track at its current position.
391 void
reset()392 World::reset ()
393 {
394 if (!m_has_controlled_car)
395 return;
396
397 size_t& segment_index = controlled_car ()->segment_index;
398 size_t& road_index = controlled_car ()->road_index;
399 Car* car = controlled_car ()->car;
400 car->reset ();
401 place_car (car,
402 mp_track->reset_position (car->chassis ().position (),
403 road_index,
404 segment_index),
405 mp_track->get_road (road_index));
406 }
407
408 // Place the car back on the track at the starting line.
409 void
restart()410 World::restart ()
411 {
412 mp_timing->reset ();
413 if (m_has_controlled_car)
414 controlled_car ()->reset ();
415 }
416
417 // Set the acceleration due to gravity. Always downward, regardless
418 // of sign.
419 void
gravity(double g)420 World::gravity (double g)
421 {
422 m_gravity = std::abs (g);
423 for (std::vector <Car_Information>::iterator it = m_cars.begin ();
424 it != m_cars.end ();
425 it++)
426 {
427 if (it->car != 0)
428 it->car->chassis ().gravity (Three_Vector (0.0, 0.0, -m_gravity));
429 }
430 }
431
432 void
place_car(Car * car,const Three_Vector & track_pos,const Road & road)433 World::place_car (Car* car, const Three_Vector& track_pos, const Road& road)
434 {
435 const Road_Segment& segment = *road.segment_at (track_pos.x);
436
437 car->chassis ().reset (0.0);
438
439 // Move the car to its initial x-y position.
440 car->chassis ().set_position (road.position (track_pos.x, track_pos.y));
441
442 // Orient the car to be level with the track.
443 {
444 Three_Matrix orientation;
445 double along = track_pos.x - segment.start_distance ();
446 double angle = segment.angle (along);
447 orientation.rotate (Three_Vector (0.0, 0.0, angle));
448 orientation.rotate (Three_Vector (-segment.banking ().angle (along), 0.0, 0.0));
449 Two_Vector up = road.elevation ().normal (track_pos.x);
450 orientation.rotate (Three_Vector (0.0, atan2 (up.x, up.y), 0.0));
451 car->chassis ().set_orientation (orientation);
452 }
453
454 // Raise the car to the requested height above the track.
455 double gap = std::numeric_limits <double>::max ();
456 for (std::vector <Particle*>::const_iterator it = car->chassis ().particles ().begin ();
457 it != car->chassis ().particles ().end ();
458 it++)
459 {
460 Three_Vector p = car->chassis ().transform_to_world ((*it)->contact_position ());
461 gap = std::min (gap, p.z - segment.world_elevation (p));
462 }
463 car->chassis ().translate (Three_Vector (0.0, 0.0, track_pos.z - gap));
464 }
465
466 void
add_car(Car * car,Driver * driver,const Road & road,bool controlled)467 World::add_car (Car* car, Driver* driver, const Road& road, bool controlled)
468 {
469 if (driver != 0)
470 driver->set_cars (&m_cars);
471 car->chassis ().gravity (Three_Vector (0.0, 0.0, -m_gravity));
472 m_cars.push_back (Car_Information (car, driver));
473
474 place_car (car, car->chassis ().position (), road);
475
476 if (controlled)
477 set_controlled_car (m_cars.size () - 1);
478 }
479
480 void
set_focused_car(size_t index)481 World::set_focused_car (size_t index)
482 {
483 assert (index < m_cars.size ());
484 m_focused_car_index = index;
485 }
486
487 void
focus_other_car(int delta)488 World::focus_other_car (int delta)
489 {
490 set_focused_car ((m_focused_car_index + delta) % m_cars.size ());
491 }
492
493 Car_Information*
focused_car()494 World::focused_car ()
495 {
496 if (m_focused_car_index >= m_cars.size ()) return 0;
497 return &m_cars [m_focused_car_index];
498 }
499
500 void
set_controlled_car(size_t index)501 World::set_controlled_car (size_t index)
502 {
503 assert (index < m_cars.size ());
504 m_has_controlled_car = true;
505 m_controlled_car_index = index;
506 }
507
508 Car_Information*
controlled_car()509 World::controlled_car ()
510 {
511 if (!m_has_controlled_car || (m_controlled_car_index >= m_cars.size ()))
512 return 0;
513 return &m_cars [m_controlled_car_index];
514 }
515