1 /*****************************************************************************
2 * DynaMechs: A Multibody Dynamic Simulation Library
3 *
4 * Copyright (C) 1994-2001 Scott McMillan All Rights Reserved.
5 *
6 * This library is free software; you can redistribute it and/or
7 * modify it under the terms of the GNU General Public License
8 * as published by the Free Software Foundation; either version 2
9 * of the License, or (at your option) any later version.
10 *
11 * This library 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 GNU
14 * General Public License for more details.
15 *
16 * You should have received a copy of the GNU Library General Public
17 * License along with this library; if not, write to the Free
18 * Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
19 *****************************************************************************
20 * File: simple.cpp
21 * Author: Scott McMillan
22 * Created: 20 March 1997
23 * Summary: simple GLUT example
24 *****************************************************************************/
25
26 #include <GL/glut.h>
27
28 #include <dmTime.h>
29 #include <dmGLMouse.hpp>
30 #include <dmGLPolarCamera_zup.hpp>
31
32 #include <dm.h> // DynaMechs typedefs, globals, etc.
33 #include <dmArticulation.hpp> // DynaMechs simulation code.
34 #include <dmEnvironment.hpp>
35 #include <dmMobileBaseLink.hpp>
36 #include <dmContactModel.hpp>
37 #include <dmIntegRK4.hpp>
38
39 #include <glLoadModels.h>
40
41 dmGLMouse *mouse;
42 dmGLPolarCamera_zup *camera;
43 GLfloat view_mat[4][4];
44
45 Float idt;
46 Float sim_time=0.0;
47 Float rtime=0.0;
48 bool paused_flag = true;
49
50 dmArticulation *G_robot;
51 dmIntegRK4 *G_integrator;
52
53 dmTimespec tv, last_tv;
54
55 int render_rate;
56 int render_count = 0;
57 int timer_count = 0;
58 int motion_plan_rate; // fixed rate of 100Hz
59 int motion_plan_count = 0; // counter for motion planning updates
60
61 float cmd_direction = 0.0;
62 float cmd_speed = 0.0;
63
64 // light_position is NOT default value
65 GLfloat light_position[] = { 1.0, -1.0, 1.0, 0.0 };
66
67 //----------------------------------------------------------------------------
68 // Summary: Initialize material property and light source.
69 // Parameters:
70 // Returns:
71 //----------------------------------------------------------------------------
myinit(void)72 void myinit (void)
73 {
74 GLfloat light_ambient[] = { 0.0, 0.0, 0.0, 1.0 };
75 GLfloat light_diffuse[] = { 1.0, 1.0, 1.0, 1.0 };
76 GLfloat light_specular[] = { 1.0, 1.0, 1.0, 1.0 };
77
78 glLightfv (GL_LIGHT0, GL_AMBIENT, light_ambient);
79 glLightfv (GL_LIGHT0, GL_DIFFUSE, light_diffuse);
80 glLightfv (GL_LIGHT0, GL_SPECULAR, light_specular);
81 glLightfv (GL_LIGHT0, GL_POSITION, light_position);
82
83 glEnable (GL_LIGHTING);
84 glEnable (GL_LIGHT0);
85 glDepthFunc(GL_LESS);
86 glEnable(GL_DEPTH_TEST);
87
88 glShadeModel(GL_FLAT);
89 glEnable(GL_CULL_FACE);
90 glCullFace(GL_BACK);
91 }
92 //----------------------------------------------------------------------------
93 // Summary:
94 // Parameters:
95 // Returns:
96 //----------------------------------------------------------------------------
display(void)97 void display (void)
98 {
99 glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
100
101 glMatrixMode (GL_MODELVIEW);
102 glPushMatrix ();
103
104 // ===============================================================
105 (dmEnvironment::getEnvironment())->draw();
106
107 glPushAttrib(GL_ALL_ATTRIB_BITS);
108 G_robot->draw();
109 glPopAttrib();
110 // ===============================================================
111
112 glDisable (GL_LIGHTING);
113
114 glBegin(GL_LINES);
115 glColor3f(1.0, 0.0, 0.0);
116 glVertex3f(2.0, 0.0, 0.0);
117 glVertex3f(0.0, 0.0, 0.0);
118 glEnd();
119
120 glBegin(GL_LINES);
121 glColor3f(0.0, 1.0, 0.0);
122 glVertex3f(0.0, 2.0, 0.0);
123 glVertex3f(0.0, 0.0, 0.0);
124 glEnd();
125
126 glBegin(GL_LINES);
127 glColor3f(0.0, 0.0, 1.0);
128 glVertex3f(0.0, 0.0, 2.0);
129 glVertex3f(0.0, 0.0, 0.0);
130 glEnd();
131
132 glEnable (GL_LIGHTING);
133
134 glPopMatrix ();
135
136 glFlush ();
137 glutSwapBuffers();
138 }
139
140 //----------------------------------------------------------------------------
141 // Summary:
142 // Parameters:
143 // Returns:
144 //----------------------------------------------------------------------------
myReshape(int w,int h)145 void myReshape(int w, int h)
146 {
147 glViewport (0, 0, w, h);
148 mouse->win_size_x = w;
149 mouse->win_size_y = h;
150
151 //if (w <= h)
152 // glOrtho (-2.5, 2.5, -2.5*(GLfloat)h/(GLfloat)w,
153 // 2.5*(GLfloat)h/(GLfloat)w, -10.0, 10.0);
154 //else
155 // glOrtho (-2.5*(GLfloat)w/(GLfloat)h,
156 // 2.5*(GLfloat)w/(GLfloat)h, -2.5, 2.5, -10.0, 10.0);
157
158 camera->setPerspective(45.0, (GLfloat)w/(GLfloat)h, 1.0, 200.0);
159
160 //glMatrixMode (GL_MODELVIEW);
161 //glLoadIdentity();
162 //glTranslatef(0,0,-10.0);
163
164 camera->setViewMat(view_mat);
165 camera->applyView();
166 }
167
168 //----------------------------------------------------------------------------
169 // Summary:
170 // Parameters:
171 // Returns:
172 //----------------------------------------------------------------------------
processKeyboard(unsigned char key,int,int)173 void processKeyboard(unsigned char key, int, int)
174 {
175 switch (key)
176 {
177 case 27:
178 glutDestroyWindow(glutGetWindow());
179 exit(1);
180 break;
181
182 case 'p':
183 paused_flag = !paused_flag;
184 break;
185 }
186 }
187
188
189 //----------------------------------------------------------------------------
190 // Summary:
191 // Parameters:
192 // Returns:
193 //----------------------------------------------------------------------------
processSpecialKeys(int key,int,int)194 void processSpecialKeys(int key, int, int)
195 {
196 switch (key)
197 {
198 case GLUT_KEY_LEFT:
199 cmd_direction += 5.0;
200 if (cmd_direction > 180.0) cmd_direction -= 360.0;
201 break;
202 case GLUT_KEY_RIGHT:
203 cmd_direction -= 5.0;
204 if (cmd_direction < -180.0) cmd_direction += 360.0;
205 break;
206 case GLUT_KEY_UP:
207 cmd_speed += 0.01f;
208 if (cmd_speed > 0.25f) cmd_speed = 0.25f;
209 break;
210 case GLUT_KEY_DOWN:
211 cmd_speed -= 0.01f;
212 if (cmd_speed < 0.0) cmd_speed = 0.0;
213 break;
214 }
215 }
216
217
218 //----------------------------------------------------------------------------
219 // Summary:
220 // Parameters:
221 // Returns:
222 //----------------------------------------------------------------------------
updateSim()223 void updateSim()
224 {
225
226 if (!paused_flag)
227 {
228 for (int i=0; i<render_rate; i++)
229 {
230 //computeControl(sim_time);
231 G_integrator->simulate(idt);
232 sim_time += idt;
233 }
234 }
235
236 camera->update(mouse);
237 camera->applyView();
238
239 glLightfv (GL_LIGHT0, GL_POSITION, light_position);
240
241 display();
242
243 // compute render rate
244 timer_count++;
245 dmGetSysTime(&tv);
246 double elapsed_time = ((double) tv.tv_sec - last_tv.tv_sec) +
247 (1.0e-9*((double) tv.tv_nsec - last_tv.tv_nsec));
248
249 if (elapsed_time > 2.5)
250 {
251 rtime += elapsed_time;
252 cerr << "time/real_time: " << sim_time << '/' << rtime
253 << " frame_rate: " << (double) timer_count/elapsed_time << endl;
254
255 timer_count = 0;
256 last_tv.tv_sec = tv.tv_sec;
257 last_tv.tv_nsec = tv.tv_nsec;
258 }
259 }
260
261 //----------------------------------------------------------------------------
262 // Summary:
263 // Parameters:
264 // Returns:
265 //----------------------------------------------------------------------------
main(int argc,char ** argv)266 int main(int argc, char** argv)
267 {
268 int i, j;
269
270 glutInit(&argc, argv);
271
272 glutInitWindowSize(700, 512);
273 glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
274 glutCreateWindow("Simple DynaMechs Example");
275
276 myinit();
277 mouse = dmGLMouse::dmInitGLMouse();
278
279 for (i=0; i<4; i++)
280 {
281 for (j=0; j<4; j++)
282 {
283 view_mat[i][j] = 0.0;
284 }
285 view_mat[i][i] = 1.0;
286 }
287 view_mat[3][2] = -10.0;
288 camera = new dmGLPolarCamera_zup();
289 camera->setRadius(30.0);
290 camera->setCOI(10.0, 10.0, 5.0);
291 camera->setTranslationScale(0.02f);
292
293 // Initialize simulation timing information.
294 idt = 0.00500f;
295 render_rate = 20;
296
297 // ===========================================================================
298 // Initialize DynaMechs environment - must occur before any linkage systems
299 CartesianVector gravity = {0.0, 0.0, (Float)-9.81};
300 dmEnvironment *environment = new dmEnvironment();
301 dmEnvironment::setEnvironment(environment);
302 environment->setGravity(gravity);
303 environment->loadTerrainData("../models/terrain_sm.dat");
304 environment->setGroundPlanarSpringConstant((Float)5500.);
305 environment->setGroundNormalSpringConstant((Float)7500.);
306 environment->setGroundPlanarDamperConstant((Float)50.0);
307 environment->setGroundNormalDamperConstant((Float)50.0);
308 environment->setFrictionCoeffs((Float)0.4, (Float)0.15); // make it slick
309 environment->drawInit();
310
311 // ===========================================================================
312 // build a simple DynaMechs system - a single rigid body
313 G_robot = new dmArticulation;
314 dmMobileBaseLink *ref = new dmMobileBaseLink;
315
316 GLuint *dlist = new GLuint;
317 *dlist = dmGLLoadFile_xan("../models/obj_cube_center.xan");
318 ref->setUserData(dlist);
319
320 CartesianTensor I_bar = {{(Float)0.7, 0.0, 0.0},
321 {0.0, (Float)0.7, 0.0},
322 {0.0, 0.0, (Float)0.7}};
323 CartesianVector cg_pos = {0.,0.,0.};
324 ref->setInertiaParameters(0.5, I_bar, cg_pos);
325
326 CartesianVector contact_pos[8] = {{-1.0, 1.0, -1.0},
327 { 1.0, 1.0, -1.0},
328 {-1.0, -1.0, -1.0},
329 { 1.0, -1.0, -1.0},
330 {-1.0, 1.0, 1.0},
331 { 1.0, 1.0, 1.0},
332 {-1.0, -1.0, 1.0},
333 { 1.0, -1.0, 1.0}};
334
335 dmContactModel *contact_model = new dmContactModel;
336 contact_model->setContactPoints(4, contact_pos);
337 ref->addForce(contact_model);
338
339 contact_model = new dmContactModel;
340 contact_model->setContactPoints(4, &contact_pos[4]);
341 ref->addForce(contact_model);
342
343 Float q[7] = {0., 0., 0., 1.0, // quat
344 17., 17., 15.}; // pos
345 SpatialVector vel = {-10.0, 0., 0.0, 0., 0., 0.};
346 ref->setState(q, vel);
347
348 G_robot->addLink(ref, NULL);
349
350 // Setup the integrator
351 G_integrator = new dmIntegRK4();
352 G_integrator->addSystem(G_robot);
353
354 // ===========================================================================
355
356 //initControl(G_robot);
357
358 glutReshapeFunc(myReshape);
359 glutKeyboardFunc(processKeyboard);
360 glutSpecialFunc(processSpecialKeys);
361 glutDisplayFunc(display);
362 glutIdleFunc(updateSim);
363
364 dmGetSysTime(&last_tv);
365
366 cout << endl;
367 cout << "p - toggles dynamic simulation" << endl;
368 cout << "Use mouse to rotate/move/zoom the camera" << endl << endl;
369
370 glutMainLoop();
371 return 0; /* ANSI C requires main to return int. */
372 }
373