Initial revision
[blender.git] / source / gameengine / Physics / Sumo / Fuzzics / sample / particle0.cpp
1 //#define FAKE_IT
2 #define USE_COMPLEX
3 #define QUADS
4
5 #include <algorithm>
6 #include <new>
7 #include <GL/glut.h>
8
9 #include "MT_MinMax.h"
10 #include "MT_Point3.h"
11 #include "MT_Vector3.h"
12 #include "MT_Quaternion.h"
13 #include "MT_Matrix3x3.h"
14 #include "MT_Transform.h"
15
16 #include "SM_Object.h"
17 #include "SM_Scene.h"
18
19 #include "solid.h"
20
21 const MT_Scalar bowl_curv = 0.10;
22 const MT_Scalar timeStep = 0.04;
23 const MT_Scalar ground_margin = 0.0;
24 const MT_Scalar sphere_radius = 0.5;
25
26 const MT_Vector3 gravity(0, -9.8, 0);
27
28 static MT_Scalar DISTANCE = 5; 
29
30 static MT_Scalar ele = 0, azi = 0;
31 static MT_Point3 eye(0, 0, DISTANCE);
32 static MT_Point3 center(0, 0, 0);
33
34 inline double irnd() { return 2 * MT_random() - 1; }  
35
36 static const double SCALE_BOTTOM = 0.5;
37 static const double SCALE_FACTOR = 2.0;
38
39 SM_ShapeProps g_shapeProps = {
40         1.0,    // mass
41         1.0,    // inertia 
42         0.9,    // linear drag
43         0.9     // angular drag
44 };
45
46 SM_MaterialProps g_materialProps = {
47         0.7,    // restitution
48         0.0,    // friction 
49         0.0,    // spring constant
50         0.0     // damping
51 };
52  
53
54 void toggleIdle();
55
56
57 void newRandom();
58
59 void coordSystem() {
60     glDisable(GL_LIGHTING);
61     glBegin(GL_LINES);
62     glColor3f(1, 0, 0);
63     glVertex3d(0, 0, 0);
64     glVertex3d(10, 0, 0);
65     glColor3f(0, 1, 0);
66     glVertex3d(0, 0, 0);
67     glVertex3d(0, 10, 0);
68     glColor3f(0, 0, 1);
69     glVertex3d(0, 0, 0);
70     glVertex3d(0, 0, 10);
71     glEnd();
72     glEnable(GL_LIGHTING);
73 }
74
75
76 void display_bbox(const MT_Point3& min, const MT_Point3& max) {
77     glDisable(GL_DEPTH_TEST);
78     glDisable(GL_LIGHTING);
79     glColor3f(0, 1, 1);
80     glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
81     glBegin(GL_QUAD_STRIP);
82     glVertex3d(min[0], min[1], min[2]);
83     glVertex3d(min[0], min[1], max[2]);
84     glVertex3d(max[0], min[1], min[2]);
85     glVertex3d(max[0], min[1], max[2]);
86     glVertex3d(max[0], max[1], min[2]);
87     glVertex3d(max[0], max[1], max[2]);
88     glVertex3d(min[0], max[1], min[2]);
89     glVertex3d(min[0], max[1], max[2]);
90     glVertex3d(min[0], min[1], min[2]);
91     glVertex3d(min[0], min[1], max[2]);
92     glEnd();  
93     glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
94     glEnable(GL_LIGHTING);
95     glEnable(GL_DEPTH_TEST);
96 }
97
98
99
100
101 class GLShape {
102 public:
103     virtual void paint(GLdouble *m) const = 0;
104 };
105
106
107 class GLSphere : public GLShape {
108     MT_Scalar radius;
109 public:
110     GLSphere(MT_Scalar r) : radius(r) {}
111
112     void paint(GLdouble *m) const {
113         glPushMatrix(); 
114         glLoadMatrixd(m);
115         coordSystem();
116         glutSolidSphere(radius, 20, 20);
117         glPopMatrix();
118     }
119 };
120
121
122 class GLBox : public GLShape {
123     MT_Vector3 extent;
124 public:
125     GLBox(MT_Scalar x, MT_Scalar y, MT_Scalar z) : 
126         extent(x, y, z) {}
127
128     void paint(GLdouble *m) const {
129         glPushMatrix(); 
130         glLoadMatrixd(m);
131         coordSystem();
132         glPushMatrix();
133         glScaled(extent[0], extent[1], extent[2]);
134         glutSolidCube(1.0);
135         glPopMatrix();
136         glPopMatrix();
137     }
138 };
139
140
141 class GLCone : public GLShape {
142     MT_Scalar bottomRadius;
143     MT_Scalar height;
144     mutable GLuint displayList;
145
146 public:
147     GLCone(MT_Scalar r, MT_Scalar h) :  
148         bottomRadius(r), 
149         height(h), 
150         displayList(0) {}
151   
152     void paint(GLdouble *m) const { 
153         glPushMatrix(); 
154         glLoadMatrixd(m);
155         coordSystem();
156         if (displayList) glCallList(displayList); 
157         else {
158             GLUquadricObj *quadObj = gluNewQuadric();
159             displayList = glGenLists(1);
160             glNewList(displayList, GL_COMPILE_AND_EXECUTE);
161             glPushMatrix();
162             glRotatef(-90.0, 1.0, 0.0, 0.0);
163             glTranslatef(0.0, 0.0, -1.0);
164             gluQuadricDrawStyle(quadObj, (GLenum)GLU_FILL);
165             gluQuadricNormals(quadObj, (GLenum)GLU_SMOOTH);
166             gluCylinder(quadObj, bottomRadius, 0, height, 15, 10);
167             glPopMatrix();
168             glEndList();
169         }
170         glPopMatrix();
171     }
172 };
173
174 class GLCylinder : public GLShape {
175     MT_Scalar radius;
176     MT_Scalar height;
177     mutable GLuint displayList;
178
179 public:
180     GLCylinder(MT_Scalar r, MT_Scalar h) : 
181         radius(r), 
182         height(h), 
183         displayList(0) {}
184
185     void paint(GLdouble *m) const { 
186         glPushMatrix(); 
187         glLoadMatrixd(m);
188         coordSystem();
189         if (displayList) glCallList(displayList); 
190         else {
191             GLUquadricObj *quadObj = gluNewQuadric();
192             displayList = glGenLists(1);
193             glNewList(displayList, GL_COMPILE_AND_EXECUTE);
194             glPushMatrix();
195             glRotatef(-90.0, 1.0, 0.0, 0.0);
196             glTranslatef(0.0, 0.0, -1.0);
197             gluQuadricDrawStyle(quadObj, (GLenum)GLU_FILL);
198             gluQuadricNormals(quadObj, (GLenum)GLU_SMOOTH);
199             gluCylinder(quadObj, radius, radius, height, 15, 10);
200             glPopMatrix ();
201             glEndList();
202         }
203         glPopMatrix();
204     }
205 };
206
207 class Object;
208
209 class Callback : public SM_Callback {
210 public:
211         Callback(Object& object) : m_object(object) {}
212
213         virtual void do_me();
214
215 private:
216         Object& m_object;
217 };
218
219
220 class Object {
221 public:
222     Object(GLShape *gl_shape, SM_Object& object) :
223         m_gl_shape(gl_shape),
224         m_object(object),
225                 m_callback(*this)
226         {
227                         m_object.registerCallback(m_callback);
228                 }
229         
230     ~Object() {}
231
232     void paint() {
233         m_gl_shape->paint(m);
234         //      display_bbox(m_bbox.lower(), m_bbox.upper());
235     }
236         
237         MT_Vector3 getAhead() {
238                 return MT_Vector3(-m[8], -m[9], -m[10]);
239         }
240
241     void clearMomentum() {
242                 m_object.clearMomentum();
243     }
244         
245     void setMargin(MT_Scalar margin) {
246         m_object.setMargin(margin);
247     }
248         
249     void setScaling(const MT_Vector3& scaling) {
250         m_object.setScaling(scaling); 
251     }
252
253     void setPosition(const MT_Point3& pos) {
254                 m_object.setPosition(pos);
255     }
256
257     void setOrientation(const MT_Quaternion& orn) {
258         m_object.setOrientation(orn);
259     }
260
261     void applyCenterForce(const MT_Vector3& force) {
262                 m_object.applyCenterForce(force);
263     }
264
265     void applyTorque(const MT_Vector3& torque) {
266                 m_object.applyTorque(torque);
267     }
268
269     MT_Point3 getWorldCoord(const MT_Point3& local) const {
270         return m_object.getWorldCoord(local);
271     }
272
273     MT_Vector3 getLinearVelocity() const {
274         return m_object.getLinearVelocity();
275     }
276
277     void setMatrix() {
278                 m_object.getMatrix(m);
279     }
280
281 private:
282     GLShape        *m_gl_shape;
283     SM_Object&      m_object;
284         DT_Scalar       m[16];
285         Callback        m_callback;
286 };
287
288
289 void Callback::do_me()
290 {
291         m_object.setMatrix();
292 }
293
294
295 const MT_Scalar SPACE_SIZE = 2;
296
297 static GLSphere gl_sphere(sphere_radius);
298 static GLBox gl_ground(50.0, 0.0, 50.0);
299
300
301
302 #ifdef USE_COMPLEX
303
304 const int GRID_SCALE = 10;
305 const MT_Scalar GRID_UNIT  = 25.0 / GRID_SCALE;
306
307 DT_ShapeHandle createComplex() {
308     DT_ShapeHandle shape = DT_NewComplexShape();
309     for (int i0 = -GRID_SCALE; i0 != GRID_SCALE; ++i0) {
310         for (int j0 = -GRID_SCALE; j0 != GRID_SCALE; ++j0) {
311             int i1 = i0 + 1;
312             int j1 = j0 + 1;
313 #ifdef QUADS
314             DT_Begin();
315             DT_Vertex(GRID_UNIT * i0, bowl_curv * i0*i0, GRID_UNIT * j0);
316             DT_Vertex(GRID_UNIT * i0, bowl_curv * i0*i0, GRID_UNIT * j1);
317             DT_Vertex(GRID_UNIT * i1, bowl_curv * i1*i1, GRID_UNIT * j1);
318             DT_Vertex(GRID_UNIT * i1, bowl_curv * i1*i1, GRID_UNIT * j0);
319             DT_End();
320 #else
321             DT_Begin();
322             DT_Vertex(GRID_UNIT * i0, 0, GRID_UNIT * j0);
323             DT_Vertex(GRID_UNIT * i0, 0, GRID_UNIT * j1);
324             DT_Vertex(GRID_UNIT * i1, 0, GRID_UNIT * j1);
325             DT_End();
326
327             DT_Begin();
328             DT_Vertex(GRID_UNIT * i0, 0, GRID_UNIT * j1);
329             DT_Vertex(GRID_UNIT * i1, 0, GRID_UNIT * j1);
330             DT_Vertex(GRID_UNIT * i1, 0, GRID_UNIT * j0);
331             DT_End();
332 #endif
333
334         }
335     }
336     DT_EndComplexShape();
337     return shape;
338 }
339
340
341 static DT_ShapeHandle ground_shape = createComplex();
342
343 #else 
344
345 static DT_ShapeHandle ground_shape = DT_Box(50, 0, 50);
346
347 #endif
348
349 static SM_Object sm_ground(ground_shape, &g_materialProps, 0, 0);
350 static Object    ground(&gl_ground, sm_ground);
351
352 static SM_Object sm_sphere(DT_Sphere(0.0), &g_materialProps, &g_shapeProps, 0);
353 static Object    object(&gl_sphere, sm_sphere);
354
355
356 static SM_Object sm_ray(DT_Ray(0.0, -1.0, 0.0), 0, 0, 0);
357
358 static SM_Scene   g_scene;
359
360
361 void myinit(void) {
362
363     GLfloat light_ambient[] = { 0.0, 0.0, 0.0, 1.0 };
364     GLfloat light_diffuse[] = { 1.0, 1.0, 1.0, 1.0 };
365     GLfloat light_specular[] = { 1.0, 1.0, 1.0, 1.0 };
366
367     /*  light_position is NOT default value     */
368     GLfloat light_position0[] = { 1.0, 1.0, 1.0, 0.0 };
369     GLfloat light_position1[] = { -1.0, -1.0, -1.0, 0.0 };
370   
371     glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient);
372     glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
373     glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular);
374     glLightfv(GL_LIGHT0, GL_POSITION, light_position0);
375   
376     glLightfv(GL_LIGHT1, GL_AMBIENT, light_ambient);
377     glLightfv(GL_LIGHT1, GL_DIFFUSE, light_diffuse);
378     glLightfv(GL_LIGHT1, GL_SPECULAR, light_specular);
379     glLightfv(GL_LIGHT1, GL_POSITION, light_position1);
380   
381
382     glEnable(GL_LIGHTING);
383     glEnable(GL_LIGHT0);
384     glEnable(GL_LIGHT1);
385   
386     glShadeModel(GL_SMOOTH);
387   
388     glEnable(GL_DEPTH_TEST);
389     glDepthFunc(GL_LESS);
390   
391     //  glEnable(GL_CULL_FACE);
392     //  glCullFace(GL_BACK);
393
394         g_scene.setForceField(gravity);
395     g_scene.add(sm_ground);
396         sm_ground.setMargin(ground_margin);
397
398     new(&object) Object(&gl_sphere, sm_sphere);
399
400
401         object.setMargin(sphere_radius);
402         
403     g_scene.add(sm_sphere);
404     
405     ground.setPosition(MT_Point3(0, -10, 0));
406     ground.setOrientation(MT_Quaternion(0, 0, 0, 1));
407     ground.setMatrix();
408     center.setValue(0.0, 0.0, 0.0);
409
410     newRandom();
411 }
412
413
414 //MT_Point3 cp1, cp2;
415 //bool intersection;
416
417 bool g_hit = false;
418 MT_Point3 g_spot;
419 MT_Vector3 g_normal;
420
421
422 void display(void) {
423     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
424
425     ground.paint();
426         object.paint();
427         
428         if (g_hit) {
429                 glPointSize(5);
430                 glBegin(GL_POINTS);
431                 glVertex3d(g_spot[0], g_spot[1], g_spot[2]);
432                 glEnd();
433                 glPointSize(1);
434         }
435
436         
437
438 #ifdef COLLISION
439     glDisable(GL_DEPTH_TEST);
440     glDisable(GL_LIGHTING);
441     glColor3f(1, 1, 0);
442     if (intersection) {
443         glPointSize(5);
444         glBegin(GL_POINTS);
445         glVertex3d(cp1[0], cp1[1], cp1[2]);
446         glEnd();
447         glPointSize(1);
448     }
449     else {
450         glBegin(GL_LINES); 
451         glVertex3d(cp1[0], cp1[1], cp1[2]);
452         glVertex3d(cp2[0], cp2[1], cp2[2]);
453         glEnd();
454     }
455     glEnable(GL_LIGHTING);
456     glEnable(GL_DEPTH_TEST);
457 #endif
458
459     glFlush();
460     glutSwapBuffers();
461 }
462
463
464
465
466
467 void newRandom() {
468         object.setPosition(MT_Point3(0, 0, 0));
469         object.clearMomentum();
470         object.setMatrix();
471         
472     display();
473 }
474
475 void moveAndDisplay() {
476         g_scene.proceed(timeStep, 0.01);
477
478         MT_Vector3 normal(0, 1, 0);
479
480         MT_Point3 from = object.getWorldCoord(MT_Point3(0, 0, 0));
481         MT_Point3 to   = from - normal * 10.0;
482
483         g_hit = DT_ObjectRayTest(sm_ground.getObjectHandle(), 
484                                                          from.getValue(),
485                                                          to.getValue(), g_spot.getValue(), 
486                                                          g_normal.getValue());
487
488         // Scrap
489 #define DO_FH
490 #ifdef DO_FH
491         MT_Scalar dist = MT_distance(from, g_spot);
492         if (dist < 5.0) {
493                 MT_Vector3 lin_vel = object.getLinearVelocity();
494                 MT_Scalar lin_vel_normal = lin_vel.dot(normal);
495
496                 MT_Scalar spring_extent = dist + lin_vel_normal * (timeStep * 0.5);
497                 
498                 MT_Scalar f_spring = (5.0 - spring_extent) * 3.0;
499                 object.applyCenterForce(normal * f_spring);
500                 object.applyCenterForce(-lin_vel_normal * normal);
501         }
502         
503 #endif
504
505
506     display();
507 }
508
509
510 void turn_left() {
511         object.applyTorque(MT_Vector3(0.0, 10.0, 0.0));
512 }
513
514 void turn_right() {
515         object.applyTorque(MT_Vector3(0.0, -10.0, 0.0));
516 }
517
518 void forward() {
519         object.applyCenterForce(20.0 * object.getAhead());
520 }
521
522 void backward() {
523         object.applyCenterForce(-20.0 * object.getAhead());
524 }
525
526 void jump() {
527         object.applyCenterForce(MT_Vector3(0.0, 200.0, 0.0));
528 }
529
530
531 void toggleIdle() {
532     static bool idle = true;
533     if (idle) {
534         glutIdleFunc(moveAndDisplay);
535         idle = false;
536     }
537     else {
538         glutIdleFunc(NULL);
539         idle = true;
540     }
541 }
542
543
544 void setCamera() {
545     glMatrixMode(GL_PROJECTION);
546     glLoadIdentity();
547     glFrustum(-1.0, 1.0, -1.0, 1.0, 1.0, 200.0);
548     MT_Scalar rele = MT_radians(ele);
549     MT_Scalar razi = MT_radians(azi);
550     eye.setValue(DISTANCE * sin(razi) * cos(rele), 
551                  DISTANCE * sin(rele),
552                  DISTANCE * cos(razi) * cos(rele));
553     gluLookAt(eye[0], eye[1], eye[2], 
554               center[0], center[1], center[2], 
555               0, 1, 0);
556     glMatrixMode(GL_MODELVIEW);
557     display();
558 }
559
560 const MT_Scalar STEPSIZE = 5;
561
562 void stepLeft() { azi -= STEPSIZE; if (azi < 0) azi += 360; setCamera(); }
563 void stepRight() { azi += STEPSIZE; if (azi >= 360) azi -= 360; setCamera(); }
564 void stepFront() { ele += STEPSIZE; if (azi >= 360) azi -= 360; setCamera(); }
565 void stepBack() { ele -= STEPSIZE; if (azi < 0) azi += 360; setCamera(); }
566 void zoomIn() { DISTANCE -= 1; setCamera(); }
567 void zoomOut() { DISTANCE += 1; setCamera(); }
568
569
570 void myReshape(int w, int h) {
571     glViewport(0, 0, w, h);
572     setCamera();
573 }
574
575 void myKeyboard(unsigned char key, int x, int y)
576 {
577     switch (key) 
578     {
579         case 'w': forward();    break;
580         case 's': backward();   break;
581         case 'a': turn_left();  break;
582         case 'd': turn_right(); break;
583         case 'e': jump(); break;        
584     case 'l' : stepLeft(); break;
585     case 'r' : stepRight(); break;
586     case 'f' : stepFront(); break;
587     case 'b' : stepBack(); break;
588     case 'z' : zoomIn(); break;
589     case 'x' : zoomOut(); break;
590     case 'i' : toggleIdle(); break;
591     case ' ' : newRandom(); break;
592     default:
593 //        std::cout << "unused key : " << key << std::endl;
594         break;
595     }
596 }
597
598 void mySpecial(int key, int x, int y)
599 {
600     switch (key) 
601     {
602     case GLUT_KEY_LEFT : stepLeft(); break;
603     case GLUT_KEY_RIGHT : stepRight(); break;
604     case GLUT_KEY_UP : stepFront(); break;
605     case GLUT_KEY_DOWN : stepBack(); break;
606     case GLUT_KEY_PAGE_UP : zoomIn(); break;
607     case GLUT_KEY_PAGE_DOWN : zoomOut(); break;
608     case GLUT_KEY_HOME : toggleIdle(); break;
609     default:
610 //        std::cout << "unused (special) key : " << key << std::endl;
611         break;
612     }
613 }
614
615 void goodbye( void)
616 {
617         g_scene.remove(sm_ground);
618         g_scene.remove(sm_sphere);
619
620     std::cout << "goodbye ..." << std::endl;
621     exit(0);
622 }
623
624 void menu(int choice)
625 {
626
627     static int fullScreen = 0;
628     static int px, py, sx, sy;
629  
630     switch(choice) {
631     case 1:
632         if (fullScreen == 1) {
633             glutPositionWindow(px,py);
634             glutReshapeWindow(sx,sy);
635             glutChangeToMenuEntry(1,"Full Screen",1);
636             fullScreen = 0;
637         } else {
638             px=glutGet((GLenum)GLUT_WINDOW_X);
639             py=glutGet((GLenum)GLUT_WINDOW_Y);
640             sx=glutGet((GLenum)GLUT_WINDOW_WIDTH);
641             sy=glutGet((GLenum)GLUT_WINDOW_HEIGHT);
642             glutFullScreen();
643             glutChangeToMenuEntry(1,"Close Full Screen",1);
644             fullScreen = 1;
645         }
646         break;
647     case 2:
648         toggleIdle();
649         break;
650     case 3:
651         goodbye();
652         break;
653     default:
654         break;
655     }
656 }
657
658 void createMenu()
659 {
660     glutCreateMenu(menu);
661     glutAddMenuEntry("Full Screen", 1);
662     glutAddMenuEntry("Toggle Idle (Start/Stop)", 2);
663     glutAddMenuEntry("Quit", 3);
664     glutAttachMenu(GLUT_RIGHT_BUTTON);
665 }
666
667 int main(int argc, char **argv) {
668     glutInit(&argc, argv);
669     glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
670     glutInitWindowPosition(0, 0);
671     glutInitWindowSize(500, 500);
672     glutCreateWindow("Physics demo");
673
674     myinit();
675     glutKeyboardFunc(myKeyboard);
676     glutSpecialFunc(mySpecial);
677     glutReshapeFunc(myReshape);
678     createMenu();
679     glutIdleFunc(NULL);
680
681     glutDisplayFunc(display);
682     glutMainLoop();
683     return 0;
684 }
685
686
687
688
689
690
691