WorldSim - 1.2.1

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
renderwobjecthierarchy.cpp
1 /********************************************************************************
2  * WorldSim -- library for robot simulations *
3  * Copyright (C) 2008-2011 Gianluca Massera <emmegian@yahoo.it> *
4  * *
5  * This program is free software; you can redistribute it and/or modify *
6  * it under the terms of the GNU General Public License as published by *
7  * the Free Software Foundation; either version 2 of the License, or *
8  * (at your option) any later version. *
9  * *
10  * This program is distributed in the hope that it will be useful, *
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13  * GNU General Public License for more details. *
14  * *
15  * You should have received a copy of the GNU General Public License *
16  * along with this program; if not, write to the Free Software *
17  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA *
18  ********************************************************************************/
19 
20 #include "renderworld.h"
21 #include "physphere.h"
22 #include "phybox.h"
23 #include "phycylinder.h"
24 #include "phycone.h"
25 // #include "phyheightfield.h"
26 #include "phyicub.h"
27 #include "wcamera.h"
28 #include "phycompoundobject.h"
29 #include "wmesh.h"
30 #include "graphicalwobject.h"
31 #include "private/renderwobjecthierarchy.h"
32 #include "Newton.h"
33 
34 #include <QImage>
35 #include <QColor>
36 #include <QList>
37 #include <QVector>
38 #include <cmath>
39 
40 // These instructions are needed because QT 4.8 no longer depends on glu, so we
41 // have to include it here explicitly
42 #ifdef FARSA_MAC
43 # include <GLUT/glut.h>
44 #else
45 # include <GL/glu.h>
46 #endif
47 
48 using namespace qglviewer;
49 
50 #define GLMultMatrix glMultMatrixf
51 // for double glMultMatrixd
52 
53 namespace farsa {
54 
59 class RenderGenericObject : public RenderWObject {
60 public:
61  RenderGenericObject( WObject* wobj, RenderWObjectContainer* container ) : RenderWObject( wobj, container ) {
62  rad = 0.02f;
63  };
64  virtual ~RenderGenericObject() { };
65  virtual void render( QGLContext* /*gw*/ ) {
66  };
67  virtual void renderAABB( RenderWorld* gw ) {
68  wVector minpoint, maxpoint;
69  calculateAABB( minpoint, maxpoint, obj->matrix() );
70  gw->drawWireBox( minpoint, maxpoint );
71  };
72  virtual void calculateAABB( wVector& minPoint, wVector& maxPoint, const wMatrix tm ) {
73  wVector rds( rad, rad, rad );
74  minPoint = tm.w_pos - rds;
75  maxPoint = tm.w_pos + rds;
76  };
77 
78  virtual void calculateOBB( wVector& dimension, wVector& minPoint, wVector& maxPoint ) {
79  dimension[0] = rad*2.0;
80  dimension[1] = rad*2.0;
81  dimension[2] = rad*2.0;
82  wVector rds = wVector( rad, rad, rad );
83  minPoint = -rds;
84  maxPoint = +rds;
85  };
86 protected:
87  real rad;
88 };
89 
90 class RenderWMesh : public RenderWObject {
91 public:
92  RenderWMesh( WObject* wobj, RenderWObjectContainer* container ) : RenderWObject( wobj, container ) {
93  wmesh = (WMesh*)wobj;
94  };
95  virtual ~RenderWMesh() { };
96  virtual void render( QGLContext* gw ) {
97  glPushMatrix();
98  wMatrix m = wmesh->matrix();
99  if ( wmesh->attachedTo() ) {
100  m = m * wmesh->attachedTo()->matrix();
101  }
102  GLMultMatrix(&m[0][0]);
103  int m_numMeshes = wmesh->meshesCount();
104  WMesh::Mesh *m_pMeshes = wmesh->meshes();
105 // int m_numMaterials = wmesh->materialsCount();
106  WMesh::Material *m_pMaterials = wmesh->materials();
107 // int m_numTriangles = wmesh->trianglesCount();
108  WMesh::Triangle *m_pTriangles = wmesh->triangles();
109 // int m_numVertices = wmesh->verticesCount();
110  WMesh::Vertex *m_pVertices = wmesh->vertices();
111  for ( int i = 0; i < m_numMeshes; i++ ) {
112  int materialIndex = m_pMeshes[i].m_materialIndex;
113  if ( materialIndex >= 0 ) {
114  glMaterialfv( GL_FRONT, GL_AMBIENT, m_pMaterials[materialIndex].m_ambient );
115  glMaterialfv( GL_FRONT, GL_DIFFUSE, m_pMaterials[materialIndex].m_diffuse );
116  glMaterialfv( GL_FRONT, GL_SPECULAR, m_pMaterials[materialIndex].m_specular );
117  glMaterialfv( GL_FRONT, GL_EMISSION, m_pMaterials[materialIndex].m_emissive );
118  glMaterialf( GL_FRONT, GL_SHININESS, m_pMaterials[materialIndex].m_shininess );
119  //container()->applyTexture( gw, wmesh->texture() );
120  container()->setupColorTexture( gw, this );
121  }
122  glBegin( GL_TRIANGLES );
123  for ( int j = 0; j < m_pMeshes[i].m_numTriangles; j++ ) {
124  int triangleIndex = m_pMeshes[i].m_pTriangleIndices[j];
125  const WMesh::Triangle* pTri = &m_pTriangles[triangleIndex];
126  for ( int k = 0; k < 3; k++ ) {
127  int index = pTri->m_vertexIndices[k];
128  glNormal3fv( pTri->m_vertexNormals[k] );
129  glTexCoord2f( pTri->m_s[k], pTri->m_t[k] );
130  glVertex3fv( m_pVertices[index].m_location );
131  }
132  }
133  glEnd();
134  }
135  glPopMatrix();
136  };
137  virtual void renderAABB( RenderWorld* /*gw*/ ) {
138  };
139  virtual void calculateAABB( wVector& /*minPoint*/, wVector& /*maxPoint*/, const wMatrix /*tm*/ ) {
140  };
141  virtual void calculateOBB( wVector& /*dimension*/, wVector& /*minPoint*/, wVector& /*maxPoint*/ ) {
142  };
143 private:
144  WMesh* wmesh;
145 };
146 
147 class RenderPhyBox : public RenderWObject {
148 public:
149  RenderPhyBox( WObject* wobj, RenderWObjectContainer* container ) : RenderWObject( wobj, container ) {
150  box = (PhyBox*)wobj;
151  dx = box->sideX();
152  dy = box->sideY();
153  dz = box->sideZ();
154  };
155  virtual ~RenderPhyBox() { };
156  virtual void render( QGLContext* gw ) {
157  glPushMatrix();
158  container()->setupColorTexture( gw, this );
159  const wMatrix& m = box->matrix();
160  GLMultMatrix(&m[0][0]);
161 
162  // the cube will just be drawn as six quads for the sake of simplicity
163  // for each face, we specify the quad's normal (for lighting), then
164  // specify the quad's 4 vertices and associated texture coordinates
165  glBegin(GL_QUADS);
166  float hdx = (dx/2.0);
167  float hdy = (dy/2.0);
168  float hdz = (dz/2.0);
169  // front
170  glNormal3f(0.0, 0.0, 1.0);
171  glTexCoord2f(0.0, 0.0); glVertex3f(-hdx, -hdy, hdz);
172  glTexCoord2f(1.0, 0.0); glVertex3f( hdx, -hdy, hdz);
173  glTexCoord2f(1.0, 1.0); glVertex3f( hdx, hdy, hdz);
174  glTexCoord2f(0.0, 1.0); glVertex3f(-hdx, hdy, hdz);
175 
176  // back
177  glNormal3f(0.0, 0.0, -1.0);
178  glTexCoord2f(0.0, 0.0); glVertex3f( hdx, -hdy, -hdz);
179  glTexCoord2f(1.0, 0.0); glVertex3f(-hdx, -hdy, -hdz);
180  glTexCoord2f(1.0, 1.0); glVertex3f(-hdx, hdy, -hdz);
181  glTexCoord2f(0.0, 1.0); glVertex3f( hdx, hdy, -hdz);
182 
183  // top
184  glNormal3f(0.0, 1.0, 0.0);
185  glTexCoord2f(0.0, 0.0); glVertex3f(-hdx, hdy, hdz);
186  glTexCoord2f(1.0, 0.0); glVertex3f( hdx, hdy, hdz);
187  glTexCoord2f(1.0, 1.0); glVertex3f( hdx, hdy, -hdz);
188  glTexCoord2f(0.0, 1.0); glVertex3f(-hdx, hdy, -hdz);
189 
190  // bottom
191  glNormal3f(0.0, -1.0, 0.0);
192  glTexCoord2f(0.0, 0.0); glVertex3f(-hdx, -hdy, -hdz);
193  glTexCoord2f(1.0, 0.0); glVertex3f( hdx, -hdy, -hdz);
194  glTexCoord2f(1.0, 1.0); glVertex3f( hdx, -hdy, hdz);
195  glTexCoord2f(0.0, 1.0); glVertex3f(-hdx, -hdy, hdz);
196 
197  // left
198  glNormal3f(-1.0, 0.0, 0.0);
199  glTexCoord2f(0.0, 0.0); glVertex3f(-hdx, -hdy, -hdz);
200  glTexCoord2f(1.0, 0.0); glVertex3f(-hdx, -hdy, hdz);
201  glTexCoord2f(1.0, 1.0); glVertex3f(-hdx, hdy, hdz);
202  glTexCoord2f(0.0, 1.0); glVertex3f(-hdx, hdy, -hdz);
203 
204  // right
205  glNormal3f(1.0, 0.0, 0.0);
206  glTexCoord2f(0.0, 0.0); glVertex3f(hdx, -hdy, hdz);
207  glTexCoord2f(1.0, 0.0); glVertex3f(hdx, -hdy, -hdz);
208  glTexCoord2f(1.0, 1.0); glVertex3f(hdx, hdy, -hdz);
209  glTexCoord2f(0.0, 1.0); glVertex3f(hdx, hdy, hdz);
210 
211  glEnd();
212  glPopMatrix();
213  };
214  virtual void renderAABB( RenderWorld* gw ) {
215  wVector minpoint, maxpoint;
216  calculateAABB( minpoint, maxpoint, obj->matrix() );
217  gw->drawWireBox( minpoint, maxpoint );
218  };
219  virtual void calculateAABB( wVector& minPoint, wVector& maxPoint, const wMatrix tm ) {
220  real tdx = fabs(tm.x_ax[0]*dx) + fabs(tm.y_ax[0]*dy) + fabs(tm.z_ax[0]*dz);
221  real tdy = fabs(tm.x_ax[1]*dx) + fabs(tm.y_ax[1]*dy) + fabs(tm.z_ax[1]*dz);
222  real tdz = fabs(tm.x_ax[2]*dx) + fabs(tm.y_ax[2]*dy) + fabs(tm.z_ax[2]*dz);
223 
224  wVector hds( tdx/2.0, tdy/2.0, tdz/2.0 );
225  minPoint = tm.w_pos - hds;
226  maxPoint = tm.w_pos + hds;
227  };
228 
229  virtual void calculateOBB( wVector& dimension, wVector& minPoint, wVector& maxPoint ) {
230  dimension[0] = dx;
231  dimension[1] = dy;
232  dimension[2] = dz;
233  wVector hds( dx/2.0, dy/2.0, dz/2.0 );
234  minPoint = -hds;
235  maxPoint = +hds;
236  };
237 
238 protected:
239  PhyBox* box;
240  real dx, dy, dz;
241 };
242 
243 class RenderPhySphere : public RenderWObject {
244 public:
245  RenderPhySphere( WObject* wobj, RenderWObjectContainer* container ) : RenderWObject( wobj, container ) {
246  sph = (PhySphere*)wobj;
247  rad = sph->radius();
248  };
249  virtual ~RenderPhySphere() { };
250  virtual void render( QGLContext* gw ) {
251  glPushMatrix();
252  GLUquadricObj *pObj;
253  container()->setupColorTexture( gw, this );
254  wMatrix mat = sph->matrix();
255  mat.x_ax = mat.x_ax.scale( rad );
256  mat.y_ax = mat.y_ax.scale( rad );
257  mat.z_ax = mat.z_ax.scale( rad );
258  GLMultMatrix(&mat[0][0]);
259 
260  // Get a new Quadric off the stack
261  pObj = gluNewQuadric();
262  // Get a new Quadric off the stack
263  gluQuadricTexture(pObj, true);
264  gluSphere(pObj, 1.0f, 20, 20);
265  gluDeleteQuadric(pObj);
266  glPopMatrix();
267  };
268  virtual void renderAABB( RenderWorld* gw ) {
269  wVector minpoint, maxpoint;
270  calculateAABB( minpoint, maxpoint, obj->matrix() );
271  gw->drawWireBox( minpoint, maxpoint );
272  };
273  virtual void calculateAABB( wVector& minPoint, wVector& maxPoint, const wMatrix tm ) {
274  wVector rds( rad, rad, rad );
275  minPoint = tm.w_pos - rds;
276  maxPoint = tm.w_pos + rds;
277  };
278 
279  virtual void calculateOBB( wVector& dimension, wVector& minPoint, wVector& maxPoint ) {
280  dimension[0] = rad*2.0;
281  dimension[1] = rad*2.0;
282  dimension[2] = rad*2.0;
283  wVector rds = wVector( rad, rad, rad );
284  minPoint = -rds;
285  maxPoint = +rds;
286  };
287 protected:
288  PhySphere* sph;
289  real rad;
290 };
291 
292 class RenderPhyCylinder : public RenderWObject {
293 public:
294  RenderPhyCylinder( WObject* wobj, RenderWObjectContainer* container ) : RenderWObject( wobj, container ) {
295  cil = (PhyCylinder*)wobj;
296  rad = cil->radius();
297  hei = cil->height();
298  };
299  virtual ~RenderPhyCylinder() { };
300  virtual void render( QGLContext* gw ) {
301  glPushMatrix();
302  GLUquadricObj *pObj;
303  container()->setupColorTexture( gw, this );
304 
305  // opengl cylinder are aligned alogn the z axis, we want it along the x axis,
306  // we create a rotation matrix to do the alignment
307  wMatrix matrix = wMatrix::yaw( PI_GRECO * 0.5f );
308  matrix.w_pos = matrix.rotateVector( wVector(0.0, 0.0, -hei*0.5f) );
309  matrix = matrix * cil->matrix();
310  GLMultMatrix(&matrix[0][0]);
311 
312  // Get a new Quadric off the stack
313  pObj = gluNewQuadric();
314  gluQuadricTexture(pObj, true);
315  gluCylinder(pObj, rad, rad, hei, 20, 2);
316 
317  // render the caps
318  gluQuadricOrientation(pObj, GLU_INSIDE);
319  gluDisk(pObj, 0.0f, rad, 20, 1);
320 
321  glTranslatef (0.0f, 0.0f, hei);
322  gluQuadricOrientation(pObj, GLU_OUTSIDE);
323  gluDisk(pObj, 0.0f, rad, 20, 1);
324 
325  gluDeleteQuadric(pObj);
326  glPopMatrix();
327  };
328  virtual void renderAABB( RenderWorld* gw ) {
329  wVector minpoint, maxpoint;
330  calculateAABB( minpoint, maxpoint, obj->matrix() );
331  gw->drawWireBox( minpoint, maxpoint );
332  };
333  virtual void calculateAABB( wVector& minPoint, wVector& maxPoint, const wMatrix tm ) {
334  real h2 = hei/2.0;
335  real tdx = fabs(tm.x_ax[0]*h2) + fabs(tm.y_ax[0]*rad) + fabs(tm.z_ax[0]*rad);
336  real tdy = fabs(tm.x_ax[1]*h2) + fabs(tm.y_ax[1]*rad) + fabs(tm.z_ax[1]*rad);
337  real tdz = fabs(tm.x_ax[2]*h2) + fabs(tm.y_ax[2]*rad) + fabs(tm.z_ax[2]*rad);
338  wVector hds( tdx, tdy, tdz );
339  minPoint = tm.w_pos - hds;
340  maxPoint = tm.w_pos + hds;
341  };
342 
343  virtual void calculateOBB( wVector& dimension, wVector& minPoint, wVector& maxPoint ) {
344  dimension[0] = hei;
345  dimension[1] = rad*2.0;
346  dimension[2] = rad*2.0;
347  wVector rds = wVector( hei/2.0, rad, rad );
348  minPoint = -rds;
349  maxPoint = +rds;
350  };
351 protected:
352  PhyCylinder* cil;
353  real rad;
354  real hei;
355 };
356 
357 class RenderPhyCone : public RenderWObject {
358 public:
359  RenderPhyCone( WObject* wobj, RenderWObjectContainer* container ) : RenderWObject( wobj, container ) {
360  cone = (PhyCone*)wobj;
361  rad = cone->radius();
362  hei = cone->height();
363  };
364  virtual ~RenderPhyCone() { };
365  virtual void render( QGLContext* gw ) {
366  glPushMatrix();
367  GLUquadricObj *pObj;
368  container()->setupColorTexture( gw, this );
369 
370  // opengl cylinder are aligned alogn the z axis, we want it along the x axis,
371  // we create a rotation matrix to do the alignment
372  wMatrix matrix = wMatrix::yaw( PI_GRECO * 0.5 );
373  matrix.w_pos = matrix.rotateVector( wVector(0.0, 0.0, -hei*0.5) );
374  matrix = matrix * cone->matrix();
375  GLMultMatrix(&matrix[0][0]);
376 
377  // Get a new Quadric off the stack
378  pObj = gluNewQuadric();
379  gluQuadricTexture(pObj, true);
380  gluCylinder(pObj, rad, 0, hei, 20, 2);
381 
382  // render the caps
383  gluQuadricOrientation(pObj, GLU_INSIDE);
384  gluDisk(pObj, 0.0f, rad, 20, 1);
385 
386  gluDeleteQuadric(pObj);
387  glPopMatrix();
388  };
389  virtual void renderAABB( RenderWorld* gw ) {
390  wVector minpoint, maxpoint;
391  calculateAABB( minpoint, maxpoint, obj->matrix() );
392  gw->drawWireBox( minpoint, maxpoint );
393  };
394  virtual void calculateAABB( wVector& minPoint, wVector& maxPoint, const wMatrix tm ) {
395  real h2 = hei/2.0;
396  real tdx = fabs(tm.x_ax[0]*h2) + fabs(tm.y_ax[0]*rad) + fabs(tm.z_ax[0]*rad);
397  real tdy = fabs(tm.x_ax[1]*h2) + fabs(tm.y_ax[1]*rad) + fabs(tm.z_ax[1]*rad);
398  real tdz = fabs(tm.x_ax[2]*h2) + fabs(tm.y_ax[2]*rad) + fabs(tm.z_ax[2]*rad);
399  wVector hds( tdx, tdy, tdz );
400  minPoint = tm.w_pos - hds;
401  maxPoint = tm.w_pos + hds;
402  };
403 
404  virtual void calculateOBB( wVector& dimension, wVector& minPoint, wVector& maxPoint ) {
405  dimension[0] = hei;
406  dimension[1] = rad*2.0;
407  dimension[2] = rad*2.0;
408  wVector rds = wVector( hei/2.0, rad, rad );
409  minPoint = -rds;
410  maxPoint = +rds;
411  };
412 protected:
413  PhyCone* cone;
414  real rad;
415  real hei;
416 };
417 
418 // class RenderPhyHeightField : public RenderWObject {
419 // public:
420 // RenderPhyHeightField( WObject* wobj, RenderWObjectContainer* container ) : RenderWObject( wobj, container ) {
421 // hf = (PhyHeightField*)wobj;
422 // dx = hf->sideX();
423 // dy = hf->sideY();
424 // dz = 0.01f;
425 // };
426 // virtual ~RenderPhyHeightField() { };
427 // virtual void render( QGLContext* gw ) {
428 // glPushMatrix();
429 // container()->setupColorTexture( gw, this );
430 // wMatrix m = hf->matrix().rotateAround( wVector(1,0,0), wVector(0,0,0), toRad(-90) );
431 // m.w_pos[0] -= dx/2.0;
432 // m.w_pos[1] -= dy/2.0;
433 // GLMultMatrix(&m[0][0]);
434 //
435 // NewtonMesh* mesh = hf->mesh;
436 // int stride = NewtonMeshGetVertexStrideInByte(mesh) / sizeof (double);
437 // const double* const vertexList = NewtonMeshGetVertexArray(mesh);
438 //
439 // glBegin(GL_TRIANGLES);
440 // for (void* face = NewtonMeshGetFirstFace(mesh); face; face = NewtonMeshGetNextFace(mesh, face)) {
441 // if (!NewtonMeshIsFaceOpen (mesh, face)) {
442 // int indices[1024];
443 // int vertexCount = NewtonMeshGetFaceIndexCount (mesh, face);
444 // Q_ASSERT (vertexCount < sizeof (indices)/sizeof (indices[0]));
445 // NewtonMeshGetFaceIndices (mesh, face, indices);
446 //
447 // //dFloat64 normal[4];
448 // //NewtonMeshCalculateFaceNormal (mesh, face, normal);
449 // //glNormal3f(normal[0], normal[1], normal[2]);
450 //
451 // int i0 = indices[0];
452 // int i1 = indices[1];
453 // wVector p0 ( dFloat(vertexList[i0 * stride + 0]),
454 // dFloat(vertexList[i0 * stride + 1]),
455 // dFloat(vertexList[i0 * stride + 2]),
456 // 0.0f );
457 // wVector p1 ( dFloat(vertexList[i1 * stride + 0]),
458 // dFloat(vertexList[i1 * stride + 1]),
459 // dFloat(vertexList[i1 * stride + 2]),
460 // 0.0f );
461 // for (int i = 2; i < vertexCount; i++) {
462 // int i2 = indices[i];
463 // wVector p2 ( dFloat(vertexList[i2 * stride + 0]),
464 // dFloat(vertexList[i2 * stride + 1]),
465 // dFloat(vertexList[i2 * stride + 2]),
466 // 0.0f );
467 // glVertex3f(p0[0], p0[1], p0[2]);
468 // glVertex3f(p1[0], p1[1], p1[2]);
469 // glVertex3f(p2[0], p2[1], p2[2]);
470 // p1 = p2;
471 // }
472 // }
473 // }
474 //
475 // glEnd();
476 //
477 // glPopMatrix();
478 // };
479 // virtual void renderAABB( RenderWorld* gw ) {
480 // wVector minpoint, maxpoint;
481 // calculateAABB( minpoint, maxpoint, obj->matrix() );
482 // gw->drawWireBox( minpoint, maxpoint );
483 // };
484 // virtual void calculateAABB( wVector& minPoint, wVector& maxPoint, const wMatrix tm ) {
485 // real tdx = fabs(tm.x_ax[0]*dx) + fabs(tm.y_ax[0]*dy) + fabs(tm.z_ax[0]*dz);
486 // real tdy = fabs(tm.x_ax[1]*dx) + fabs(tm.y_ax[1]*dy) + fabs(tm.z_ax[1]*dz);
487 // real tdz = fabs(tm.x_ax[2]*dx) + fabs(tm.y_ax[2]*dy) + fabs(tm.z_ax[2]*dz);
488 //
489 // wVector hds( tdx/2.0, tdy/2.0, tdz/2.0 );
490 // minPoint = tm.w_pos - hds;
491 // maxPoint = tm.w_pos + hds;
492 // };
493 //
494 // virtual void calculateOBB( wVector& dimension, wVector& minPoint, wVector& maxPoint ) {
495 // dimension[0] = dx;
496 // dimension[1] = dy;
497 // dimension[2] = dz;
498 // wVector hds( dx/2.0, dy/2.0, dz/2.0 );
499 // minPoint = -hds;
500 // maxPoint = +hds;
501 // };
502 //
503 // protected:
504 // PhyHeightField* hf;
505 // real dx, dy, dz;
506 // };
507 
508 #ifdef FARSA_USE_YARP_AND_ICUB
509 
510 RenderICub::RenderICub( WObject* wobj, RenderWObjectContainer* container ) :
511  RenderWObject( wobj, container )
512 {
513  icub = (PhyiCub*)wobj;
514  icub->forceKinematicChainsUpdate(true);
515  initRobjv(container);
516 }
517 
518 RenderICub::~RenderICub()
519 {
520  if (icub != NULL) {
521  icub->forceKinematicChainsUpdate(false);
522  }
523 }
524 
525 void RenderICub::render( QGLContext* gw )
526 {
527  // Nothing to do here, all components are rendered by their respective renderers
528 }
529 
530 void RenderICub::renderAABB( RenderWorld* gw )
531 {
532  wVector minpoint, maxpoint;
533  calculateAABB( minpoint, maxpoint, obj->matrix() );
534  gw->drawWireBox( minpoint, maxpoint );
535 }
536 
537 void RenderICub::calculateAABB( wVector& minPoint, wVector& maxPoint, const wMatrix tm )
538 {
539  UNUSED_PARAM( tm );
540  QList<RenderWObject*> robjs = robjv.values();
541  robjs[0]->calculateAABB( minPoint, maxPoint, robjs[0]->object()->matrix() );
542  wVector minP, maxP;
543  for( int i=1; i<robjs.size(); i++ ) {
544  robjs[i]->calculateAABB( minP, maxP, robjs[i]->object()->matrix() );
545  mergeAABB( minPoint, maxPoint, minP, maxP );
546  }
547 }
548 
549 void RenderICub::calculateOBB( wVector& dimension, wVector& minPoint, wVector& maxPoint )
550 {
551  /* ***
552  wVector minPoint, maxPoint;
553  wMatrix tm = wMatrix::identity();
554  for( int i=1; i<robjv.size(); i++ ) {
555  robjv[i]->calculateAABB( minP, maxP, tm );
556  mergeAABB( minPoint, maxPoint, minP, maxP );
557  }
558  *** */
559  calculateAABB( minPoint, maxPoint, wMatrix::identity() );
560  dimension[0] = fabs( maxPoint[0] - minPoint[0] );
561  dimension[1] = fabs( maxPoint[1] - minPoint[1] );
562  dimension[2] = fabs( maxPoint[2] - minPoint[2] );
563 }
564 
565 void RenderICub::objectAlreadyDestroyed()
566 {
567  icub = NULL;
568 }
569 
570 void RenderICub::initRobjv( RenderWObjectContainer* container )
571 {
572  robjv.clear();
573  foreach( RenderWObject* wobj, container->graphics() ) {
574  if ( wobj->object()->owner() == icub ) {
575  robjv[wobj->object()] = wobj;
576  }
577  }
578  foreach( WObject* obj, icub->leftLeg() ) {
579  obj->setUseColorTextureOfOwner(true);
580  }
581  foreach( WObject* obj, icub->rightLeg() ) {
582  obj->setUseColorTextureOfOwner(true);
583  }
584  foreach( WObject* obj, icub->torso() ) {
585  obj->setUseColorTextureOfOwner(true);
586  }
587  foreach( WObject* obj, icub->leftArm() ) {
588  obj->setUseColorTextureOfOwner(true);
589  }
590  foreach( WObject* obj, icub->rightArm() ) {
591  obj->setUseColorTextureOfOwner(true);
592  }
593  int i = 0;
594  foreach( WObject* obj, icub->headNeck() ) {
595  //--- the eye will be displayed with the texture of the eye
596  if ( ( i == 4 ) || ( i == 5 ) ) {
597  obj->setUseColorTextureOfOwner(false);
598  } else {
599  obj->setUseColorTextureOfOwner(true);
600  }
601  i++;
602  }
603  foreach( WObject* obj, icub->covers() ) {
604  obj->setUseColorTextureOfOwner(false);
605  }
606 }
607 
608 void RenderICub::mergeAABB( wVector& minPointA, wVector& maxPointA, const wVector& minPointB, const wVector& maxPointB )
609 {
610  minPointA[0] = min( minPointA[0], minPointB[0] );
611  minPointA[1] = min( minPointA[1], minPointB[1] );
612  minPointA[2] = min( minPointA[2], minPointB[2] );
613  maxPointA[0] = max( maxPointA[0], maxPointB[0] );
614  maxPointA[1] = max( maxPointA[1], maxPointB[1] );
615  maxPointA[2] = max( maxPointA[2], maxPointB[2] );
616 }
617 
618 #endif
619 
620 class RenderCompoundObject : public RenderWObject {
621 public:
622  RenderCompoundObject( WObject* wobj, RenderWObjectContainer* container ) : RenderWObject( wobj, container ) {
623  co = (PhyCompoundObject*)wobj;
624  initRobjv( container );
625  };
626  virtual ~RenderCompoundObject() {
627  foreach( RenderWObject* ro, robjv ) {
628  delete ro;
629  }
630  };
631  virtual void render( QGLContext* gw ) {
632  glPushMatrix();
633  container()->setupColorTexture( gw, this );
634  wMatrix matrix = co->matrix();
635  GLMultMatrix(&matrix[0][0]);
636  foreach( RenderWObject* ro, robjv ) {
637  ro->render( gw );
638  }
639  glPopMatrix();
640  };
641  virtual void renderAABB( RenderWorld* gw ) {
642  wVector minpoint, maxpoint;
643  calculateAABB( minpoint, maxpoint, obj->matrix() );
644  gw->drawWireBox( minpoint, maxpoint );
645  };
646  virtual void calculateAABB( wVector& minPoint, wVector& maxPoint, const wMatrix tm ) {
647  robjv[0]->calculateAABB( minPoint, maxPoint, robjv[0]->object()->matrix()*tm );
648  wVector minP, maxP;
649  for( int i=1; i<robjv.size(); i++ ) {
650  robjv[i]->calculateAABB( minP, maxP, robjv[i]->object()->matrix()*tm );
651  mergeAABB( minPoint, maxPoint, minP, maxP );
652  }
653  };
654  virtual void calculateOBB( wVector& dimension, wVector& minPoint, wVector& maxPoint ) {
655  //--- the right calculation is done calculating the AABB when
656  //--- the transformation matrix is the identity
657  wVector minP, maxP;
658  if ( robjv.size() == 0 ) {
659  qWarning( "== this point should never reached: renderwobjecthierarchy.cpp:435" );
660  return;
661  }
662  robjv[0]->calculateAABB( minPoint, maxPoint, robjv[0]->object()->matrix() );
663  for( int i=1; i<robjv.size(); i++ ) {
664  robjv[i]->calculateAABB( minP, maxP, robjv[i]->object()->matrix() );
665  mergeAABB( minPoint, maxPoint, minP, maxP );
666  }
667  dimension[0] = fabs( maxPoint[0] - minPoint[0] );
668  dimension[1] = fabs( maxPoint[1] - minPoint[1] );
669  dimension[2] = fabs( maxPoint[2] - minPoint[2] );
670  };
671 protected:
672  PhyCompoundObject* co;
673  QVector<RenderWObject*> robjv;
674 private:
675  void initRobjv( RenderWObjectContainer* container ) {
676  foreach( WObject* obj, co->bodies() ) {
677  robjv << RenderWObjectContainer::createRenderWObjectFor( obj, container );
678  }
679  };
680  void mergeAABB( wVector& minPointA, wVector& maxPointA, const wVector& minPointB, const wVector& maxPointB ) {
681  minPointA[0] = min( minPointA[0], minPointB[0] );
682  minPointA[1] = min( minPointA[1], minPointB[1] );
683  minPointA[2] = min( minPointA[2], minPointB[2] );
684  maxPointA[0] = max( maxPointA[0], maxPointB[0] );
685  maxPointA[1] = max( maxPointA[1], maxPointB[1] );
686  maxPointA[2] = max( maxPointA[2], maxPointB[2] );
687  };
688 };
689 
690 #ifdef FARSA_USE_YARP_AND_ICUB
691 
692 class RenderWCamera : public RenderWObject {
693 public:
694  RenderWCamera( WObject* wobj, RenderWObjectContainer* container ) : RenderWObject( wobj, container ) {
695  wcam = (WCamera*)(wobj);
696  dx = 0.015f;
697  dy = 0.01f;
698  dz = 0.025f;
699  };
700  virtual ~RenderWCamera() { };
701  virtual void render( QGLContext* gw ) {
702  if ( wcam->attachedTo() != NULL ) return;
703  glPushMatrix();
704  container()->setupColorTexture( gw, this );
705  const wMatrix& m = wcam->matrix();
706  GLMultMatrix(&m[0][0]);
707  // the cube will just be drawn as six quads for the sake of simplicity
708  // for each face, we specify the quad's normal (for lighting), then
709  // specify the quad's 4 vertices and associated texture coordinates
710  glBegin(GL_QUADS);
711  float hdx = (dx/2.0);
712  float hdy = (dy/2.0);
713  float hdz = (dz/2.0);
714  // front
715  glNormal3f(0.0, 0.0, 1.0);
716  glTexCoord2f(0.0, 0.0); glVertex3f(-hdx, -hdy, hdz);
717  glTexCoord2f(1.0, 0.0); glVertex3f( hdx, -hdy, hdz);
718  glTexCoord2f(1.0, 1.0); glVertex3f( hdx, hdy, hdz);
719  glTexCoord2f(0.0, 1.0); glVertex3f(-hdx, hdy, hdz);
720  // back
721  glNormal3f(0.0, 0.0, -1.0);
722  glTexCoord2f(0.0, 0.0); glVertex3f( hdx, -hdy, -hdz);
723  glTexCoord2f(1.0, 0.0); glVertex3f(-hdx, -hdy, -hdz);
724  glTexCoord2f(1.0, 1.0); glVertex3f(-hdx, hdy, -hdz);
725  glTexCoord2f(0.0, 1.0); glVertex3f( hdx, hdy, -hdz);
726  // top
727  glNormal3f(0.0, 1.0, 0.0);
728  glTexCoord2f(0.0, 0.0); glVertex3f(-hdx, hdy, hdz);
729  glTexCoord2f(1.0, 0.0); glVertex3f( hdx, hdy, hdz);
730  glTexCoord2f(1.0, 1.0); glVertex3f( hdx, hdy, -hdz);
731  glTexCoord2f(0.0, 1.0); glVertex3f(-hdx, hdy, -hdz);
732  // bottom
733  glNormal3f(0.0, -1.0, 0.0);
734  glTexCoord2f(0.0, 0.0); glVertex3f(-hdx, -hdy, -hdz);
735  glTexCoord2f(1.0, 0.0); glVertex3f( hdx, -hdy, -hdz);
736  glTexCoord2f(1.0, 1.0); glVertex3f( hdx, -hdy, hdz);
737  glTexCoord2f(0.0, 1.0); glVertex3f(-hdx, -hdy, hdz);
738  // left
739  glNormal3f(-1.0, 0.0, 0.0);
740  glTexCoord2f(0.0, 0.0); glVertex3f(-hdx, -hdy, -hdz);
741  glTexCoord2f(1.0, 0.0); glVertex3f(-hdx, -hdy, hdz);
742  glTexCoord2f(1.0, 1.0); glVertex3f(-hdx, hdy, hdz);
743  glTexCoord2f(0.0, 1.0); glVertex3f(-hdx, hdy, -hdz);
744  // right
745  glNormal3f(1.0, 0.0, 0.0);
746  glTexCoord2f(0.0, 0.0); glVertex3f(hdx, -hdy, hdz);
747  glTexCoord2f(1.0, 0.0); glVertex3f(hdx, -hdy, -hdz);
748  glTexCoord2f(1.0, 1.0); glVertex3f(hdx, hdy, -hdz);
749  glTexCoord2f(0.0, 1.0); glVertex3f(hdx, hdy, hdz);
750  glEnd();
751 
752  //--- some fancy stuff for understanding the orientation of camera
753  container()->drawCylinder( wVector(0,1,0), wVector(-hdx-0.006f,0,hdz-0.006f), 0.004f, 0.008f );
754  container()->drawCylinder( wVector(0,1,0), wVector(-hdx,0,-hdz), 0.004f, 0.008f );
755  container()->drawCylinder( wVector(0,0,1), wVector(0,0.0,hdz+0.003f), 0.006f, 0.004f );
756  glPopMatrix();
757  };
758  virtual void renderAABB( RenderWorld* gw ) {
759  wVector minpoint, maxpoint;
760  calculateAABB( minpoint, maxpoint, obj->matrix() );
761  gw->drawWireBox( minpoint, maxpoint );
762  };
763  virtual void calculateAABB( wVector& minPoint, wVector& maxPoint, const wMatrix tm ) {
764  real tdx = fabs(tm.x_ax[0]*dx) + fabs(tm.y_ax[0]*dy) + fabs(tm.z_ax[0]*dz);
765  real tdy = fabs(tm.x_ax[1]*dx) + fabs(tm.y_ax[1]*dy) + fabs(tm.z_ax[1]*dz);
766  real tdz = fabs(tm.x_ax[2]*dx) + fabs(tm.y_ax[2]*dy) + fabs(tm.z_ax[2]*dz);
767  wVector hds( tdx/2.0, tdy/2.0, tdz/2.0 );
768  minPoint = tm.w_pos - hds;
769  maxPoint = tm.w_pos + hds;
770  };
771  virtual void calculateOBB( wVector& dimension, wVector& minPoint, wVector& maxPoint ) {
772  dimension[0] = dx;
773  dimension[1] = dy;
774  dimension[2] = dz;
775  wVector hds( dx/2.0, dy/2.0, dz/2.0 );
776  minPoint = -hds;
777  maxPoint = +hds;
778  };
779 protected:
780  WCamera* wcam;
781  real dx, dy, dz;
782 };
783 
784 #endif // FARSA_USE_YARP_AND_ICUB
785 
786 // This class is the renderer associated with GraphicalWObject. It simply
787 // forwards calls to GraphicalWObject functions
788 class GraphicalWObjectRenderer : public RenderWObject
789 {
790 public:
791  GraphicalWObjectRenderer(WObject* obj, RenderWObjectContainer* rw) :
792  RenderWObject(obj, rw),
793  m_graphicalWObject(dynamic_cast<GraphicalWObject*>(obj))
794  {
795  }
796 
797  virtual ~GraphicalWObjectRenderer()
798  {
799  }
800 
801  virtual void render(QGLContext *gw)
802  {
803  m_graphicalWObject->updateAndRender(this, gw);
804  }
805 
806  virtual void renderAABB(RenderWorld *gw)
807  {
808  m_graphicalWObject->updateAndRenderAABB(this, gw);
809  }
810 
811  virtual void calculateAABB(wVector &minPoint, wVector &maxPoint, const wMatrix tm)
812  {
813  m_graphicalWObject->updateAndCalculateAABB(minPoint, maxPoint, tm);
814  }
815 
816  virtual void calculateOBB(wVector &dimension, wVector &minPoint, wVector &maxPoint)
817  {
818  m_graphicalWObject->updateAndCalculateOBB(dimension, minPoint, maxPoint);
819  }
820 
821 private:
822  GraphicalWObject *const m_graphicalWObject;
823 };
824 
825 bool RenderWObjectContainer::facInited = false;
826 QMap<QString, WAbstractCreator*>* RenderWObjectContainer::fac;
827 void RenderWObjectContainer::initFactory() {
828  if ( facInited ) return;
829 
830  fac = new QMap<QString, WAbstractCreator*>();
831 
832  (*fac)["farsa::WMesh"] = new WCreator<RenderWMesh>();
833  (*fac)["farsa::PhyBox"] = new WCreator<RenderPhyBox>();
834  (*fac)["farsa::PhySphere"] = new WCreator<RenderPhySphere>();
835  (*fac)["farsa::PhyCylinder"] = new WCreator<RenderPhyCylinder>();
836  (*fac)["farsa::PhyCone"] = new WCreator<RenderPhyCone>();
837 // (*fac)["farsa::PhyHeightField"] = new WCreator<RenderPhyHeightField>();
838  (*fac)["farsa::PhyCompoundObject"] = new WCreator<RenderCompoundObject>();
839 #ifdef FARSA_USE_YARP_AND_ICUB
840  (*fac)["farsa::WCamera"] = new WCreator<RenderWCamera>();
841  (*fac)[PhyiCub::staticMetaObject.className()] = new WCreator<RenderICub>();
842 #endif
843  (*fac)["farsa::GraphicalWObject"] = new WCreator<GraphicalWObjectRenderer>();
844 
845  //--- FIXME a specific class should be implemented
846  (*fac)["farsa::WObject"] = new WCreator<RenderGenericObject>();
847  (*fac)["farsa::WorldController"] = new WCreator<RenderGenericObject>();
848 
849  facInited = true;
850 }
851 
852 } // end namespace farsa