1 /*
2 * GRacer
3 *
4 * Copyright (C) 1999 Takashi Matsuda <matsu@users.sourceforge.net>
5 *
6 * This program is free software; you can redistribute it and/or
7 * modify it under the terms of the GNU General Public License as
8 * published by the Free Software Foundation; either version 2 of the
9 * License, or (at your option) any later version.
10 *
11 * This program 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, write to the Free Software
18 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
19 * USA
20 */
21
22 #include <stdlib.h>
23 #include <stdio.h>
24 #include <string.h>
25 #include <math.h>
26 #include <GL/gl.h>
27 #include <GL/glu.h>
28 #include <common/gr_physics.h>
29 #include <common/gr_debug.h>
30 #include <common/gr_plane.h>
31 #include <common/gr_angle.h>
32 #include "vehicle.h"
33 #include "glbind.h"
34
35 #define ADJUST_INDEX(i,l,h) { \
36 if ((i) < (l)) { \
37 (i) = (l); \
38 } else if ((i) > (h)) { \
39 (i) = (h); \
40 } \
41 }
42
43 #define MASS 800.0
44 #define MOMENT 10000.0
45
46 #define FRONT_KS (GRAVITY * MASS / 4.0 / 0.1)
47 #define FRONT_KD (4000.0 * FPS)
48
49 #define REAR_KS (GRAVITY * MASS / 4.0 / 0.1)
50 #define REAR_KD (4000.0 * FPS)
51
52 #define ENGINE_FORCE 3000.0
53
54 #define FOOT_BREAK_FORCE 6000.0
55 #define SIDE_BREAK_FORCE 5000.0
56
57 #define FPS 20
58
59 #define SIGN(d) ((d)>=0.0? 1.0:-1.0)
60
61 static void
next_word(char * str,int * index)62 next_word (char *str, int *index)
63 {
64 char c;
65
66 while (c = str[*index], !(c == ' ' || c == '\t' || c == '\n' || c=='\0')) {
67 (*index) ++;
68 }
69
70 while (c = str[*index], c == ' ' || c == '\t' || c == '\n') {
71 (*index) ++;
72 }
73 }
74
75 static int
token_match(char * token,char * str,int * index)76 token_match (char *token, char *str, int *index)
77 {
78 int len;
79 char c;
80
81 while (c = str[*index], c == ' ' || c == '\t' || c == '\n') {
82 (*index) ++;
83 }
84
85 len = strlen (token);
86 if (strncmp (token, str + *index, len))
87 return 0;
88
89 c = str[*index + len];
90 if (c == ' ' || c == '\t' || c == '\n' || c == '\0') {
91 (*index) += len;
92 while (c = str[*index], c == ' ' || c == '\t' || c == '\n') {
93 (*index) ++;
94 }
95 return 1;
96 }
97
98 return 0;
99 }
100
101 static int
token_peek(char * token,char * str,int * index)102 token_peek (char *token, char *str, int *index)
103 {
104 int len;
105 char c;
106
107 while (c = str[*index], c == ' ' || c == '\t' || c == '\n') {
108 (*index) ++;
109 }
110
111 len = strlen (token);
112 if (strncmp (token, str + *index, len))
113 return 0;
114
115 c = str[*index + len];
116 if (c == ' ' || c == '\t' || c == '\n' || c == '\0') {
117 return 1;
118 }
119
120 return 0;
121 }
122
123 static char *
get_string(char * str,int * index)124 get_string (char *str, int *index)
125 {
126 char *name = NULL;
127 char *tmp;
128 int i = *index;
129 int len;
130 int c;
131
132 if (str[i] == '"') {
133 i++;
134 tmp = strchr (str+i, '"');
135 while (tmp && tmp[-1] == '\\') {
136 tmp = strchr (tmp+1, '"');
137 }
138 if (!tmp) {
139 len = strlen (str + i);
140 } else {
141 len = tmp - (str + i);
142 }
143 if (len > 0) {
144 name = gr_new (char, len+1);
145 strncpy (name, str+i, len);
146 name[len] = '\0';
147 }
148 i += len + 1;
149 } else {
150 tmp = strpbrk (str+i, " \t\n");
151 if (!tmp) {
152 len = strlen (str + i);
153 } else {
154 len = tmp - (str + i);
155 }
156 name = gr_new (char, len+1);
157 strncpy (name, str+i, len);
158 name[len] = '\0';
159 i += len;
160 }
161
162 while (c = str[i], c == ' ' || c=='\t' || c == '\n') {
163 i++;
164 }
165
166 *index = i;
167 return name;
168 }
169
170 GrBreakData *
gr_break_data_new_from_file(FILE * file)171 gr_break_data_new_from_file (FILE *file)
172 {
173 int i;
174 GrBreakData *data;
175
176 data = gr_new0 (GrBreakData, 1);
177
178 for (i=0; i<128 && !feof(file); i++) {
179 fscanf (file, "%f", &data->mu[i]);
180 }
181 if (i != 128) {
182 fputs ("failed to read break data file.\n", stderr);
183 free (data);
184 return NULL;
185 }
186
187 return data;
188 }
189
190 GrTireData *
gr_tire_data_new_from_file(FILE * file)191 gr_tire_data_new_from_file (FILE *file)
192 {
193 int i, j;
194 char *buf;
195 size_t top;
196 size_t size;
197 GrTireData *data;
198
199 data = gr_new0 (GrTireData, 1);
200 if (!data) {
201 return NULL;
202 }
203
204 top = ftell (file);
205 fseek (file, 0, SEEK_END);
206 size = ftell (file) - top;
207 fseek (file, top, SEEK_SET);
208
209 buf = gr_new (char, size + 1);
210 fread (buf, 1, size, file);
211 buf[size] = '\0';
212
213 for (i=0; i<size; ) {
214 if (token_match ("RADIUS", buf, &i)) {
215 sscanf (buf+i, "%f", &data->radius);
216 next_word (buf, &i);
217 } else if (token_match ("MU", buf, &i)) {
218 for (j=0; j<128 && i<size; j++) {
219 sscanf (buf+i, "%f", &data->mu[j]);
220 next_word (buf, &i);
221 }
222 } else {
223 goto ERROR;
224 }
225 }
226
227 free (buf);
228 return data;
229
230 ERROR:
231 fputs ("failed to read tire data file.\n", stderr);
232 free (buf);
233 free (data);
234 return NULL;
235 }
236
237 GrEngineData *
gr_engine_data_new_from_file(FILE * file)238 gr_engine_data_new_from_file (FILE *file)
239 {
240 int i, j;
241 char *buf;
242 size_t top;
243 size_t size;
244 GrEngineData *data;
245
246 data = gr_new0 (GrEngineData, 1);
247 if (!data) {
248 return NULL;
249 }
250
251 top = ftell (file);
252 fseek (file, 0, SEEK_END);
253 size = ftell (file) - top;
254 fseek (file, top, SEEK_SET);
255
256 buf = gr_new (char, size + 1);
257 fread (buf, 1, size, file);
258 buf[size] = '\0';
259
260 for (i=0; i<size; ) {
261 if (token_match ("MAXGEAR", buf, &i)) {
262 sscanf (buf+i, "%d", &data->max_gear);
263 data->gear_ratio = gr_new (float, data->max_gear);
264 next_word (buf, &i);
265 for (j=0; j<data->max_gear && i<size; j++) {
266 sscanf (buf+i, "%f", &data->gear_ratio[j]);
267 next_word (buf, &i);
268 }
269 } else if (token_match ("FRICTION", buf, &i)) {
270 sscanf (buf+i, "%f", &data->friction);
271 next_word (buf, &i);
272 } else if (token_match ("MOMENT", buf, &i)) {
273 sscanf (buf+i, "%f", &data->moment);
274 next_word (buf, &i);
275 } else if (token_match ("LIMIT", buf, &i)) {
276 sscanf (buf+i, "%f", &data->limit);
277 next_word (buf, &i);
278 } else if (token_match ("TORQUE", buf, &i)) {
279 for (j=0; j<128 && i<size; j++) {
280 sscanf (buf+i, "%f", &data->torque[j]);
281 next_word (buf, &i);
282 }
283 } else {
284 goto ERROR;
285 }
286 }
287
288 free (buf);
289 return data;
290
291 ERROR:
292 fputs ("failed to read engine data file.\n", stderr);
293 free (buf);
294 free (data);
295 return NULL;
296 }
297
298 static void
gr_vehicle_data_free(GrVehicleData * data)299 gr_vehicle_data_free (GrVehicleData *data)
300 {
301 int i;
302
303 free (data->title);
304 free (data->model_url);
305 free (data->engine_url);
306
307 for (i=0; i<data->num_wheel; i++) {
308 free (data->wheel[i].object_name);
309 free (data->wheel[i].tire_url);
310 free (data->wheel[i].fb_url);
311 free (data->wheel[i].sb_url);
312
313 if (data->wheel[i].tire)
314 gr_DECREF (data->wheel[i].tire);
315 if (data->wheel[i].fb)
316 gr_DECREF (data->wheel[i].fb);
317 if (data->wheel[i].sb)
318 gr_DECREF (data->wheel[i].sb);
319 }
320
321 free (data);
322 }
323
324 GrVehicleData*
gr_vehicle_data_new_from_file(FILE * file)325 gr_vehicle_data_new_from_file (FILE *file)
326 {
327 GrVehicleData *data;
328 char *buf;
329 size_t top;
330 size_t size;
331 int i, j, k;
332 double length;
333
334 data = gr_new0 (GrVehicleData, 1);
335 if (!data) {
336 return NULL;
337 }
338 gr_FREE_FUNC (data, gr_vehicle_data_free);
339
340 top = ftell (file);
341 fseek (file, 0, SEEK_END);
342 size = ftell (file) - top;
343 fseek (file, top, SEEK_SET);
344
345 buf = gr_new (char, size + 1);
346 if (!buf) {
347 return NULL;
348 }
349 fread (buf, 1, size, file);
350 buf[size] = '\0';
351
352 for (i=0; i<size; ) {
353 if (token_match ("TITLE", buf, &i)) {
354 data->title = get_string (buf, &i);
355 } else if (token_match ("TIMESTAMP", buf, &i)) {
356 data->timestamp = get_string (buf, &i);
357 } else if (token_match ("MODEL", buf, &i)) {
358 data->model_url = get_string (buf, &i);
359 } else if (token_match ("OBJECT", buf, &i)) {
360 data->object_name = get_string (buf, &i);
361 } else if (token_match ("TYPE", buf, &i)) {
362 if (token_match ("motorcar", buf, &i)) {
363 data->type = GR_VEHICLE_MOTORCAR;
364 } else if (token_match ("motorcycle", buf, &i)) {
365 data->type = GR_VEHICLE_MOTORCYCLE;
366 } else if (token_match ("bicycle", buf, &i)) {
367 data->type = GR_VEHICLE_BICYCLE;
368 }
369 } else if (token_match ("COCKPITHEIGHT", buf, &i)) {
370 sscanf (buf+i, "%f", &data->cockpit_height);
371 next_word (buf, &i);
372 } else if (token_match ("COCKPITPOS", buf, &i)) {
373 for (j=0; j<3; j++) {
374 sscanf (buf+i, "%f", &data->cockpit.f[j]);
375 next_word (buf, &i);
376 }
377 } else if (token_match ("MASS", buf, &i)) {
378 sscanf (buf+i, "%f", &data->mass);
379 next_word (buf, &i);
380 } else if (token_match ("MOMENT", buf, &i)) {
381 for (j=0; j<9; j++) {
382 sscanf (buf+i, "%f", &data->moment[j]);
383 next_word (buf, &i);
384 }
385 } else if (token_match ("GEARRATIO", buf, &i)) {
386 sscanf (buf+i, "%f", &data->gear_ratio);
387 next_word (buf, &i);
388 } else if (token_match ("DOWNFORCE", buf, &i)) {
389 sscanf (buf+i, "%f", &data->down_force_speed_ratio);
390 next_word (buf, &i);
391 } else if (token_match ("ENGINE", buf, &i)) {
392 data->engine_url = get_string (buf, &i);
393 } else if (token_match ("NUMWHEEL", buf, &i)) {
394 sscanf (buf+i, "%d", &data->num_wheel);
395 next_word (buf, &i);
396 data->wheel = gr_new0 (GrWheelData, data->num_wheel);
397 for (j=0; j<data->num_wheel && i<size; j++) {
398 if (token_match ("WHEEL", buf, &i)) {
399 sscanf (buf+i, "%d", &data->wheel[j].flag);
400 next_word (buf, &i);
401 if (data->wheel[j].flag & GR_WHEEL_DRIVING) {
402 data->num_driving_wheel ++;
403 }
404 for (; i<size; ) {
405 if (token_peek ("WHEEL", buf, &i)) {
406 break;
407 } else if (token_match ("MODEL", buf, &i)) {
408 data->wheel[j].model_url = get_string (buf, &i);
409 } else if (token_match ("OBJECT", buf, &i)) {
410 data->wheel[j].object_name = get_string (buf, &i);
411 } else if (token_match ("TIRE", buf, &i)) {
412 data->wheel[j].tire_url = get_string (buf, &i);
413 } else if (token_match ("POS", buf, &i)) {
414 for (k=0; k<3; k++) {
415 sscanf (buf+i, "%f", &data->wheel[j].pos.f[k]);
416 next_word (buf, &i);
417 }
418 } else if (token_match ("PIVOT", buf, &i)) {
419 for (k=0; k<3; k++) {
420 sscanf (buf+i, "%f", &data->wheel[j].pivot.f[k]);
421 next_word (buf, &i);
422 }
423 } else if (token_match ("MOMENT", buf, &i)) {
424 sscanf (buf+i, "%f", &data->wheel[j].m);
425 next_word (buf, &i);
426 } else if (token_match ("SUSPLEN", buf, &i)) {
427 sscanf (buf+i, "%f", &data->wheel[j].slen);
428 next_word (buf, &i);
429 } else if (token_match ("SUSPDIR", buf, &i)) {
430 for (k=0; k<3; k++) {
431 sscanf (buf+i, "%f", &data->wheel[j].susp.f[k]);
432 next_word (buf, &i);
433 }
434 } else if (token_match ("SUSPKS", buf, &i)) {
435 sscanf (buf+i, "%f", &data->wheel[j].ks);
436 next_word (buf, &i);
437 } else if (token_match ("SUSPKD", buf, &i)) {
438 sscanf (buf+i, "%f", &data->wheel[j].kd);
439 next_word (buf, &i);
440 } else if (token_match ("FOOTBREAK", buf, &i)) {
441 data->wheel[j].fb_url = get_string (buf, &i);
442 } else if (token_match ("SIDEBREAK", buf, &i)) {
443 data->wheel[j].sb_url = get_string (buf, &i);
444 } else {
445 goto ERROR;
446 }
447 }
448 } else {
449 goto ERROR;
450 }
451 }
452 } else {
453 goto ERROR;
454 }
455 }
456
457 for (i=0; i<data->num_wheel; i++) {
458 length = gr_vertex_length (&data->wheel[i].susp);
459 if (length) {
460 data->wheel[i].susp.c.x /= length;
461 data->wheel[i].susp.c.y /= length;
462 data->wheel[i].susp.c.z /= length;
463 } else {
464 data->wheel[i].susp.c.x = 0;
465 data->wheel[i].susp.c.y = 0;
466 data->wheel[i].susp.c.z = 1;
467 }
468 }
469
470 free (buf);
471 return data;
472
473 ERROR:
474 fputs ("failed to read grv file.\n", stderr);
475 free (buf);
476 if (data && data->wheel) {
477 free (data->wheel);
478 }
479 free (data);
480 return NULL;
481 }
482
483 GrVehicleData *
gr_get_vehicle_data(char * url)484 gr_get_vehicle_data (char *url)
485 {
486 GrVehicleData *data;
487 char *str;
488 FILE *file;
489 int i;
490
491 data = tcl_GetCache (url);
492 if (!data) {
493 file = gr_open_file (url, "r");
494 if (!file) {
495 return NULL;
496 }
497
498 data = gr_vehicle_data_new_from_file (file);
499 fclose (file);
500 if (!data) {
501 return NULL;
502 }
503
504 if (data->engine_url) {
505 str = gr_get_fullurl (data->engine_url, url);
506 data->engine = tcl_GetCache (str);
507 if (!data->engine) {
508 file = gr_open_file (str, "r");
509 if (file) {
510 data->engine = gr_engine_data_new_from_file (file);
511 fclose (file);
512
513 if (!data) {
514 goto ERROR;
515 }
516 tcl_PutCache (str, data);
517 } else {
518 goto ERROR;
519 }
520 }
521 gr_INCREF (data->engine);
522 }
523
524 for (i=0; i<data->num_wheel; i++) {
525 if (data->wheel[i].tire_url) {
526 str = gr_get_fullurl (data->wheel[i].tire_url, url);
527 data->wheel[i].tire = tcl_GetCache (str);
528 if (!data->wheel[i].tire) {
529 file = gr_open_file (str, "r");
530 if (file) {
531 data->wheel[i].tire = gr_tire_data_new_from_file (file);
532 fclose (file);
533
534 if (!data->wheel[i].tire) {
535 goto ERROR;
536 }
537 tcl_PutCache (str, data->wheel[i].tire);
538 } else {
539 goto ERROR;
540 }
541 }
542 gr_INCREF (data->wheel[i].tire);
543 }
544
545 if (data->wheel[i].fb_url) {
546 str = gr_get_fullurl (data->wheel[i].fb_url, url);
547 data->wheel[i].fb = tcl_GetCache (str);
548 if (!data->wheel[i].fb) {
549 file = gr_open_file (str, "r");
550 if (file) {
551 data->wheel[i].fb = gr_break_data_new_from_file (file);
552 fclose (file);
553
554 if (!data->wheel[i].fb) {
555 goto ERROR;
556 }
557 tcl_PutCache (str, data->wheel[i].fb);
558 } else {
559 goto ERROR;
560 }
561 }
562 gr_INCREF (data->wheel[i].fb);
563 }
564
565 if (data->wheel[i].sb_url) {
566 str = gr_get_fullurl (data->wheel[i].sb_url, url);
567 data->wheel[i].sb = tcl_GetCache (str);
568 if (!data->wheel[i].sb) {
569 file = gr_open_file (str, "r");
570 if (file) {
571 data->wheel[i].sb = gr_break_data_new_from_file (file);
572 fclose (file);
573
574 if (!data->wheel[i].sb) {
575 goto ERROR;
576 }
577 tcl_PutCache (str, data->wheel[i].sb);
578 } else {
579 goto ERROR;
580 }
581 }
582 gr_INCREF (data->wheel[i].sb);
583 }
584 }
585 tcl_PutCache (url, data);
586 }
587
588 gr_INCREF (data);
589 return data;
590
591 ERROR:
592 if (data) {
593 if (data->engine) {
594 gr_DECREF (data->engine);
595 }
596 if (data->wheel) {
597 for (i=0; i<data->num_wheel; i++) {
598 if (data->wheel[i].tire) {
599 gr_DECREF (data->wheel[i].tire);
600 }
601 if (data->wheel[i].fb) {
602 gr_DECREF (data->wheel[i].fb);
603 }
604 if (data->wheel[i].sb) {
605 gr_DECREF (data->wheel[i].sb);
606 }
607 }
608 }
609 free (data);
610 }
611 return NULL;
612 }
613
614 static void
gr_vehicle_calculate_constants(GrVehicle * vehicle)615 gr_vehicle_calculate_constants (GrVehicle *vehicle)
616 {
617 float s, c;
618 GrMatrix m;
619 int i;
620
621 GR_BEGIN_FUNCTION ();
622
623 s = 1.0; /* sin (90[deg]) */
624 c = 0.0; /* cos (90[deg]) */
625
626 gr_matrix_rotate_z (gr_matrix_identity, &m, s, c);
627 for (i=0; i<vehicle->data->num_wheel; i++) {
628 gr_matrix_mult_vertex (&m, &vehicle->data->wheel[i].pos,
629 &vehicle->data->wheel[i].rpos);
630 }
631
632 vehicle->fg.c.x = 0;
633 vehicle->fg.c.y = 0;
634 vehicle->fg.c.z = - GRAVITY * vehicle->body.mass;
635
636 gr_rigid_body_reset (&vehicle->body, 0);
637 gr_rigid_body_reset (&vehicle->body, 1);
638 }
639
640 static void
gr_vehicle_free(GrVehicle * vehicle)641 gr_vehicle_free (GrVehicle *vehicle)
642 {
643 int i;
644
645 if (vehicle->scene) {
646 gr_DECREF (vehicle->scene);
647 }
648 if (vehicle->wheel && vehicle->data) {
649 for (i=0; i<vehicle->data->num_wheel; i++) {
650 if (vehicle->wheel[i].scene) {
651 gr_DECREF (vehicle->wheel[i].scene);
652 }
653 }
654 free (vehicle->wheel);
655 }
656
657 if (vehicle->data)
658 gr_DECREF (vehicle->data);
659
660 free (vehicle);
661 }
662
663 GrVehicle *
gr_vehicle_new(GrVehicleData * vd)664 gr_vehicle_new (GrVehicleData *vd)
665 {
666 GrVehicle *v;
667 int i, j;
668
669 v = gr_new0 (GrVehicle, 1);
670 gr_FREE_FUNC (v, gr_vehicle_free);
671
672 v->wheel = gr_new0 (GrWheel, vd->num_wheel);
673 v->data = vd;
674
675 /* initialize rigid body's parameters */
676 v->body.mass = v->data->mass;
677
678 v->body.m_j = *gr_matrix_identity;
679
680 for (i=0; i<3; i++) {
681 for (j=0; j<3; j++) {
682 v->body.m_j.f[i][j] = v->data->moment[i*3 + j];
683 }
684 }
685
686 gr_vehicle_calculate_constants (v);
687
688 return v;
689 }
690
691 static void
gr_vehicle_calc_geometry(GrVehicle * vehicle,GrCourse * course,int which)692 gr_vehicle_calc_geometry (GrVehicle *vehicle, GrCourse *course, int which)
693 {
694 int i;
695 GrWheel *w;
696 GrWheelData *wd;
697
698 GR_BEGIN_FUNCTION ();
699
700 for (i=0; i<vehicle->data->num_wheel; i++) {
701 w = &vehicle->wheel[i];
702 wd = &vehicle->data->wheel[i];
703
704 #if 0
705 /* RELATIVE WHEEL POS */
706 gr_vertex_scale (&w->state[which].pos, &wd->susp, w->state[which].ds);
707 gr_vertex_add_a (&w->state[which].pos, &wd->pos);
708 #endif
709
710 /* ABSOLUTE WHEEL POS */
711 gr_matrix_mult_vertex (&vehicle->body.state[which].m_zyx,
712 &wd->pos,
713 &w->state[which].abs_pos);
714 gr_vertex_add_a (&w->state[which].abs_pos,
715 &vehicle->body.state[which].pos);
716
717 #if 0
718 if (wd->flag & GR_WHEEL_STEERING) {
719 /* RELATIVE WHEEL PIVOT */
720 gr_vertex_rotate (&w->pivot, &wd->pivot,
721 sin (w->control.angle), cos (w->control.angle),
722 wd->susp.c.x, wd->susp.c.y, wd->susp.c.z);
723 }
724
725 /* ABSOLUTE WHEEL PIVOT */
726 gr_matrix_mult_vertex (&vehicle->body.state[which].m_zyx,
727 &pivot, &w->state[which].pivot);
728 #endif
729 }
730 }
731
732 typedef struct {
733 int num;
734 GrVehicle *vehicle;
735 int wheel_no;
736 int which;
737 float ds;
738 GrVertex top;
739 GrVertex dir;
740 GrVertex susp;
741 GrVertex pivot;
742 GrVertex pos;
743 } GeometryCalcHint;
744
745 static int
surf_foreach_callback(GrCourse * course,int seg,int surf,void * data)746 surf_foreach_callback (GrCourse *course, int seg, int surf, void *data)
747 {
748 GeometryCalcHint *hint = data;
749 GrCSurface *surface;
750 GrVehicle *vehicle;
751 GrWheel *w;
752 GrWheelData *wd;
753 GrVertex top, tmp, pos, cp;
754 GrVertex pivot;
755 float ds;
756
757 vehicle = hint->vehicle;
758 w = &vehicle->wheel[hint->wheel_no];
759 wd = &vehicle->data->wheel[hint->wheel_no];
760 surface = course->segs[seg]->surfs + surf;
761
762 gr_matrix_mult_vertex (&surface->plane.m, &hint->pos, &pos);
763 if (pos.c.z > hint->ds) {
764 /* already more closed plane was found. */
765 return 0;
766 }
767
768 /*
769 * top = pivot x normal x pivot
770 */
771 gr_matrix_mult_vertex (&surface->plane.m, &hint->pivot, &pivot);
772 gr_vertex_sub_a (&pivot, &surface->plane.orig);
773 //gr_vertex_cross (&tmp, &pivot, gr_vertex_unit_z);
774 tmp.c.x = pivot.c.y;
775 tmp.c.y = -pivot.c.x;
776 tmp.c.z = 0;
777 gr_vertex_cross (&top, &tmp, &pivot);
778 gr_vertex_normalize (&top, &top);
779
780 if (top.c.z == 0) {
781 /*
782 * FIXME
783 * when top.c.z == 0, then disk and plane are parallel. This type of
784 * collision must be handled by different algorism.
785 */
786
787 return 0;
788 }
789
790 ds = pos.c.z / top.c.z;
791 if (ds >= hint->ds) {
792 /* Already more closed plane was found. */
793 /* By initializing hint->ds as wd->tire->radius,
794 * non-contacting surface is ignored.
795 */
796 return 0;
797 }
798
799 /* cp vector is where disk and plane contact with each other */
800
801 gr_vertex_scale_xy (&tmp, &top, ds);
802 gr_vertex_sub_xy (&cp, &pos, &tmp);
803 cp.c.z = 0.0;
804
805 if (gr_course_surface_check_inner (surface, &cp)) {
806 w->state[hint->which].surf = surface;
807 w->state[hint->which].segment = seg;
808 hint->ds = ds;
809 hint->top = top;
810
811 return 1;
812 } else {
813 return 0;
814 }
815 }
816
817 #define MAX_SPEED (400.0 * 1000 / 3600)
818
gr_vehicle_integral(GrVehicle * vehicle,GrCourse * course,int from,int to,double h)819 void gr_vehicle_integral (GrVehicle *vehicle,
820 GrCourse *course,
821 int from, int to, double h)
822 {
823 GrVehicleData *vdata = vehicle->data;
824 int i;
825 GrVertex speed;
826 GrVertex a, b, c, grip, top, pivot, plane_n;
827 GrVertex tmp; /* temporary */
828 float la, lb, lc;
829 float slip;
830 float grip_friction;
831 float n;
832 float mu;
833 float f; /* temorary */
834 GrVertex force;
835 GrWheel *w;
836 GrWheelData *wd;
837 GrTireData *td;
838 int index;
839 float engine_torque;
840 GeometryCalcHint hint;
841
842 GR_BEGIN_FUNCTION ();
843
844 gr_vertex_copy (&tmp, &vehicle->body.state[from].spd);
845 gr_matrix_mult_vertex (&vehicle->body.state[from].m_rzyx,
846 &tmp, &speed);
847
848 if (vehicle->control.gear >= vdata->engine->max_gear) {
849 vehicle->control.gear = vdata->engine->max_gear - 1;
850 } else if (vehicle->control.gear < 0) {
851 vehicle->control.gear = 0;
852 }
853
854 f = vehicle->state[from].rpm / vdata->engine->limit;
855 if (f > 1.0) {
856 engine_torque = 0;
857 } else {
858 if (f < 0.0)
859 f = 0.0;
860
861 index = 127 * f;
862 ADJUST_INDEX (index, 0, 127);
863 engine_torque = vehicle->control.accel * vdata->engine->torque[index];
864 }
865 engine_torque += (vehicle->control.accel - 1.0) * f * vdata->engine->friction;
866 engine_torque *= vdata->engine->gear_ratio[vehicle->control.gear]
867 * vehicle->data->gear_ratio
868 / vehicle->data->num_driving_wheel;
869
870 hint.vehicle = vehicle;
871 hint.which = from;
872 vehicle->state[to].rpm = 0;
873
874 for (i=0; i<vehicle->data->num_wheel; i++) {
875 w = vehicle->wheel + i;
876 wd = vehicle->data->wheel + i;
877 td = wd->tire;
878
879 w->control.f = 0.0;
880 if (wd->flag & GR_WHEEL_DRIVING) {
881 w->control.f += engine_torque / td->radius;
882 }
883
884 if (wd->flag & GR_WHEEL_STEERING) {
885 w->control.angle = - RAD(50.0) * vehicle->control.steering;
886 }
887
888 slip = SLIP_FACTOR * fabs(w->state[from].dr) * td->radius;
889 if (slip > 1.0)
890 slip = 1.0;
891 index = slip * 127;
892 ADJUST_INDEX (index, 0, 127);
893
894 if (wd->sb) {
895 w->control.f -= SIDE_BREAK_FORCE * SIGN(w->state[from].dr)
896 * wd->sb->mu[index] * vehicle->control.side_break;
897 }
898 if (wd->fb) {
899 w->control.f -= FOOT_BREAK_FORCE * SIGN(w->state[from].dr)
900 * wd->fb->mu[index] * vehicle->control.foot_break;
901 }
902
903 w->state[from].surf = NULL;
904 hint.wheel_no = i;
905 hint.which = from;
906 hint.num = 0;
907 hint.ds = td->radius;
908
909 /* ABSOLUTE WHEEL PIVOT DIRECTORY WITH CURRENT STEERING ANGLE
910 * pivot: pivot vector in vehicle's view.
911 * hint.pivot: pivot vector in world view.
912 */
913 gr_vertex_rotate (&pivot, &wd->pivot,
914 sin (w->control.angle), cos (w->control.angle),
915 wd->susp.c.x, wd->susp.c.y, wd->susp.c.z);
916 gr_matrix_mult_vertex (&vehicle->body.state[from].m_zyx,
917 &pivot, &hint.pivot);
918
919 /* CURRENT ABSOLUTE COORDINATE OF WHEEL */
920 hint.pos = w->state[from].abs_pos;
921
922 gr_course_foreach_surface (course, &hint.pos, MAX_SPEED * h + 3,
923 surf_foreach_callback, &hint);
924
925 grip_friction = 0.0;
926 if (!w->state[from].surf) {
927 w->state[from].ds = 0;
928 w->state[from].slip = 0;
929 } else {
930 gr_vertex_add (&tmp, &hint.top, &w->state[from].surf->plane.orig);
931 gr_matrix_mult_vertex (&w->state[from].surf->plane.im, &tmp, &tmp);
932 gr_matrix_mult_vertex (&vehicle->body.state[from].m_rzyx, &tmp, &top);
933 gr_vertex_cross (&grip, &pivot, &top);
934
935 /* NORMAL VECTOR OF SURFACE RELATIVE FROM VEHICLE'S VIEW */
936 gr_matrix_mult_vertex (&vehicle->body.state[from].m_rzyx,
937 &w->state[from].surf->plane.n,
938 &plane_n);
939
940 f = gr_vertex_product (&top, &wd->susp);
941 w->state[from].ds = - f * hint.ds + td->radius;
942 n = w->state[from].ds * wd->ks
943 + (w->state[from].ds - w->state[from].pds) / h * wd->kd;
944
945 gr_vertex_scale (&tmp, &top, -f);
946 gr_vertex_add_a (&tmp, &plane_n);
947 f = gr_vertex_product (&plane_n, &tmp);
948 n += f * 10000;
949
950 force = *gr_vertex_origin;
951
952 if (n > 0) {
953 /* WHEEL CIRCUMFERENCE VELOCITY RELATIVE FROM VEHICLE BODY */
954 la = td->radius * w->state[from].dr;
955 gr_vertex_scale (&a, &grip, la);
956 la = fabs(la);
957
958 #if 0
959 w->state[from].grip = grip;
960 gr_vertex_scale_a (&w->state[from].grip,
961 1.0 / gr_vertex_length (&w->state[from].grip));
962 #endif
963
964 /* WHEEL VELOCITY RELATIVE FROM VEHICLE BODY */
965 gr_vertex_scale (&b, &wd->rpos, vehicle->body.state[from].omg.c.z);
966 gr_vertex_add_a (&b, &speed);
967 f = gr_vertex_product (&plane_n, &b);
968 gr_vertex_scale (&tmp, &plane_n, f);
969 gr_vertex_sub_a (&b, &tmp);
970 lb = gr_vertex_length (&b);
971
972 gr_vertex_normalize (&w->state[from].speed, &b);
973
974 /* TRUE GRIP VECTOR RELATIVE FROM VEHICLE BODY */
975 gr_vertex_sub (&c, &a, &b);
976 lc = gr_vertex_length (&c);
977
978 if (lc > 0.0001) {
979 if (la + lb > 0)
980 slip = lc / (la + lb);
981 else
982 slip = 0;
983
984 gr_message ("slip ratio[%d] %f", i, slip);
985
986 index = slip * 127;
987 ADJUST_INDEX (index, 0, 127);
988 mu = td->mu[index];
989
990 gr_vertex_normalize (&tmp, &c);
991 gr_vertex_scale (&force, &tmp, mu * n);
992 #if 0
993 gr_vertex_scale_a (&w->state[from].force, 1 / mu / n);
994 #endif
995
996 grip_friction = gr_vertex_product (&grip, &force);
997 }
998
999 w->state[to].slip = slip;
1000 lc /= 30.0 / 3.6; // 30 [km/h]
1001 if (lc < 1.0) {
1002 w->state[to].slip *= lc;
1003 }
1004 } else {
1005 w->state[to].slip = 0;
1006 }
1007
1008 gr_vertex_scale (&tmp, &plane_n, n);
1009 gr_vertex_add_a (&force, &tmp);
1010
1011 #if 0
1012 //printf ("%f %f %f\n", force.c.x, force.c.y, force.c.z);
1013 w->state[from].force = force;
1014 gr_vertex_scale_a (&w->state[from].force,
1015 1.0 / gr_vertex_length (&w->state[from].force));
1016 #endif
1017
1018 gr_vertex_scale_a (&top, wd->tire->radius);
1019 gr_vertex_scale (&tmp, &wd->susp, w->state[from].ds);
1020 gr_vertex_sub_a (&tmp, &top);
1021 gr_vertex_add_a (&tmp, &wd->pos);
1022 gr_rigid_body_add_force_relative (&vehicle->body, from, &tmp, &force);
1023 }
1024 w->state[to].pds = w->state[from].ds;
1025 /* INTEGRAL WHEEL SPIN */
1026 w->state[to].dr = w->state[from].dr +
1027 h * td->radius * (w->control.f - grip_friction) / wd->m;
1028
1029 if (wd->flag & GR_WHEEL_DRIVING) {
1030 vehicle->state[to].rpm += w->state[to].dr;
1031 }
1032 }
1033
1034 /*
1035 * FIXME
1036 * Engine rotational velocity is calculated from average of driving wheels.
1037 */
1038 vehicle->state[to].rpm *= 60 / 2.0 / M_PI
1039 / vehicle->data->num_driving_wheel
1040 * vdata->engine->gear_ratio[vehicle->control.gear]
1041 * vdata->gear_ratio;
1042
1043 /* DOWN FORCE BY WING */
1044 if (speed.c.x > 0) {
1045 force = *gr_vertex_unit_z;
1046 force.c.z *= - vehicle->data->down_force_speed_ratio * speed.c.x;
1047 gr_rigid_body_add_force_relative (&vehicle->body, from,
1048 gr_vertex_origin, &force);
1049 }
1050 /* GRAVITY */
1051 gr_rigid_body_add_force (&vehicle->body, from,
1052 gr_vertex_origin, &vehicle->fg);
1053 gr_rigid_body_integral (&vehicle->body, from, h);
1054
1055 gr_vehicle_calc_geometry (vehicle, course, to);
1056 }
1057
1058 static int
height_foreach_callback(GrCourse * course,int seg,int surf,float z,void * data)1059 height_foreach_callback (GrCourse *course,
1060 int seg,
1061 int surf,
1062 float z,
1063 void *data)
1064 {
1065 float *biggest = data;
1066
1067 if (*biggest < z) {
1068 *biggest = z;
1069 }
1070
1071 return 0;
1072 }
1073
1074 /*
1075 * gr_vehicle_find_initial_height
1076 *
1077 * x and y coordinate of vehicle must be set as initial position.
1078 * This routine find the biggest z coordinate in specified course and
1079 * initialize z coordinate of vehicle.
1080 *
1081 * vehicle: vehicle.
1082 * course: course data.
1083 * which: which state of vehicle must be initialized.
1084 */
1085
gr_vehicle_find_initial_height(GrVehicle * vehicle,GrCourse * course,int which)1086 void gr_vehicle_find_initial_height (GrVehicle *vehicle,
1087 GrCourse *course,
1088 int which)
1089 {
1090 int i;
1091 GrWheel *w;
1092 GrWheelData *wd;
1093 float min_z;
1094 float max_z;
1095 float z;
1096
1097 GR_BEGIN_FUNCTION ();
1098
1099 /* find smallest z coordinate of the course */
1100 min_z = course->segs[0]->min.c.z;
1101 for (i=1; i<course->num_segs; i++) {
1102 if (min_z > course->segs[i]->min.c.z) {
1103 min_z = course->segs[i]->min.c.z;
1104 }
1105 }
1106 max_z = min_z;
1107
1108 gr_vehicle_calc_geometry (vehicle, course, which);
1109
1110 for (i=0; i<vehicle->data->num_wheel; i++) {
1111 w = vehicle->wheel + i;
1112 wd = vehicle->data->wheel + i;
1113 z = min_z;
1114
1115 gr_course_foreach_surface_xy (course, &w->state[which].abs_pos,
1116 height_foreach_callback, &z);
1117 z += wd->tire->radius - wd->pos.c.z;
1118 if (max_z < z) {
1119 max_z = z;
1120 }
1121 }
1122
1123 vehicle->body.state[which].pos.c.z = max_z;
1124 gr_vehicle_calc_geometry (vehicle, course, which);
1125 }
1126
1127 static void
gr_vehicle_init(GrVehicle * vehicle,int state)1128 gr_vehicle_init (GrVehicle *vehicle, int state)
1129 {
1130 int i;
1131
1132 gr_rigid_body_reset (&vehicle->body, state);
1133
1134 for (i=0; i<vehicle->data->num_wheel; i++) {
1135 vehicle->wheel[i].state[state].dr = 0;
1136 vehicle->wheel[i].state[state].ds = 0;
1137 vehicle->wheel[i].state[state].pds = 0;
1138 vehicle->wheel[i].state[state].pds = 0;
1139 }
1140 }
1141
1142 int
gr_vehicle_put(GrVehicle * vehicle,GrCourse * course,double threshold,double h,int limit)1143 gr_vehicle_put (GrVehicle *vehicle,
1144 GrCourse *course,
1145 double threshold,
1146 double h,
1147 int limit)
1148 {
1149 GrVertex diff;
1150 double len;
1151 int i, j;
1152 int from, to;
1153 int done = 0;
1154
1155 GR_BEGIN_FUNCTION ();
1156
1157 gr_vehicle_init (vehicle, 0);
1158 gr_vehicle_init (vehicle, 1);
1159
1160 gr_vehicle_find_initial_height (vehicle, course, 0);
1161
1162 for (i=0; i<vehicle->data->num_wheel; i++) {
1163 vehicle->wheel[i].state[0].pds = 0;
1164 vehicle->data->wheel[i].kd /= 100;
1165
1166 if (!(vehicle->data->wheel[i].flag & GR_WHEEL_STEERING)) {
1167 vehicle->wheel[i].state[0].pivot = vehicle->data->wheel[i].pivot;
1168 vehicle->wheel[i].state[1].pivot = vehicle->data->wheel[i].pivot;
1169 }
1170 }
1171
1172 for (i=0; i<limit; i++) {
1173 from = i % 2;
1174 to = (i + 1) % 2;
1175
1176 gr_vehicle_integral (vehicle, course, from, to, h);
1177 gr_vertex_scale (&vehicle->body.state[to].spd,
1178 &vehicle->body.state[to].spd, 0.1);
1179 gr_vertex_scale (&vehicle->body.state[to].omg,
1180 &vehicle->body.state[to].omg, 0.1);
1181
1182 gr_vertex_sub (&diff,
1183 &vehicle->body.state[to].pos,
1184 &vehicle->body.state[from].pos);
1185
1186 len = gr_vertex_length (&diff);
1187
1188 if (len <= threshold) {
1189 vehicle->body.state[from] = vehicle->body.state[to];
1190 for (j=0; j<vehicle->data->num_wheel; j++) {
1191 vehicle->wheel[j].state[from] = vehicle->wheel[j].state[to];
1192 }
1193 done = 1;
1194 break;
1195 }
1196 }
1197
1198 return done;
1199 }
1200