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: gldraw.cpp
21 * Author: Scott McMillan
22 * Created: 24 March 1997
23 * Summary:
24 *****************************************************************************/
25
26 #include <dm.h>
27 #include <dmArticulation.hpp>
28 #include <dmLink.hpp>
29 #include <dmZScrewTxLink.hpp>
30 #include <dmStaticRootLink.hpp>
31 #include <dmMobileBaseLink.hpp>
32 #include <dmSphericalLink.hpp>
33 #include <dmQuaternionLink.hpp>
34 #include <dmMDHLink.hpp>
35 #include <dmRevoluteLink.hpp>
36 #include <dmPrismaticLink.hpp>
37 #include <dmContactModel.hpp>
38 #include <dmEnvironment.hpp>
39 #include <dmTreadmill.hpp>
40
41 #include <GL/gl.h>
42
43 // yeah I know this is in other places but I really want to limit its scope
44 const float RADTODEG = (float)(180.0/M_PI); // M_PI is defined in math.h
45
46 //----------------------------------------------------------------------------
compute_face_normal(float v0[3],float v1[3],float v2[3],float normal[3])47 inline void compute_face_normal(float v0[3], float v1[3], float v2[3],
48 float normal[3])
49 {
50 float a[3], b[3];
51 register int i;
52
53 for (i=0; i<3; i++)
54 {
55 a[i] = v1[i] - v0[i];
56 b[i] = v2[i] - v0[i];
57 }
58
59 //cross(a, b, normal);
60 normal[0] = a[1]*b[2] - a[2]*b[1];
61 normal[1] = a[2]*b[0] - a[0]*b[2];
62 normal[2] = a[0]*b[1] - a[1]*b[0];
63
64 //normalize(normal);
65 float norm = sqrt(normal[0]*normal[0] +
66 normal[1]*normal[1] +
67 normal[2]*normal[2]);
68
69 if (norm > 0.0)
70 {
71 normal[0] /= norm;
72 normal[1] /= norm;
73 normal[2] /= norm;
74 }
75 }
76
77 //============================================================================
78 // Initialize DynaMechs/OpenGL drawing routines
79 //============================================================================
80
81 //----------------------------------------------------------------------------
drawInit()82 void dmEnvironment::drawInit()
83 {
84 register int i, j;
85
86 GLfloat vertex[3][3], normal[3];
87
88 // read in and allocate depth data
89
90 m_terrain_model_index = glGenLists(1);
91
92 if (m_terrain_model_index == 0)
93 {
94 cerr << "loadModel_grid: Error unable to allocate dlist index." << endl;
95 }
96
97 glNewList(m_terrain_model_index, GL_COMPILE);
98 {
99 glPolygonMode(GL_FRONT, GL_LINE); //FILL);
100 glPolygonMode(GL_BACK, GL_LINE);
101
102 GLfloat color[4] = {0.5,0.5,1.0,1.0};
103 glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, color);
104
105 for (j=0; j<m_y_dim-1; j++)
106 {
107 glBegin(GL_TRIANGLE_STRIP);
108 {
109 for (i=0; i<m_x_dim; i++)
110 {
111 vertex[0][0] = ((GLfloat) i)*m_grid_resolution;
112 vertex[0][1] = ((GLfloat) j + 1.0)*m_grid_resolution;
113 vertex[0][2] = m_depth[i][j+1];
114
115 if (i > 0)
116 {
117 vertex[1][0] = ((GLfloat) i - 1.0)*m_grid_resolution;
118 vertex[1][1] = ((GLfloat) j + 1.0)*m_grid_resolution;
119 vertex[1][2] = m_depth[i-1][j+1];
120
121 vertex[2][0] = ((GLfloat) i - 1.0)*m_grid_resolution;
122 vertex[2][1] = ((GLfloat) j)*m_grid_resolution;
123 vertex[2][2] = m_depth[i-1][j];
124
125 compute_face_normal(vertex[1], vertex[2], vertex[0], normal);
126 glNormal3fv(normal);
127 }
128 glVertex3fv(vertex[0]);
129
130 vertex[1][0] = ((GLfloat) i)*m_grid_resolution;
131 vertex[1][1] = ((GLfloat) j)*m_grid_resolution;
132 vertex[1][2] = m_depth[i][j];
133
134 if (i > 0)
135 {
136 compute_face_normal(vertex[1], vertex[0], vertex[2], normal);
137 glNormal3fv(normal);
138 }
139 glVertex3fv(vertex[1]);
140 }
141 }
142 glEnd();
143 }
144 }
145 glEndList();
146 }
147
148 //============================================================================
149 // draw environment and robot
150 //============================================================================
151
152 //----------------------------------------------------------------------------
draw() const153 void dmArticulation::draw() const
154 {
155 glPushMatrix();
156
157 glTranslatef(m_p_ICS[0], m_p_ICS[1], m_p_ICS[2]);
158
159 float len = sqrt(m_quat_ICS[0]*m_quat_ICS[0] +
160 m_quat_ICS[1]*m_quat_ICS[1] +
161 m_quat_ICS[2]*m_quat_ICS[2]);
162 if (len > 1.0e-6)
163 {
164 float angle = 2.0*atan2(len, m_quat_ICS[3]);
165 glRotatef(angle*RADTODEG,
166 m_quat_ICS[0]/len, m_quat_ICS[1]/len, m_quat_ICS[2]/len);
167 }
168
169 // render some sort of base element
170 if (getUserData())
171 glCallList(*((GLuint *) getUserData()));
172
173 for (unsigned int j=0; j<m_link_list.size(); j++)
174 {
175 if (m_link_list[j]->parent == NULL)
176 {
177 glPushMatrix();
178
179 // draw base link
180 m_link_list[j]->link->draw();
181
182 // recurse through the children
183 for (unsigned int i=0; i<m_link_list[j]->child_list.size(); i++)
184 {
185 glPushMatrix();
186 drawTraversal(m_link_list[j]->child_list[i]);
187 glPopMatrix();
188 }
189
190 glPopMatrix();
191 }
192 }
193
194 glPopMatrix();
195 }
196
197 //----------------------------------------------------------------------------
drawTraversal(LinkInfoStruct * node) const198 void dmArticulation::drawTraversal(LinkInfoStruct *node) const
199 {
200 if (node && node->parent)
201 {
202 node->link->draw();
203
204 for (unsigned int i=0; i<node->child_list.size(); i++)
205 {
206 if (node->child_list.size() > 1)
207 {
208 glPushMatrix();
209 drawTraversal(node->child_list[i]);
210 glPopMatrix();
211 }
212 else
213 {
214 drawTraversal(node->child_list[i]);
215 }
216 }
217 }
218 }
219
220
221 //----------------------------------------------------------------------------
draw() const222 void dmZScrewTxLink::draw() const
223 {
224 glTranslatef(0.0, 0.0, m_dMDH);
225 glRotatef(m_thetaMDH*RADTODEG, 0.0, 0.0, 1.0);
226 }
227
228 //----------------------------------------------------------------------------
draw() const229 void dmStaticRootLink::draw() const
230 {
231 glCallList(*((GLuint *) getUserData()));
232 }
233
234 //----------------------------------------------------------------------------
draw() const235 void dmRevoluteLink::draw() const
236 {
237 // set static portion of the MDH transformation.
238 if (m_alphaMDH != 0.0)
239 {
240 glRotatef(m_alphaMDH*RADTODEG, 1.0, 0.0, 0.0);
241 }
242
243 if ((m_aMDH != 0.0) || (m_dMDH != 0.0))
244 {
245 glTranslatef(m_aMDH, 0.0, m_dMDH);
246 }
247
248 // set dynamic z-axis transformation.
249 glRotatef(m_thetaMDH*RADTODEG, 0.0, 0.0, 1.0);
250
251 glCallList(*((GLuint *) getUserData()));
252 }
253
254 //----------------------------------------------------------------------------
draw() const255 void dmPrismaticLink::draw() const
256 {
257 // set static portion of the MDH transformation.
258 if (m_alphaMDH != 0.0)
259 {
260 glRotatef(m_alphaMDH*RADTODEG, 1.0, 0.0, 0.0);
261 }
262
263 if ((m_aMDH != 0.0) || (m_dMDH != 0.0))
264 {
265 glTranslatef(m_aMDH, 0.0, m_dMDH);
266 }
267
268 if (m_thetaMDH != 0.0)
269 {
270 glRotatef(m_thetaMDH*RADTODEG, 0.0, 0.0, 1.0);
271 }
272
273 glCallList(*((GLuint *) getUserData()));
274 }
275
276 //----------------------------------------------------------------------------
draw() const277 void dmSphericalLink::draw() const
278 {
279 glTranslatef(m_p[0], m_p[1], m_p[2]);
280 glRotatef(m_q[2]*RADTODEG, 0.0, 0.0, 1.0);
281 glRotatef(m_q[1]*RADTODEG, 0.0, 1.0, 0.0);
282 glRotatef(m_q[0]*RADTODEG, 1.0, 0.0, 0.0);
283
284 glCallList(*((GLuint *) getUserData()));
285 }
286
287 //----------------------------------------------------------------------------
draw() const288 void dmQuaternionLink::draw() const
289 {
290 glTranslatef(m_p[0], m_p[1], m_p[2]);
291
292 float len = sqrt(m_q[0]*m_q[0] + m_q[1]*m_q[1] + m_q[2]*m_q[2]);
293 if (len > 1.0e-6)
294 {
295 float angle = 2.0*atan2(len, m_q[3]);
296 glRotatef(angle*RADTODEG, m_q[0]/len, m_q[1]/len, m_q[2]/len);
297 }
298
299 glCallList(*((GLuint *) getUserData()));
300 }
301
302 //----------------------------------------------------------------------------
draw() const303 void dmMobileBaseLink::draw() const
304 {
305 glTranslatef(m_p[0], m_p[1], m_p[2]);
306
307 float len = sqrt(m_quat[0]*m_quat[0] +
308 m_quat[1]*m_quat[1] +
309 m_quat[2]*m_quat[2]);
310 if (len > 1.0e-6)
311 {
312 float angle = 2.0*atan2(len, m_quat[3]);
313 glRotatef(angle*RADTODEG, m_quat[0]/len, m_quat[1]/len, m_quat[2]/len);
314 }
315
316 glCallList(*((GLuint *) getUserData()));
317 }
318
319 //----------------------------------------------------------------------------
draw() const320 void dmContactModel::draw() const
321 {
322 }
323
324 //----------------------------------------------------------------------------
draw() const325 void dmEnvironment::draw() const
326 {
327 glCallList(m_terrain_model_index);
328 }
329
330 //----------------------------------------------------------------------------
draw() const331 void dmTreadmill::draw() const
332 {
333 dmEnvironment::draw();
334
335 glBegin(GL_QUAD_STRIP);
336 float dxl = m_half_width*m_left[0];
337 float dyl = m_half_width*m_left[1];
338 float dzl = m_half_width*m_left[2];
339
340 float dxf = m_half_length*m_forward[0];
341 float dyf = m_half_length*m_forward[1];
342 float dzf = m_half_length*m_forward[2];
343
344 glVertex3f(m_position[0] + dxl - dxf,
345 m_position[1] + dyl - dyf,
346 m_position[2] + dzl - dzf);
347 glVertex3f(m_position[0] - dxl - dxf,
348 m_position[1] - dyl - dyf,
349 m_position[2] - dzl - dzf);
350
351 float step = 2.0f*m_half_length/10.f;
352 float offset = m_q - ((int)(m_q/step))*step;
353 if (offset < 0) offset += step;
354
355 for (unsigned int ix = 0; ix < 10; ++ix)
356 {
357 float distance = offset + ix*step - m_half_length;
358 float dx = distance*m_forward[0];
359 float dy = distance*m_forward[1];
360 float dz = distance*m_forward[2];
361 glVertex3f(m_position[0] + dxl + dx,
362 m_position[1] + dyl + dy,
363 m_position[2] + dzl + dz);
364 glVertex3f(m_position[0] - dxl + dx,
365 m_position[1] - dyl + dy,
366 m_position[2] - dzl + dz);
367 }
368
369 glVertex3f(m_position[0] + dxl + dxf,
370 m_position[1] + dyl + dyf,
371 m_position[2] + dzl + dzf);
372 glVertex3f(m_position[0] - dxl + dxf,
373 m_position[1] - dyl + dyf,
374 m_position[2] - dzl + dzf);
375 glEnd();
376 }
377