1 /*
2 * crashtest
3 * (c) 2006 by Bram Stolk, SARA for NWO
4 * bram at gmail.com
5 * LICENSED ACCORDING TO THE GPL
6 */
7
8
9 #include <assert.h>
10 #include <libgen.h> // for dirname(), Is this portable?
11
12 #ifdef __APPLE__
13 #include <OpenGL/glu.h>
14 #else
15 #include <GL/glu.h>
16 #endif
17
18 #include <FL/Fl.H>
19 #include <FL/Fl_Box.H>
20 #include <FL/Fl_Button.H>
21 #include <FL/Fl_Check_Button.H>
22 #include <FL/Fl_Chart.H>
23 #include <FL/Fl_Value_Slider.H>
24 #include <FL/Fl_Gl_Window.H>
25 #include <FL/fl_draw.H>
26
27 #include <plib/ssg.h>
28 #include <plib/ul.h>
29
30 #include "staticworldobject.h"
31 #include "cartobject.h"
32 #include "usercam.h"
33 #include "dynamicboxobject.h"
34 #include "dynamiccylobject.h"
35
36 #include "modelmap.h"
37 #include "crashworld.h"
38 #include "bipedobject.h"
39 #include "stereocontext.h"
40
41
42 static const int MENUH=144;
43 static const int MENUW=1024;
44 static const int WIDGETW=MENUW/4;
45 static const int WIDGETH=MENUH/4;
46
47
48 static float aspectratio=1.0;
49 static int winw, winh;
50 static ssgRoot *scene=0;
51 static ssgContext *monocontext=0;
52 static StereoContext *stereocontext=0;
53 static float fps=60.0;
54 static float timescale=1.0;
55 static float groundgrip=10.0;
56 static float gametime=0.0;
57 static float dt_hist[10]={0.016,0.016,0.016,0.016,0.016,0.016,0.016,0.016,0.016,0.016};
58 static std::string dirprefix;
59 static std::string displaymode="monoscopic";
60
61 static dJointGroupID contactgroup;
62 static dSpaceID bigspace;
63 static dSpaceID staticspace;
64 static dWorldID world;
65
66 static CrashWorld *crashworld=0;
67
68 static CartObject *cart=0;
69 static BipedObject *biped=0;
70 static UserCam *usercam=0;
71
72 static ModelMap *modelmap=0;
73
74 class GraphWidget;
75 class MainWindow;
76
77 MainWindow *mainwin = 0;
78
79 static Fl_Value_Slider *s_speed = 0;
80 static Fl_Value_Slider *s_react = 0;
81 static Fl_Value_Slider *s_tscal = 0;
82 static Fl_Value_Slider *s_bricm = 0;
83 static Fl_Value_Slider *s_cartm = 0;
84 static Fl_Value_Slider *s_ggrip = 0;
85
86 static Fl_Check_Button *c_seatb = 0;
87
88 static Fl_Button *b_start = 0;
89 static Fl_Button *b_reset = 0;
90 static GraphWidget *c_force = 0;
91
92
93
94 static void stop_game(void);
95 static void start_game(void);
96 static void setup_plib(void);
97
98
99
OglErrorCheck(const std::string & context)100 static void OglErrorCheck(const std::string &context)
101 {
102 GLenum error = glGetError();
103 while (error != GL_NO_ERROR)
104 {
105 fprintf(stderr, "plode OpenGL error (%s): %s\n", context.c_str(),gluErrorString(error));
106 error = glGetError();
107 }
108 }
109
110
near_callback(void * data,dGeomID o1,dGeomID o2)111 static void near_callback(void *data, dGeomID o1, dGeomID o2)
112 {
113 assert(o1);
114 assert(o2);
115
116 if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
117 {
118 // colliding a space with something
119 dSpaceCollide2(o1,o2,data,&near_callback);
120 // Note we do not want to test intersections within a space,
121 // only between spaces.
122 return;
123 }
124
125 // Two non space geoms
126
127 WorldObject *wo1 = static_cast<WorldObject*>(dGeomGetData(o1));
128 WorldObject *wo2 = static_cast<WorldObject*>(dGeomGetData(o2));
129
130 const int N = 32;
131 dContact contact[N];
132 int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
133
134 if (n > 0)
135 {
136 assert(wo1);
137 assert(wo2);
138
139 // Create constraints for all the contacts between the geometries.
140 for (int i=0; i<n; i++)
141 {
142 #if 0
143 contact[i].surface.slip1 = 0.14;
144 contact[i].surface.slip2 = 0.14;
145 // contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactSlip1 | dContactSlip2;
146 #endif
147 contact[i].surface.mode = dContactSoftERP | dContactSoftCFM;
148 contact[i].surface.mu = 10.0;
149 contact[i].surface.soft_erp = 0.95;
150 contact[i].surface.soft_cfm = 0.05;
151
152 if (wo1->name == "ground" || wo2->name == "ground")
153 {
154 contact[i].surface.mu = groundgrip;
155 // contact[i].surface.slip1 = 0.55;
156 // contact[i].surface.slip2 = 0.55;
157 }
158
159 dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
160 dGeomID g1 = contact[i].geom.g1;
161 dGeomID g2 = contact[i].geom.g2;
162 dBodyID b1 = dGeomGetBody(g1);
163 dBodyID b2 = dGeomGetBody(g2);
164 assert(b1 || b2);
165 dJointAttach (c, b1, b2);
166 }
167 }
168 }
169
170
171
172
reshape(int w,int h)173 static void reshape(int w, int h)
174 {
175 assert(w && h);
176 winw=w; winh=h;
177 if (monocontext)
178 {
179 glViewport ( 0, 0, w, h ) ;
180 aspectratio = w / (float) h;
181 float fovy = 60.0 * M_PI / 180.0;
182 float nearPlaneDistance = 0.4;
183 float farPlaneDistance = 2500.0;
184 float y = tan(0.5 * fovy) * nearPlaneDistance;
185 float x = aspectratio * y;
186 assert(monocontext);
187 monocontext->setFrustum(-x,x,-y,y,nearPlaneDistance,farPlaneDistance);
188 }
189 if (stereocontext)
190 {
191 stereocontext->SetWindowSize(winw/2, winh);
192 }
193 OglErrorCheck("reshape");
194 }
195
196
197 class GraphWidget : public Fl_Widget
198 {
199 public:
GraphWidget(int X,int Y,int W,int H)200 GraphWidget(int X, int Y, int W, int H) :
201 Fl_Widget(X,Y,W,H,"graph"),
202 sz(W)
203 {
204 values = new float[sz];
205 pixels = new unsigned char[H*W*3];
206 for (int i=0; i<sz; i++) values[i]=0.0;
207 writepos = 0;
208 }
draw(void)209 void draw(void)
210 {
211 memset(pixels, 192, 3*w()*h());
212 for (int i=0; i<sz; i++)
213 {
214 float v = values[(writepos+i)%sz];
215 int y = (int) (h() - 1 - v * h());
216 if (y >= h()) y = h()-1;
217 if (y < 0) y = 0;
218 unsigned char *p = pixels + y *sz*3 + i*3;
219 int b=4;
220 while (y < h() && b--)
221 {
222 p[0]=p[1]=p[2]=0;
223 p += 3*w();
224 y++;
225 }
226 }
227 fl_draw_image(pixels, x(), y(), w(), h());
228 }
sample(float v)229 void sample(float v)
230 {
231 values[writepos] = v;
232 writepos = (writepos+1) % sz;
233 }
234
235 protected:
236 int sz;
237 float *values;
238 unsigned char *pixels;
239 int writepos;
240 };
241
242
243 class SimulationWindow : public Fl_Gl_Window
244 {
245 public:
SimulationWindow(int X,int Y,int W,int H,const char * L=0)246 SimulationWindow(int X, int Y, int W, int H, const char *L=0) : Fl_Gl_Window(X,Y,W,H,L)
247 {
248 shape_dirty = false;
249 }
draw()250 void draw()
251 {
252 if (!valid())
253 {
254 if (!scene)
255 {
256 setup_plib();
257 start_game();
258 }
259 reshape(w(), h());
260 return;
261 }
262 if (shape_dirty)
263 {
264 shape_dirty = false;
265 reshape(neww, newh);
266 }
267 glClearColor(0,0,0.2,0);
268 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ) ;
269 glMatrixMode(GL_MODELVIEW);
270 if (monocontext)
271 {
272 ssgCullAndDraw(scene);
273 }
274 if (stereocontext)
275 {
276 stereocontext->MakeCurrent("left");
277 ssgCullAndDraw(scene);
278 stereocontext->MakeCurrent("right");
279 ssgCullAndDraw(scene);
280 }
281 OglErrorCheck("ssgCullAndDraw");
282 }
handle(int event)283 virtual int handle(int event)
284 {
285 static int lastx, lasty;
286 if (event == FL_PUSH)
287 {
288 lastx = Fl::event_x();
289 lasty = Fl::event_y();
290 return 1;
291 }
292 if (event == FL_DRAG)
293 {
294 int s = Fl::event_state();
295 if (s & FL_BUTTON1)
296 {
297 usercam->ChangeHeading (0.5 * (Fl::event_x() - lastx));
298 usercam->ChangePitch (0.5 * (Fl::event_y() - lasty));
299 }
300 if (s & FL_BUTTON3)
301 {
302 usercam->ChangeDistance(0.1 * (Fl::event_y() - lasty));
303 }
304 lastx = Fl::event_x();
305 lasty = Fl::event_y();
306 return 1;
307 }
308 return Fl_Window::handle(event);
309 }
resize(int X,int Y,int W,int H)310 virtual void resize(int X, int Y, int W, int H)
311 {
312 if (W!=neww || H!=newh)
313 {
314 neww = W;
315 newh = H;
316 shape_dirty=true;
317 }
318 Fl_Gl_Window::resize(X, Y, W, H);
319 }
320 protected:
321 bool shape_dirty;
322 int neww, newh;
323 };
324
325
326
327 class MainWindow : public Fl_Window
328 {
329 public:
resize(int X,int Y,int W,int H)330 virtual void resize(int X, int Y, int W, int H)
331 {
332 child(0)->resize(0,0,W,H-MENUH);
333 child(1)->resize(0,H-MENUH,W,MENUH);
334 Fl_Window::resize(X,Y,W,H);
335 }
handle(int event)336 virtual int handle(int event)
337 {
338 if (event == FL_KEYBOARD)
339 {
340 switch (Fl::event_key())
341 {
342 case FL_Escape:
343 stop_game();
344 this->hide();
345 break;
346 default:
347 break;
348 }
349 return 1;
350 }
351 return Fl_Window::handle(event);
352 }
MainWindow(int X,int Y,int W,int H,const char * L=0)353 MainWindow(int X, int Y, int W, int H, const char *L=0) : Fl_Window(X,Y,W,H,L)
354 {
355 }
356 };
357
358
setup_plib(void)359 static void setup_plib(void)
360 {
361 ssgInit();
362
363 glEnable(GL_DEPTH_TEST);
364 glEnable(GL_CULL_FACE);
365
366 float amb[4]={0.5, 0.5, 0.5, 1};
367 glLightModelfv(GL_LIGHT_MODEL_AMBIENT, amb);
368
369 if (getenv("PLODE_DISPLAYMODE"))
370 displaymode = getenv("PLODE_DISPLAYMODE");
371 assert(displaymode == "monoscopic" || displaymode == "quadbufferstereoscopic" || displaymode == "passivestereoscopic");
372
373 if (displaymode == "monoscopic")
374 {
375 monocontext = new ssgContext();
376 monocontext->makeCurrent();
377 }
378 else
379 {
380 stereocontext = new StereoContext(displaymode=="quadbufferstereoscopic");
381 }
382
383 if (displaymode=="passivestereoscopic")
384 {
385 mainwin->resize(0,0,2048,768);
386 }
387
388 ssgLight *light=ssgGetLight(0);
389 light->setColour(GL_AMBIENT,0,0,0);
390 light->setPosition(0,0,8);
391 light->setSpotlight(true);
392 light->setSpotDirection(0,0,-1);
393 light->setSpotDiffusion(100,180);
394 light->setSpotAttenuation(1, 0.1, 0.04);
395 light->on();
396 }
397
398
idle(void * info)399 static void idle(void *info)
400 {
401 SimulationWindow *simwin = static_cast<SimulationWindow*>(info);
402 assert(simwin);
403
404 if (!scene)
405 return;
406 static ulClock clk;
407 static int frameCounter = 0;
408 clk.setMaxDelta(0.4); // Don't accomodate for systems slower than 2.5 fps
409
410 float elapsed = clk.getDeltaTime();
411 clk.update();
412
413 dt_hist[frameCounter] = elapsed;
414 frameCounter = (frameCounter+1)%10;
415
416 float dt=0;
417 int i;
418 for (i=0; i<10; i++)
419 dt += dt_hist[i];
420 dt = dt / 10;
421 assert(dt>0.0);
422
423 // do not update the fps every time, it is not readable when it is done each tick
424 if (frameCounter == 0)
425 fps = 1.0f / dt;
426
427 gametime += elapsed;
428
429 float timestep=timescale*0.005; // Use 5ms timesteps
430 static float remaining_sim_time=0;
431 remaining_sim_time += timescale * dt;
432
433 while (remaining_sim_time > timestep && timestep)
434 {
435 dSpaceCollide (bigspace, 0, &near_callback);
436 if (crashworld) crashworld->Sustain(timestep);
437 if (cart) cart->Sustain(timestep);
438 dWorldQuickStep (world, timestep);
439 dJointGroupEmpty (contactgroup);
440 remaining_sim_time -= timestep;
441 }
442
443 if (biped) biped->Sustain(timescale*dt); // updates visual
444 usercam->Update(timescale*dt);
445 sgVec3 eye, coi, up;
446 sgSetVec3(up,0,0,1);
447 usercam->GetCameraPos(eye);
448 usercam->GetTargetPos(coi);
449
450 if (monocontext) monocontext->setCameraLookAt(eye, coi, up);
451 if (stereocontext) stereocontext->SetCameraLookAt(eye, coi, up);
452
453 ssgLight *light = ssgGetLight(0);
454 sgVec3 p;
455 biped->GetPos(p);
456 light->setPosition(p[0],p[1],p[2]+5.0);
457 light->setColour(GL_DIFFUSE, 3,3,3);
458
459 // stats
460 if (biped)
461 {
462 static int pos=0;
463 static float accum=0;
464 static int cnt=0;
465 static const int filtersize = 4;
466 float f = biped->NeckForce();
467 accum += f;
468 cnt++;
469 if (cnt==filtersize)
470 {
471 c_force->sample(0.3*accum/filtersize);
472 c_force->redraw();
473 pos = (pos+1)%MENUW;
474 accum=0;
475 cnt=0;
476 }
477 }
478
479 simwin->redraw();
480 }
481
482
483
start_game(void)484 static void start_game(void)
485 {
486 gametime = 0.0;
487
488 // Create ODE world
489
490 dInitODE();
491
492 world = dWorldCreate();
493 bigspace = dHashSpaceCreate(0);
494 staticspace = dSimpleSpaceCreate(bigspace);
495 contactgroup = dJointGroupCreate (0);
496 dWorldSetGravity(world,0,0,-9.8);
497 dWorldSetAutoDisableFlag(world, true);
498 dWorldSetAutoDisableLinearThreshold(world, 0.04);
499 dWorldSetAutoDisableAngularThreshold(world, 0.04);
500 dWorldSetQuickStepNumIterations(world, 20);
501
502 // Create PLIB world
503
504 scene = new ssgRoot();
505
506 crashworld = new CrashWorld(world, bigspace, staticspace, scene, modelmap, dirprefix);
507
508 sgVec3 cartpos;
509 sgSetVec3(cartpos, -29, 0, 0.5);
510 cart = new CartObject(modelmap->Get("cart.3ds"), modelmap->Get("wheel.3ds"), modelmap->Get("nwoplate.ac"), world, bigspace, cartpos);
511 scene->addKid(cart->GetEntity());
512
513 biped = new BipedObject
514 (
515 world,
516 bigspace,
517 cartpos[0], cartpos[1], 1.55,
518 0.2,
519 modelmap->Get("biped_torso.3ds"),
520 modelmap->Get("biped_head.3ds"),
521 modelmap->Get("biped_upperarm.3ds"),
522 modelmap->Get("biped_lowerarm.3ds"),
523 modelmap->Get("biped_upperleg.3ds"),
524 modelmap->Get("biped_lowerleg.3ds"),
525 modelmap->Get("biped_foot.3ds")
526 );
527 scene->addKid(biped->GetEntity());
528 biped->FixFeet(cart->GetBody());
529
530 // Setup camera
531 usercam = new UserCam(-27,0,0);
532 usercam->AddTarget(biped->GetTransform());
533 }
534
535
stop_game(void)536 static void stop_game(void)
537 {
538 delete cart; cart=0;
539 delete biped; biped=0;
540 delete usercam; usercam=0;
541
542 delete crashworld;
543 crashworld=0;
544
545 // Delete ODE stuff
546 dJointGroupDestroy(contactgroup);
547 dSpaceDestroy(staticspace);
548 dSpaceDestroy(bigspace);
549 dWorldDestroy(world);
550
551 dCloseODE();
552 }
553
554
start_cb(Fl_Widget * o)555 void start_cb(Fl_Widget *o)
556 {
557 cart->SetDesiredLinearSpeed(s_speed->value());
558 biped->ReleaseFeet();
559 if (c_seatb->value())
560 biped->FixTorso(cart->GetBody()); // seat belt
561 crashworld->brickwall->SetMass(s_bricm->value());
562 cart->SetReactionTime(s_react->value());
563 cart->SetDensity(s_cartm->value());
564 }
565
566
reset_cb(Fl_Widget * o)567 void reset_cb(Fl_Widget *o)
568 {
569 stop_game();
570 start_game();
571 }
572
573
play_cb(Fl_Widget * o)574 void play_cb(Fl_Widget *o)
575 {
576 if (cart) cart->SetDesiredLinearSpeed(s_speed->value());
577 if (biped) biped->ReleaseFeet();
578 }
579
580
timescale_cb(Fl_Widget * o)581 void timescale_cb(Fl_Widget *o)
582 {
583 timescale = s_tscal->value();
584 }
585
586
brickmass_cb(Fl_Widget * o)587 void brickmass_cb(Fl_Widget *o)
588 {
589 if (crashworld) crashworld->brickwall->SetMass(s_bricm->value());
590 }
591
592
cartmass_cb(Fl_Widget * o)593 void cartmass_cb(Fl_Widget *o)
594 {
595 if (cart) cart->SetDensity(s_cartm->value());
596 }
597
598
groundgrip_cb(Fl_Widget * o)599 void groundgrip_cb(Fl_Widget *o)
600 {
601 groundgrip = s_ggrip->value();
602 }
603
604
reactiontime_cb(Fl_Widget * o)605 void reactiontime_cb(Fl_Widget *o)
606 {
607 if (cart) cart->SetReactionTime(s_react->value());
608 }
609
610
seatbelt_cb(Fl_Widget * o)611 void seatbelt_cb(Fl_Widget *o)
612 {
613 }
614
615
main(int argc,char * argv[])616 int main(int argc, char *argv[])
617 {
618 char *bindirname = dirname(argv[0]);
619 dirprefix="/usr/local/share/crashtest";
620 if (getenv("PLODE_DATADIR"))
621 dirprefix = getenv("PLODE_DATADIR");
622 modelmap = new ModelMap(dirprefix);
623
624 if (!Fl::gl_visual(FL_RGB | FL_MULTISAMPLE))
625 {
626 Fl::warning("Sorry, Your display does not do Multi Sample OpenGL");
627 if (!Fl::gl_visual(FL_RGB))
628 Fl::fatal("Sorry, Your display does not do OpenGL");
629 }
630 Fl::visual(FL_RGB);
631
632 mainwin = new MainWindow(0,0, 1024, 576+MENUH, "crashtest simulator, developed for NWO by Bram Stolk, SARA");
633 mainwin->size_range(352, 288, 1600, 1200, 16,16);
634 SimulationWindow simwin(0, 0, mainwin->w(), mainwin->h()-MENUH, "simulation");
635 Fl_Window guiwin(0,576, mainwin->w(), MENUH, "gui");
636
637 int ypos=0;
638 int xpos=0;
639 int ww = WIDGETW-16;
640
641 s_speed = new Fl_Value_Slider(xpos,ypos,ww,14,"cart speed (m/s)");
642 s_speed->type(FL_HOR_NICE_SLIDER);
643 s_speed->textsize(8);
644 s_speed->precision(0);
645 s_speed->range(-5,25);
646 s_speed->value(20.0);
647 s_speed->align(FL_ALIGN_BOTTOM);
648 s_speed->labelsize(9);
649 ypos += WIDGETH;
650
651 s_react = new Fl_Value_Slider(xpos,ypos,ww,14,"reaction time (s)");
652 s_react->type(FL_HOR_NICE_SLIDER);
653 s_react->textsize(8);
654 s_react->precision(1);
655 s_react->range(0,1);
656 s_react->value(0.5);
657 s_react->callback(reactiontime_cb);
658 s_react->align(FL_ALIGN_BOTTOM);
659 s_react->labelsize(9);
660 ypos += WIDGETH;
661
662 s_bricm = new Fl_Value_Slider(xpos,ypos,ww,14,"wall density (kg/l)");
663 s_bricm->type(FL_HOR_NICE_SLIDER);
664 s_bricm->textsize(8);
665 s_bricm->precision(0);
666 s_bricm->range(1, 20.0);
667 s_bricm->value(15.0);
668 s_bricm->callback(brickmass_cb);
669 s_bricm->align(FL_ALIGN_BOTTOM);
670 s_bricm->labelsize(9);
671 ypos += WIDGETH;
672
673 s_cartm = new Fl_Value_Slider(xpos,ypos,ww,14,"cart density (kg/l)");
674 s_cartm->type(FL_HOR_NICE_SLIDER);
675 s_cartm->textsize(8);
676 s_cartm->precision(1);
677 s_cartm->range(0.5, 8.0);
678 s_cartm->value(2.5);
679 s_cartm->callback(cartmass_cb);
680 s_cartm->align(FL_ALIGN_BOTTOM);
681 s_cartm->labelsize(9);
682 ypos += WIDGETH;
683
684
685 ypos = 0;
686 xpos += WIDGETW;
687
688
689 s_ggrip = new Fl_Value_Slider(xpos,ypos,ww,14,"grip ground");
690 s_ggrip->type(FL_HOR_NICE_SLIDER);
691 s_ggrip->textsize(8);
692 s_ggrip->precision(1);
693 s_ggrip->range(0.1, 10.0);
694 s_ggrip->value(5.0);
695 s_ggrip->callback(groundgrip_cb);
696 s_ggrip->align(FL_ALIGN_BOTTOM);
697 s_ggrip->labelsize(9);
698 ypos += WIDGETH;
699
700 s_tscal = new Fl_Value_Slider(xpos,ypos,ww,14,"time scale (factor)");
701 s_tscal->type(FL_HOR_NICE_SLIDER);
702 s_tscal->textsize(8);
703 s_tscal->precision(2);
704 s_tscal->range(0.0, 1.0);
705 s_tscal->value(1.0);
706 s_tscal->callback(timescale_cb);
707 s_tscal->align(FL_ALIGN_BOTTOM);
708 s_tscal->labelsize(9);
709 ypos += WIDGETH;
710
711 c_seatb = new Fl_Check_Button(xpos,ypos,ww,14,"seat belt");
712 c_seatb->labelsize(9);
713 c_seatb->callback(seatbelt_cb);
714 ypos += WIDGETH;
715
716 ypos = 0;
717 xpos += WIDGETW;
718
719
720 b_start = new Fl_Button(xpos,ypos,ww,14,"start");
721 b_start->callback(start_cb);
722 ypos += WIDGETH;
723
724 b_reset = new Fl_Button(xpos,ypos,ww,14,"reset experiment");
725 b_reset->callback(reset_cb);
726 ypos += WIDGETH;
727
728 ypos = 0;
729 xpos += WIDGETW;
730
731 c_force = new GraphWidget(xpos,ypos,WIDGETW,MENUH);
732
733
734 guiwin.end();
735 guiwin.show();
736
737 mainwin->end();
738 mainwin->show();
739
740 Fl::add_idle(idle, &simwin);
741 Fl::run();
742 }
743
744