1 /*
2 * Copyright 2010, 2011, 2012, 2013, 2014, 2016 Peter Olsson
3 *
4 * This file is part of Brum Brum Rally.
5 *
6 * Brum Brum Rally is free software: you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation, either version 3 of the License, or
9 * (at your option) any later version.
10 *
11 * Brum Brum Rally is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with this program. If not, see <http://www.gnu.org/licenses/>.
18 */
19
20 #include "Track.h"
21 #include "Driver.h"
22 #include "TrackState.h"
23 #include <algorithm>
24 #include <cassert>
25 #include <climits>
26 #include <cmath>
27 #include <cstddef>
28 #include <cstdlib>
29 #include <iterator>
30 #include <limits>
31
32 const int TILE_COLLISION_LEFT = 0x01;
33 const int TILE_COLLISION_RIGHT = 0x02;
34 const int TILE_COLLISION_UP = 0x04;
35 const int TILE_COLLISION_DOWN = 0x08;
36 const int TILE_COLLISION_LEFT_UP = 0x10;
37 const int TILE_COLLISION_RIGHT_UP = 0x20;
38 const int TILE_COLLISION_LEFT_DOWN = 0x40;
39 const int TILE_COLLISION_RIGHT_DOWN = 0x80;
40
41 const int TILE_COLLISION_CORNERS = TILE_COLLISION_LEFT_UP
42 | TILE_COLLISION_RIGHT_UP
43 | TILE_COLLISION_LEFT_DOWN
44 | TILE_COLLISION_RIGHT_DOWN;
45
46 const double TIME_STATE_DURATION = 0.8;
47
48 // angle to +- PI
angle2range(double angle)49 static double angle2range(double angle)
50 {
51 return angle - 2 * PI * std::floor(angle / (2 * PI)) - PI;
52 }
53
54
Track(int laps,int numCars,const std::vector<Driver * > & drivers,const Map & map,bool carCollisionsEnabled)55 Track::Track(int laps, int numCars, const std::vector<Driver*>& drivers, const Map& map, bool carCollisionsEnabled)
56 : finishX(map.getFinishX()),
57 finishY(map.getFinishY()),
58 carsLeft(0),
59 tiles(),
60 trackLength(map.getRoadCount()),
61 laps(laps),
62 carCollisionsEnabled(carCollisionsEnabled),
63 time(0.0),
64 timeState(TIME_STATE_READY)
65 {
66 for (int y = 0; y < MAX_MAP_Y; y++)
67 {
68 for (int x = 0; x < MAX_MAP_X; x++)
69 {
70 switch (map[y][x])
71 {
72 case ROAD_HORIZONTAL:
73 tiles[y][x].collisionFlags = TILE_COLLISION_UP
74 | TILE_COLLISION_DOWN;
75 break;
76 case ROAD_VERTICAL:
77 tiles[y][x].collisionFlags = TILE_COLLISION_LEFT
78 | TILE_COLLISION_RIGHT;
79 break;
80 case ROAD_LEFT_UP:
81 tiles[y][x].collisionFlags = TILE_COLLISION_LEFT_UP
82 | TILE_COLLISION_RIGHT
83 | TILE_COLLISION_DOWN;
84 break;
85 case ROAD_LEFT_DOWN:
86 tiles[y][x].collisionFlags = TILE_COLLISION_LEFT_DOWN
87 | TILE_COLLISION_RIGHT
88 | TILE_COLLISION_UP;
89 break;
90 case ROAD_RIGHT_UP:
91 tiles[y][x].collisionFlags = TILE_COLLISION_RIGHT_UP
92 | TILE_COLLISION_LEFT
93 | TILE_COLLISION_DOWN;
94 break;
95 case ROAD_RIGHT_DOWN:
96 tiles[y][x].collisionFlags = TILE_COLLISION_RIGHT_DOWN
97 | TILE_COLLISION_LEFT
98 | TILE_COLLISION_UP;
99 break;
100 default:
101 tiles[y][x].collisionFlags = 0;
102 break;
103 }
104 }
105 }
106
107 int x = finishX;
108 int y = finishY;
109 int finishDir = map.getFinishDir();
110 int prevDir = finishDir;
111 int n = 0;
112 do
113 {
114 tiles[y][x].orderNum = n++;
115 int dir = map[y][x];
116 switch (prevDir)
117 {
118 case LEFT:
119 dir &= ~RIGHT;
120 break;
121 case RIGHT:
122 dir &= ~LEFT;
123 break;
124 case UP:
125 dir &= ~DOWN;
126 break;
127 case DOWN:
128 dir &= ~UP;
129 break;
130 }
131 tiles[y][x].dir = dir;
132 switch (dir)
133 {
134 case LEFT:
135 x--;
136 break;
137 case RIGHT:
138 x++;
139 break;
140 case UP:
141 y--;
142 break;
143 case DOWN:
144 y++;
145 break;
146 }
147 prevDir = dir;
148 } while (x != finishX || y != finishY);
149
150
151 cars.resize(numCars);
152
153 Vector finishMiddle(finishX * TILE_SIZE + TILE_SIZE / 2.0,
154 finishY * TILE_SIZE + TILE_SIZE / 2.0);
155 for (int i = 0; i < numCars; i++)
156 {
157 cars[i].position.x = numCars > 1 ? ((i % 2) ? 1 : -1) * TILE_SIZE * 0.2 : 0;
158 cars[i].position.y = 9.0 + (i / 2) * 11.0;
159
160 switch (finishDir)
161 {
162 case LEFT:
163 cars[i].position.rotate(-PI_HALF);
164 ++cars[i].position.x; // Some kind of rounding error.
165 cars[i].rotation = -PI_HALF;
166 break;
167 case RIGHT:
168 cars[i].position.rotate(PI_HALF);
169 cars[i].rotation = PI_HALF;
170 break;
171 case UP:
172 //cars[i].position.rotate(0.0);
173 cars[i].rotation = 0.0;
174 break;
175 case DOWN:
176 cars[i].position.rotate(PI);
177 cars[i].rotation = PI;
178 break;
179 }
180
181 cars[i].position += finishMiddle;
182
183 cars[i].velocity = Vector(0, 0);
184 cars[i].rotationVelocity = 0.0;
185 cars[i].driver = drivers[i];
186 cars[i].lap = 0;
187 cars[i].tileOrderNum = trackLength;
188 cars[i].posFinished = 0;
189 cars[i].timeFinished = 0.0;
190 }
191 }
192
getFinishX() const193 int Track::getFinishX() const
194 {
195 return finishX;
196 }
getFinishY() const197 int Track::getFinishY() const
198 {
199 return finishY;
200 }
201
202 // These values needs testing!
203 const double CAR_CAR_FRICTION = 0.025;
204 const double CAR_WALL_FRICTION = 0.006;
205 const double SIDE_WHEEL_FRICTION = 320.0;
206 const double ROLL_WHEEL_FRICTION = 5.0;
207 const double BRAKE_WHEEL_FRICTION = 160;
208
209 const double TURN_ANGLE = 0.41;
210 const double GAS_SPEED = 0.50 * 50;
211 const double BRAKE_SPEED = 0.40 * 50;
212
carFinishSorter(const Track::Car * car1,const Track::Car * car2)213 bool carFinishSorter(const Track::Car* car1, const Track::Car* car2)
214 {
215 return car1->timeFinished < car2->timeFinished;
216 }
217
update(double dt,Uint32 * input)218 void Track::update(double dt, Uint32* input)
219 {
220 time += dt;
221
222 if (timeState < TIME_STATE_GO)
223 {
224 if (time > TIME_STATE_DURATION)
225 {
226 ++timeState;
227 time = 0.0;
228 }
229 return;
230 }
231 else if (timeState == TIME_STATE_GO)
232 {
233 if (time > 2 * TIME_STATE_DURATION)
234 {
235 ++timeState;
236 }
237 }
238
239 std::vector<Car>::iterator c1;
240 std::vector<Car>::iterator c2;
241 if (carCollisionsEnabled)
242 {
243 for (c1 = cars.begin(); c1 != cars.end(); ++c1)
244 {
245 for (c2 = c1 + 1; c2 != cars.end(); ++c2)
246 {
247 Vector c1c2Vec = c2->position - c1->position;
248
249 double distance = c1c2Vec.length();
250 if (distance <= 2 * CAR_RADIUS)
251 {
252 // relative velocity
253 Vector relVel = c2->velocity - c1->velocity;
254
255 double pt = findCollisionTime(c1c2Vec, relVel, 2 * CAR_RADIUS);
256
257 if (-pt > dt)
258 {
259 Vector v = c1c2Vec;
260 v.normalize();
261 v *= 0.1;
262 c1->position -= v;
263 c2->position += v;
264 }
265
266 // calculate the collision direction
267 Vector colDir(c1c2Vec.x + relVel.x * pt, c1c2Vec.y + relVel.y * pt);
268
269 double colAngle = Vector::angle(colDir, VECTOR_Y);
270
271 // rotate the velocity in the collision direction
272 c1->velocity.rotate(colAngle);
273 c2->velocity.rotate(colAngle);
274
275 // the velocity in the y direction will be swapped
276 // due to the momentum. It's as easy as this because
277 // the masses of the two cars is always the same
278 std::swap(c1->velocity.y, c2->velocity.y);
279
280 // the velocity in the x direction will change the
281 // rotation speed
282 double friction = CAR_CAR_FRICTION * std::fabs(c2->velocity.y - c1->velocity.y);
283 if (friction > 1.0)
284 {
285 friction = 1.0;
286 }
287 double rotChange1 = (c2->rotationVelocity - c1->rotationVelocity) * 0.5;
288 double rotChange2 = (c1->velocity.x - c2->velocity.x) * 0.5;
289
290 c1->rotationVelocity = friction * (rotChange2 - rotChange1) + (1.0 - friction) * c1->rotationVelocity;
291 c2->rotationVelocity = friction * (rotChange2 + rotChange1) + (1.0 - friction) * c2->rotationVelocity;
292
293 // change the rotation back to world space
294 c1->velocity.rotate(-colAngle);
295 c2->velocity.rotate(-colAngle);
296
297 }
298 }
299 }
300 }
301
302 std::vector<Car*> finishedCars;
303 for (c1 = cars.begin(); c1 != cars.end(); ++c1)
304 {
305 int tilex = (int) c1->position.x / TILE_SIZE;
306 int tiley = (int) c1->position.y / TILE_SIZE;
307
308 if (tilex < 0 || tilex >= MAX_MAP_X
309 || tiley < 0 || tiley >= MAX_MAP_Y)
310 {
311 // this should not be possible
312 continue;
313 }
314
315 int colFlags = tiles[tiley][tilex].collisionFlags;
316
317 // Corner collisions
318 if (colFlags & TILE_COLLISION_CORNERS)
319 {
320 Vector prevPos = c1->position;
321 Vector prevVel = c1->velocity;
322 double prevRotVel = c1->rotationVelocity;
323 Vector tilePos(tilex * TILE_SIZE, tiley * TILE_SIZE);
324 if (colFlags & TILE_COLLISION_LEFT_UP)
325 {
326 handleCarCornerCollision(*c1, tilePos + Vector(0, 0));
327 }
328 else if (colFlags & TILE_COLLISION_RIGHT_UP)
329 {
330 handleCarCornerCollision(*c1, tilePos + Vector(TILE_SIZE, 0));
331 }
332 else if (colFlags & TILE_COLLISION_LEFT_DOWN)
333 {
334 handleCarCornerCollision(*c1, tilePos + Vector(0, TILE_SIZE));
335 }
336 else if (colFlags & TILE_COLLISION_RIGHT_DOWN)
337 {
338 handleCarCornerCollision(*c1, tilePos + Vector(TILE_SIZE, TILE_SIZE));
339 }
340
341 // When a car that has contact with a wall reaches the corner
342 // the corner collision will push the car back into the previous
343 // tile so the car gets stuck. To avoid this we need to detect
344 // when this happens and in that case undo the corner collision
345 // and do wall collision on that tile instead.
346 int tilex2 = (int) c1->position.x / TILE_SIZE;
347 int tiley2 = (int) c1->position.y / TILE_SIZE;
348 if (tilex != tilex2 || tiley != tiley2)
349 {
350 int colFlags2 = tiles[tiley2][tilex2].collisionFlags;
351 if (((colFlags & TILE_COLLISION_LEFT_UP) && colFlags2 & (tilex != tilex2 ? TILE_COLLISION_UP : TILE_COLLISION_LEFT)) ||
352 ((colFlags & TILE_COLLISION_LEFT_DOWN) && colFlags2 & (tilex != tilex2 ? TILE_COLLISION_DOWN : TILE_COLLISION_LEFT)) ||
353 ((colFlags & TILE_COLLISION_RIGHT_UP) && colFlags2 & (tilex != tilex2 ? TILE_COLLISION_UP : TILE_COLLISION_RIGHT)) ||
354 ((colFlags & TILE_COLLISION_RIGHT_DOWN) && colFlags2 & (tilex != tilex2 ? TILE_COLLISION_DOWN : TILE_COLLISION_RIGHT)))
355 {
356 c1->position = prevPos;
357 c1->velocity = prevVel;
358 c1->rotationVelocity = prevRotVel;
359 colFlags = colFlags2;
360 }
361 }
362 }
363
364 // handle collision with walls
365
366 Vector posInTile(c1->position.x - tilex * TILE_SIZE, c1->position.y - tiley * TILE_SIZE);
367
368 // collision with walls we know that the collisions on one of
369 // the axis. We only have to flip the velocity on the that axis
370 // but to make sure we don't get stuck in walls or something we
371 // use std::fabs to make sure the velocity is flipped in the
372 // correct direction
373 // We also place the car so that it does not overlap the wall.
374 // This is to prevent cars from going through walls by standing
375 // against the wall and try to accelerate.
376 if (colFlags & TILE_COLLISION_LEFT && CAR_RADIUS > posInTile.x)
377 {
378 c1->velocity.x = std::fabs(c1->velocity.x);
379 c1->position.x = tilex * TILE_SIZE + CAR_RADIUS;
380
381 double friction = CAR_WALL_FRICTION * std::fabs(c1->velocity.x);
382 if (friction > 1.0)
383 {
384 friction = 1.0;
385 }
386 c1->rotationVelocity = friction * (c1->velocity.y / CAR_RADIUS) * 0.5 + (1.0 - friction) * c1->rotationVelocity;
387 }
388 else if (colFlags & TILE_COLLISION_RIGHT && TILE_SIZE - CAR_RADIUS < posInTile.x)
389 {
390 c1->velocity.x = -std::fabs(c1->velocity.x);
391 c1->position.x = tilex * TILE_SIZE + TILE_SIZE - CAR_RADIUS;
392
393 double friction = CAR_WALL_FRICTION * std::fabs(c1->velocity.x);
394 if (friction > 1.0)
395 {
396 friction = 1.0;
397 }
398 c1->rotationVelocity = friction * (-c1->velocity.y / CAR_RADIUS) * 0.5 + (1.0 - friction) * c1->rotationVelocity;
399 }
400
401 if (colFlags & TILE_COLLISION_UP && CAR_RADIUS > posInTile.y)
402 {
403 c1->velocity.y = std::fabs(c1->velocity.y);
404 c1->position.y = tiley * TILE_SIZE + CAR_RADIUS;
405
406 double friction = CAR_WALL_FRICTION * std::fabs(c1->velocity.y);
407 if (friction > 1.0)
408 {
409 friction = 1.0;
410 }
411 c1->rotationVelocity = friction * (-c1->velocity.x / CAR_RADIUS) * 0.5 + (1.0 - friction) * c1->rotationVelocity;
412 }
413 else if (colFlags & TILE_COLLISION_DOWN && TILE_SIZE < posInTile.y + CAR_RADIUS)
414 {
415 c1->velocity.y = -std::fabs(c1->velocity.y);
416 c1->position.y = tiley * TILE_SIZE + TILE_SIZE - CAR_RADIUS;
417
418 double friction = CAR_WALL_FRICTION * std::fabs(c1->velocity.y);
419 if (friction > 1.0)
420 {
421 friction = 1.0;
422 }
423 c1->rotationVelocity = friction * (c1->velocity.x / CAR_RADIUS) * 0.5 + (1.0 - friction) * c1->rotationVelocity;
424 }
425
426
427 // count laps
428
429 bool finished = false;
430 double timeSinceFinish = 0.0;
431 if (tiles[tiley][tilex].orderNum == 0)
432 {
433 switch (tiles[finishY][finishX].dir)
434 {
435 case LEFT:
436 finished = (posInTile.x < TILE_SIZE * 0.5);
437 timeSinceFinish = (TILE_SIZE * 0.5 - posInTile.x) / -c1->velocity.x;
438 break;
439 case RIGHT:
440 finished = (posInTile.x > TILE_SIZE * 0.5);
441 timeSinceFinish = (posInTile.x - TILE_SIZE * 0.5) / c1->velocity.x;
442 break;
443 case UP:
444 finished = (posInTile.y < TILE_SIZE * 0.5);
445 timeSinceFinish = (TILE_SIZE * 0.5 - posInTile.y) / -c1->velocity.y;
446 break;
447 case DOWN:
448 finished = (posInTile.y > TILE_SIZE * 0.5);
449 timeSinceFinish = (posInTile.y - TILE_SIZE * 0.5) / c1->velocity.y;
450 break;
451 }
452 if (!finished)
453 {
454 if (c1->tileOrderNum == 0)
455 {
456 c1->lap--;
457 c1->tileOrderNum = trackLength;
458 }
459 else if (c1->tileOrderNum == trackLength - 1)
460 {
461 c1->tileOrderNum++;
462 }
463 }
464 }
465 if (finished)
466 {
467 if (c1->tileOrderNum > 1)
468 {
469 c1->lap++;
470 c1->tileOrderNum = 0;
471 if (c1->lap > laps && c1->posFinished == 0)
472 {
473 // This car has finished the race!
474
475 // Calculate finish time.
476 if (timeSinceFinish < 0)
477 {
478 timeSinceFinish = 0;
479 }
480 else if (timeSinceFinish > dt)
481 {
482 timeSinceFinish = dt;
483 }
484
485 c1->timeFinished = time - timeSinceFinish;
486
487 // Add car to vector so that finish
488 // position can be calculated correctly.
489 finishedCars.push_back(&*c1);
490 }
491 }
492 }
493 else if (c1->tileOrderNum + 1 == tiles[tiley][tilex].orderNum)
494 {
495 c1->tileOrderNum++;
496 }
497 else if (c1->tileOrderNum - 1 == tiles[tiley][tilex].orderNum)
498 {
499 c1->tileOrderNum--;
500 }
501 }
502
503 // Calculate finish positions.
504 if (!finishedCars.empty())
505 {
506 std::sort(finishedCars.begin(), finishedCars.end(), carFinishSorter);
507 std::vector<Car*>::iterator it;
508 for (it = finishedCars.begin(); it != finishedCars.end(); ++it)
509 {
510 (*it)->posFinished = ++carsLeft;
511 }
512 }
513
514 for (c1 = cars.begin(); c1 != cars.end(); ++c1)
515 {
516 // handle wheel friction
517 Vector frontWheelVelocity(c1->velocity);
518 frontWheelVelocity.rotate(-c1->rotation);
519 Vector backWheelVelocity(c1->velocity);
520 backWheelVelocity.rotate(-c1->rotation);
521
522 frontWheelVelocity.x += CAR_RADIUS * c1->rotationVelocity;
523 backWheelVelocity.x -= CAR_RADIUS * c1->rotationVelocity;
524
525 int carId = std::distance(cars.begin(), c1);
526
527 int driverFlags = DRIVER_BRAKE|DRIVER_ACCELERATE;
528 if (c1->posFinished == 0)
529 {
530 if (c1->driver && (!input || c1->driver->isAI()))
531 {
532 driverFlags = c1->driver->drive(carId, *this);
533 if (input)
534 {
535 *input = (*input & ~(0xF << (4 * carId))) | (driverFlags << (4 * carId));
536 }
537 }
538 else if (input)
539 {
540 driverFlags = (*input >> (4 * carId)) & 0xF;
541 }
542 }
543
544 double turnAngle = 0.0;
545 if (DRIVER_LEFT & driverFlags)
546 {
547 turnAngle -= TURN_ANGLE;
548 }
549 if (DRIVER_RIGHT & driverFlags)
550 {
551 turnAngle += TURN_ANGLE;
552 }
553
554 double backWheelRollFriction = ROLL_WHEEL_FRICTION;
555 double backWheelSpeedAcc = 0.0;
556
557 if (driverFlags & DRIVER_BRAKE)
558 {
559 if (driverFlags & DRIVER_ACCELERATE) // accelerate + reverse = brake
560 {
561 backWheelRollFriction = BRAKE_WHEEL_FRICTION;
562 }
563 else if (backWheelVelocity.y > -BRAKE_SPEED * dt) // reverse
564 {
565 backWheelSpeedAcc = -BRAKE_SPEED;
566 }
567 else // brake
568 {
569 backWheelRollFriction = BRAKE_WHEEL_FRICTION;
570 }
571 }
572 else if (driverFlags & DRIVER_ACCELERATE)
573 {
574 if (backWheelVelocity.y < GAS_SPEED * dt) // forward
575 {
576 backWheelSpeedAcc = GAS_SPEED;
577 }
578 else // brake
579 {
580 backWheelRollFriction = BRAKE_WHEEL_FRICTION;
581 }
582 }
583
584 // the front wheel can have different angles
585 frontWheelVelocity.rotate(-turnAngle);
586 if (std::fabs(frontWheelVelocity.x) > SIDE_WHEEL_FRICTION * dt)
587 {
588 frontWheelVelocity.x += (frontWheelVelocity.x > 0.0 ? -1.0 : 1.0) * SIDE_WHEEL_FRICTION * dt;
589 }
590 else
591 {
592 frontWheelVelocity.x = 0;
593 }
594
595 if (std::fabs(frontWheelVelocity.y) > ROLL_WHEEL_FRICTION * dt)
596 {
597 frontWheelVelocity.y += (frontWheelVelocity.y > 0.0 ? -1.0 : 1.0) * ROLL_WHEEL_FRICTION * dt;
598 }
599 else
600 {
601 frontWheelVelocity.y = 0;
602 }
603 frontWheelVelocity.rotate(turnAngle);
604
605 if (std::fabs(backWheelVelocity.x) > SIDE_WHEEL_FRICTION * dt)
606 {
607 backWheelVelocity.x += (backWheelVelocity.x > 0.0 ? -1.0 : 1.0) * SIDE_WHEEL_FRICTION * dt;
608 }
609 else
610 {
611 backWheelVelocity.x = 0;
612 }
613
614 if (std::fabs(backWheelVelocity.y) > backWheelRollFriction * dt)
615 {
616 backWheelVelocity.y += (backWheelVelocity.y > 0.0 ? -1.0 : 1.0) * backWheelRollFriction * dt;
617 }
618 else
619 {
620 backWheelVelocity.y = 0;
621 }
622 c1->velocity.y = (frontWheelVelocity.y + backWheelVelocity.y) * 0.5 - backWheelSpeedAcc * dt;
623 c1->velocity.x = (frontWheelVelocity.x + backWheelVelocity.x) * 0.5;
624 c1->rotationVelocity = (frontWheelVelocity.x - backWheelVelocity.x) * 0.5 / CAR_RADIUS; // ?
625
626 // avoid that car has too high speed.
627 if (c1->velocity.y < -CAR_SPEED_MAX)
628 {
629 c1->velocity.y = -CAR_SPEED_MAX;
630 }
631 else if (c1->velocity.y > CAR_SPEED_MAX)
632 {
633 c1->velocity.y = CAR_SPEED_MAX;
634 }
635
636 c1->velocity.rotate(c1->rotation);
637
638 // update positions
639 c1->position.x += dt * c1->velocity.x;
640 c1->position.y += dt * c1->velocity.y;
641 c1->rotation += dt * c1->rotationVelocity;
642 }
643 }
644
handleCarCornerCollision(Car & car,const Vector & corner)645 void Track::handleCarCornerCollision(Car& car, const Vector& corner)
646 {
647 Vector carCornerVector = car.position - corner;
648
649 if (carCornerVector.lengthSquared() < CAR_RADIUS_SQUARED)
650 {
651 // Car collisions can affect the velocity direction so we need
652 // to make sure we use the shortest collision time to avoid
653 // that the car is teleported to the other side of the wall.
654 double pt1 = findCollisionTime(carCornerVector, car.velocity, CAR_RADIUS);
655 double pt2 = findCollisionTime(carCornerVector, -car.velocity, CAR_RADIUS);
656 double pt;
657 if (pt1 >= pt2)
658 {
659 pt = pt1;
660 }
661 else
662 {
663 pt = -pt2;
664 }
665
666 Vector colDir(carCornerVector.x + car.velocity.x * pt, carCornerVector.y + car.velocity.y * pt);
667 double colAngle = Vector::angle(colDir, VECTOR_Y);
668
669 car.velocity.rotate(colAngle);
670 car.position.rotate(colAngle);
671
672 // move car to y collision point
673 car.position.y += pt * car.velocity.y;
674
675 car.velocity.y = std::fabs(car.velocity.y);
676
677 double friction = CAR_WALL_FRICTION * std::fabs(car.velocity.y);
678 if (friction > 1.0)
679 {
680 friction = 1.0;
681 }
682
683 car.rotationVelocity = friction * (-car.velocity.x / CAR_RADIUS) + (1.0 - friction) * car.rotationVelocity;
684 car.position.rotate(-colAngle);
685 car.velocity.rotate(-colAngle);
686 }
687 }
688
findCollisionTime(const Vector & vec,const Vector & velocity,double distance)689 double Track::findCollisionTime(const Vector& vec,
690 const Vector& velocity,
691 double distance)
692 {
693 // Calculate how much time pt have passed since the collision.
694 // To do this we use an equation that says that the
695 // distance between the two points should be equal to radius
696 // because this is when the collision occurred.
697 // sqrt((vec.x+vec.x*dt)^2+(vec.y+vec.y*dt)^2)=distance
698 // Solving this on paper we get an quadratic equation
699 // that can be solved by the quadratic formula
700 // ax^2 + bx + c = 0
701 // x = -(b/2a) ± sqrt((b^2)/(4a^2)-c/a)
702 double a = velocity.x * velocity.x + velocity.y * velocity.y;
703 double b = 2 * (vec.x * velocity.x + vec.y * velocity.y);
704 double c = vec.x * vec.x + vec.y * vec.y - distance * distance;
705
706 // avoid division by zero
707 if (std::fabs(a) < std::numeric_limits<double>::epsilon())
708 {
709 a = std::numeric_limits<double>::epsilon();
710 }
711
712 // f1 = -(b/2a)
713 double f1 = -(b / (2 * a));
714 // f2 = sqrt((b^2)/(4a^2)-c/a)
715 double f2 = std::sqrt((b * b) / ((4 * a) * a) - c / a);
716 // We have two solutions to the equation, f1+f2 and f1-f2
717 // but we are only interested in the negative one
718 double dt = std::min(f1 + f2, f1 - f2);
719
720 return dt;
721 }
722
getCarPosition(int carId) const723 const Vector& Track::getCarPosition(int carId) const
724 {
725 return cars[carId].position;
726 }
getCarRotation(int carId) const727 double Track::getCarRotation(int carId) const
728 {
729 return cars[carId].rotation;
730 }
731
getCarLap(int carId) const732 int Track::getCarLap(int carId) const
733 {
734 return cars[carId].lap;
735 }
736
isCarFinished(int carId) const737 bool Track::isCarFinished(int carId) const
738 {
739 return cars[carId].posFinished != 0;
740 }
741
getCarProgress(int carId) const742 double Track::getCarProgress(int carId) const
743 {
744 if (cars[carId].posFinished)
745 {
746 return INT_MAX - cars[carId].posFinished;
747 }
748
749 int tilex = (int) cars[carId].position.x / TILE_SIZE;
750 int tiley = (int) cars[carId].position.y / TILE_SIZE;
751 Vector posInTile(cars[carId].position.x - tilex * TILE_SIZE,
752 cars[carId].position.y - tiley * TILE_SIZE);
753
754 double progressInTile = 0.0;
755 switch (tiles[tiley][tilex].dir)
756 {
757 case LEFT:
758 progressInTile = TILE_SIZE - posInTile.x;
759 break;
760 case RIGHT:
761 progressInTile = posInTile.x;
762 break;
763 case UP:
764 progressInTile = TILE_SIZE - posInTile.y;
765 break;
766 case DOWN:
767 progressInTile = posInTile.y;
768 break;
769 }
770
771 // The tileOrderNum is updated once after the collisions and are
772 // used later for the drivers. After that the positions are updated
773 // so the tileOrderNum can be old. To avoid wrong positions we
774 // calculate the updated tileOrderNum here.
775 int ton = cars[carId].tileOrderNum;
776 if ((ton + 1 == tiles[tiley][tilex].orderNum) ||
777 (ton + 1 == trackLength && tiles[tiley][tilex].orderNum == 0))
778 {
779 ton++;
780 }
781 else if (cars[carId].tileOrderNum - 1 == tiles[tiley][tilex].orderNum)
782 {
783 ton--;
784 }
785
786 int tiles = (cars[carId].lap - 1) * trackLength + ton;
787
788 return tiles * TILE_SIZE + progressInTile;
789 }
790
vdistance(const Vector & p1,const Vector & p2,double & minDistance,double & angle)791 static void vdistance(const Vector& p1, const Vector& p2, double& minDistance, double& angle)
792 {
793 Vector v = p2 - p1;
794 double distance = v.length();
795 if (distance < minDistance)
796 {
797 minDistance = distance;
798 angle = Vector::angle(v, VECTOR_Y);
799 }
800 }
801
getCarDistanceNearestWall(int carId,double & angle) const802 double Track::getCarDistanceNearestWall(int carId, double& angle) const
803 {
804 Vector p = cars[carId].position;
805 int tilex = (int) p.x / TILE_SIZE;
806 int tiley = (int) p.y / TILE_SIZE;
807 Vector posInTile(cars[carId].position.x - tilex * TILE_SIZE,
808 cars[carId].position.y - tiley * TILE_SIZE);
809 int colFlags = tiles[tiley][tilex].collisionFlags;
810
811 angle = 0;
812 double distance = TILE_SIZE;
813 if (colFlags & TILE_COLLISION_LEFT)
814 {
815 vdistance(posInTile, Vector(0, posInTile.y), distance, angle);
816 }
817 if (colFlags & TILE_COLLISION_RIGHT)
818 {
819 vdistance(posInTile, Vector(TILE_SIZE, posInTile.y), distance, angle);
820 }
821 if (colFlags & TILE_COLLISION_UP)
822 {
823 vdistance(posInTile, Vector(posInTile.x, 0), distance, angle);
824 }
825 if (colFlags & TILE_COLLISION_DOWN)
826 {
827 vdistance(posInTile, Vector(posInTile.x, TILE_SIZE), distance, angle);
828 }
829
830 if (colFlags & TILE_COLLISION_LEFT_UP)
831 {
832 vdistance(posInTile, Vector(0, 0), distance, angle);
833 }
834 if (colFlags & TILE_COLLISION_LEFT_DOWN)
835 {
836 vdistance(posInTile, Vector(0, TILE_SIZE), distance, angle);
837 }
838 if (colFlags & TILE_COLLISION_RIGHT_UP)
839 {
840 vdistance(posInTile, Vector(TILE_SIZE, 0), distance, angle);
841 }
842 if (colFlags & TILE_COLLISION_RIGHT_DOWN)
843 {
844 vdistance(posInTile, Vector(TILE_SIZE, TILE_SIZE), distance, angle);
845 }
846 angle = angle2range(angle + cars[carId].rotation);
847
848 return distance;
849 }
850
getCarTileAngle(int carId) const851 double Track::getCarTileAngle(int carId) const
852 {
853 double angle = 0.0;
854
855 int x = (int) cars[carId].position.x / TILE_SIZE;
856 int y = (int) cars[carId].position.y / TILE_SIZE;
857 Vector posInTile(cars[carId].position.x - x * TILE_SIZE,
858 cars[carId].position.y - y * TILE_SIZE);
859
860 switch (tiles[y][x].dir)
861 {
862 case LEFT:
863 angle = PI / 2;
864 break;
865 case RIGHT:
866 angle = 3 * PI / 2;
867 break;
868 case UP:
869 angle = 0;
870 break;
871 case DOWN:
872 angle = PI;
873 break;
874 }
875
876 return angle2range(PI - (angle + cars[carId].rotation));
877 }
878
finished() const879 bool Track::finished() const
880 {
881 return carsLeft == (int) cars.size();
882 }
883
isMoving(double velocity)884 static bool isMoving(double velocity)
885 {
886 return std::fabs(velocity) >= 1e-10;
887 }
888
noMovement() const889 bool Track::noMovement() const
890 {
891 for (std::size_t i = 0; i < cars.size(); ++i)
892 {
893 const Car& car = cars[i];
894 if (isMoving(car.velocity.x) || isMoving(car.velocity.y) || isMoving(car.rotationVelocity))
895 {
896 return false;
897 }
898 }
899 return true;
900 }
901
getTrackLength() const902 int Track::getTrackLength() const
903 {
904 return trackLength;
905 }
906
getCarForwardSpeed(int carId) const907 double Track::getCarForwardSpeed(int carId) const
908 {
909 Vector v = cars[carId].velocity;
910 v.rotate(-cars[carId].rotation);
911 return -v.y;
912 }
913
getFinishDir() const914 int Track::getFinishDir() const
915 {
916 return tiles[finishY][finishX].dir;
917 }
918
getBestDrivingAngle(int carId,double safeMargin,double maxDepth) const919 double Track::getBestDrivingAngle(int carId, double safeMargin, double maxDepth) const
920 {
921 double minAngle = cars[carId].rotation - 0.5 * PI;
922 double maxAngle = cars[carId].rotation + 0.5 * PI;
923
924 Vector p = getBestDrivingPoint(cars[carId], CAR_RADIUS + safeMargin, maxDepth, minAngle, maxAngle);
925 Vector v = p - cars[carId].position;
926
927 return angle2range(Vector::angle(v, VECTOR_Y) + cars[carId].rotation);
928 }
929
930
931 // shoots two rays from the car,
rayWallTest(const Car & car,double angle,double radius) const932 Vector Track::rayWallTest(const Car& car, double angle, double radius) const
933 {
934 Vector pos1(-radius, 0);
935 pos1.rotate(angle);
936 Vector pos2(radius, 0);
937 pos2.rotate(angle);
938 pos1 = rayWallTest(car.position + pos1, angle);
939 pos2 = rayWallTest(car.position + pos2, angle);
940
941 if ((pos1 - car.position).length() < (pos2 - car.position).length())
942 {
943 return pos1;
944 }
945 else
946 {
947 return pos2;
948 }
949 }
950
951 // shoots one ray in the angle direction
rayWallTest(Vector pos,double angle) const952 Vector Track::rayWallTest(Vector pos, double angle) const
953 {
954 Vector dir = -VECTOR_Y;
955 dir.rotate(angle);
956
957 int xColFlag;
958 int yColFlag;
959 int xTileChange;
960 int yTileChange;
961 int xWrap;
962 int yWrap;
963
964 if (dir.x < 0.0)
965 {
966 xColFlag = TILE_COLLISION_LEFT;
967 xTileChange = -1;
968 xWrap = 0;
969 }
970 else
971 {
972 xColFlag = TILE_COLLISION_RIGHT;
973 xTileChange = 1;
974 xWrap = TILE_SIZE;
975 }
976
977 if (dir.y < 0.0)
978 {
979 yColFlag = TILE_COLLISION_UP;
980 yTileChange = -1;
981 yWrap = 0;
982 }
983 else
984 {
985 yColFlag = TILE_COLLISION_DOWN;
986 yTileChange = 1;
987 yWrap = TILE_SIZE;
988 }
989
990 int tilex = (int) pos.x / TILE_SIZE;
991 int tiley = (int) pos.y / TILE_SIZE;
992 Vector posInTile(pos.x - tilex * TILE_SIZE,
993 pos.y - tiley * TILE_SIZE);
994
995 while (true)
996 {
997 assert(tilex >= 0);
998 assert(tiley >= 0);
999 assert(tilex < MAX_MAP_X);
1000 assert(tiley < MAX_MAP_Y);
1001 int colFlags = tiles[tiley][tilex].collisionFlags;
1002
1003 double kx = (xWrap - posInTile.x) / dir.x;
1004 double ky = (yWrap - posInTile.y) / dir.y;
1005
1006 if (std::fabs(kx) < std::fabs(ky))
1007 {
1008 posInTile.y = posInTile.y + dir.y * kx;
1009 posInTile.x = TILE_SIZE - xWrap;
1010 tilex += xTileChange;
1011 if (colFlags & xColFlag)
1012 {
1013 break;
1014 }
1015 }
1016 else
1017 {
1018 posInTile.x = posInTile.x + dir.x * ky;
1019 posInTile.y = TILE_SIZE - yWrap;
1020 tiley += yTileChange;
1021 if (colFlags & yColFlag)
1022 {
1023 break;
1024 }
1025 }
1026 }
1027 // Back up a little to make sure that we don't end up in the wrong tile.
1028 posInTile += -0.1 * dir;
1029
1030 return Vector(tilex * TILE_SIZE + posInTile.x,
1031 tiley * TILE_SIZE + posInTile.y);
1032 }
1033
rayCarTest(const Car & car,double angle,double radius) const1034 Vector Track::rayCarTest(const Car& car, double angle, double radius) const
1035 {
1036 Vector shortesRayPoint = Vector(2212, 2113);
1037 double shortestRayLength = 10000000.0; // a huge number
1038
1039 Vector dir = -VECTOR_Y;
1040 dir.rotate(angle);
1041 double r = 2 * CAR_RADIUS + radius / 2;
1042
1043 // maybe I do too many test below
1044 std::vector<Car>::const_iterator it;
1045 for (it = cars.begin(); it != cars.end(); ++it)
1046 {
1047 if (&car == &*it)
1048 {
1049 continue;
1050 }
1051 Vector carVector = car.position - it->position;
1052 double p = 2.0 * Vector::dot(carVector, dir);
1053 double q = Vector::dot(carVector, carVector) - r * r;
1054 if (p > 0.0 && q < 0.0)
1055 {
1056 continue;
1057 }
1058 double t1 = -0.5 * p;
1059 double t2Sqr = 0.25 * p * p - q;
1060 if (t2Sqr < 0)
1061 {
1062 continue;
1063 }
1064 double t2 = std::sqrt(t2Sqr);
1065 double t = std::min(t1 + t2, t1 - t2);
1066 if (t > 0.0)
1067 {
1068 Vector rayPoint = car.position + dir * t;
1069 double rayLength = (rayPoint - car.position).length();
1070 if (rayLength < shortestRayLength)
1071 {
1072 shortestRayLength = rayLength;
1073 shortesRayPoint = rayPoint;
1074 }
1075 }
1076 }
1077 return shortesRayPoint;
1078 }
1079
PointProgress(Vector pos) const1080 double Track::PointProgress(Vector pos) const
1081 {
1082 int tilex = (int) pos.x / TILE_SIZE;
1083 int tiley = (int) pos.y / TILE_SIZE;
1084 Vector posInTile(pos.x - tilex * TILE_SIZE,
1085 pos.y - tiley * TILE_SIZE);
1086 const Tile& tile = tiles[tiley][tilex];
1087 double progressInTile = 0.0;
1088 switch (tile.dir)
1089 {
1090 case LEFT:
1091 progressInTile = TILE_SIZE - posInTile.x;
1092 break;
1093 case RIGHT:
1094 progressInTile = posInTile.x;
1095 break;
1096 case UP:
1097 progressInTile = TILE_SIZE - posInTile.y;
1098 break;
1099 case DOWN:
1100 progressInTile = posInTile.y;
1101 break;
1102 }
1103 return tile.orderNum * TILE_SIZE + progressInTile;
1104 }
1105
rayProgress(Vector pos1,Vector pos2) const1106 double Track::rayProgress(Vector pos1, Vector pos2) const
1107 {
1108 // no two points can have more than half the track progress
1109 double maxProgress = 0.5 * trackLength * TILE_SIZE;
1110
1111 double progress = PointProgress(pos2) - PointProgress(pos1);
1112
1113 if (progress > maxProgress)
1114 {
1115 progress -= trackLength * TILE_SIZE;
1116 }
1117 else if (progress < -maxProgress)
1118 {
1119 progress += trackLength * TILE_SIZE;
1120 }
1121
1122 return progress;
1123 }
1124
rayProgress(const Car & car,Vector rayHitpos) const1125 double Track::rayProgress(const Car& car, Vector rayHitpos) const
1126 {
1127 return rayProgress(car.position, rayHitpos);
1128 }
1129
getBestDrivingPoint(const Car & car,double radius,double maxDepth,double minAngle,double maxAngle) const1130 Vector Track::getBestDrivingPoint(const Car& car, double radius, double maxDepth, double minAngle, double maxAngle) const
1131 {
1132 Vector bestRayPoint = car.position;
1133 double bestRayProgress = -10000.0;
1134
1135 while (maxDepth)
1136 {
1137 --maxDepth;
1138 double middleAngle = (minAngle + maxAngle) * 0.5;
1139
1140 Vector rayWallPoint = rayWallTest(car, middleAngle, radius);
1141 double rayWallDistance = (rayWallPoint - car.position).length();
1142
1143
1144 if (carCollisionsEnabled)
1145 {
1146 Vector rayCarPoint = rayCarTest(car, middleAngle, radius);
1147 double rayCarDistance = (rayCarPoint - car.position).length();
1148
1149 if (rayCarDistance < rayWallDistance)
1150 {
1151 Vector rayPoint1 = getBestDrivingPoint(car, radius, maxDepth, minAngle, middleAngle);
1152 Vector rayPoint2 = getBestDrivingPoint(car, radius, maxDepth, middleAngle, maxAngle);
1153 double rayProgress1 = rayProgress(car, rayPoint1);
1154 double rayProgress2 = rayProgress(car, rayPoint2);
1155 double rayCarProgress = rayProgress(car, rayCarPoint);
1156 if (rayCarProgress > rayProgress1 && rayCarProgress > rayProgress2)
1157 {
1158 return rayCarPoint;
1159 }
1160 else if (rayProgress1 > rayProgress2)
1161 {
1162 return rayPoint1;
1163 }
1164 else
1165 {
1166 return rayPoint2;
1167 }
1168 }
1169 }
1170
1171 int tilex = (int) rayWallPoint.x / TILE_SIZE;
1172 int tiley = (int) rayWallPoint.y / TILE_SIZE;
1173 Vector dir;
1174 switch (tiles[tiley][tilex].dir)
1175 {
1176 case LEFT:
1177 dir = Vector(-1, 0);
1178 break;
1179 case RIGHT:
1180 dir = Vector(1, 0);
1181 break;
1182 case UP:
1183 dir = Vector(0, -1);
1184 break;
1185 case DOWN:
1186 dir = Vector(0, 1);
1187 break;
1188 }
1189 dir.rotate(-middleAngle);
1190 (dir.x > 0 ? minAngle : maxAngle) = middleAngle;
1191
1192 double rayWallProgress = rayProgress(car, rayWallPoint);
1193
1194 if (rayWallProgress > bestRayProgress)
1195 {
1196 bestRayProgress = rayWallProgress;
1197 bestRayPoint = rayWallPoint;
1198 }
1199 }
1200 return bestRayPoint;
1201 }
1202
save(TrackState & state) const1203 void Track::save(TrackState& state) const
1204 {
1205 state.time = time;
1206 state.timeState = timeState;
1207 for (std::size_t i = 0; i < cars.size(); ++i)
1208 {
1209 TrackCarState& stateCar = state.cars[i];
1210 const Car& trackCar = cars[i];
1211
1212 stateCar.velocity = trackCar.velocity;
1213 stateCar.position = trackCar.position;
1214 stateCar.rotationVelocity = trackCar.rotationVelocity;
1215 stateCar.rotation = trackCar.rotation;
1216 stateCar.tileOrderNum = trackCar.tileOrderNum;
1217 stateCar.lap = trackCar.lap;
1218 stateCar.posFinished = trackCar.posFinished;
1219 stateCar.timeFinished = trackCar.timeFinished;
1220 }
1221 }
1222
restore(const TrackState & state)1223 void Track::restore(const TrackState& state)
1224 {
1225 time = state.time;
1226 timeState = state.timeState;
1227
1228 carsLeft = 0;
1229
1230 for (std::size_t i = 0; i < cars.size(); ++i)
1231 {
1232 const TrackCarState& stateCar = state.cars[i];
1233 Car& trackCar = cars[i];
1234
1235 trackCar.velocity = stateCar.velocity;
1236 trackCar.position = stateCar.position;
1237 trackCar.rotationVelocity = stateCar.rotationVelocity;
1238 trackCar.rotation = stateCar.rotation;
1239 trackCar.tileOrderNum = stateCar.tileOrderNum;
1240 trackCar.lap = stateCar.lap;
1241 trackCar.posFinished = stateCar.posFinished;
1242 trackCar.timeFinished = stateCar.timeFinished;
1243
1244 if (trackCar.posFinished)
1245 {
1246 ++carsLeft;
1247 }
1248 }
1249 }
1250
removeCar(unsigned carId)1251 void Track::removeCar(unsigned carId)
1252 {
1253 if (carId < cars.size())
1254 {
1255 Car& car = cars[carId];
1256 if (car.posFinished)
1257 {
1258 --carsLeft;
1259 }
1260 cars.erase(cars.begin() + carId);
1261 removedCars.push_back(carId);
1262 }
1263 }
1264
getRemovedCars()1265 std::vector<int>& Track::getRemovedCars()
1266 {
1267 return removedCars;
1268 }
1269
getRaceTime() const1270 double Track::getRaceTime() const
1271 {
1272 return timeState < TIME_STATE_GO ? 0 : time;
1273 }
1274
getTimeState() const1275 int Track::getTimeState() const
1276 {
1277 return timeState;
1278 }
1279
getCarFinishTime(int carId) const1280 double Track::getCarFinishTime(int carId) const
1281 {
1282 return cars[carId].timeFinished;
1283 }
1284
getCarCount() const1285 int Track::getCarCount() const
1286 {
1287 return cars.size();
1288 }
1289
finish(double dt,int minUpdates,int maxRealDuration)1290 void Track::finish(double dt, int minUpdates, int maxRealDuration)
1291 {
1292 int updates = 0;
1293 Uint32 stop = SDL_GetTicks() + maxRealDuration;
1294 while (!finished())
1295 {
1296 update(dt);
1297 ++updates;
1298 if (updates >= minUpdates)
1299 {
1300 if (stop <= SDL_GetTicks())
1301 {
1302 break;
1303 }
1304 bool noComputerDrivers = true;
1305 std::vector<Car>::iterator it;
1306 for (it = cars.begin(); it != cars.end(); ++it)
1307 {
1308 Car& car = *it;
1309 if (car.posFinished == 0 && car.driver && car.driver->isAI())
1310 {
1311 noComputerDrivers = false;
1312 break;
1313 }
1314 }
1315 if (noComputerDrivers)
1316 {
1317 break;
1318 }
1319 }
1320 }
1321 }
1322