WorldSim - 1.1.0

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