WorldSim - 1.2.0

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
renderworld.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 "phyjoint.h"
22 
23 #include <QImage>
24 #include <QColor>
25 #include <QKeyEvent>
26 #include <QMenu>
27 #include <QAction>
28 #include <QList>
29 #include <cmath>
30 #include <QDir>
31 #include <QLinkedList>
32 #include <QMutexLocker>
33 #include <QEvent>
34 
35 // These instructions are needed because QT 4.8 no longer depends on glu, so we
36 // have to include it here explicitly
37 #ifdef FARSA_MAC
38 # include <GLUT/glut.h>
39 #else
40 # include <GL/glu.h>
41 #endif
42 
43 using namespace qglviewer;
44 
45 #define GLMultMatrix glMultMatrixf
46 // for double use #define GLMultMatrix glMultMatrixd
47 
48 #include "qglviewer/camera.h"
49 #if QT_VERSION >= 0x040000
50 # include <QWheelEvent>
51 #endif
52 
53 namespace farsa {
54 
55 QMap<QString, QImage>* RenderWObjectContainer::textmap = NULL;
56 unsigned int RenderWObjectContainer::textmapRefCounter = 0;
57 
58 RenderWObjectContainer::RenderWObjectContainer( QString wResName ) :
59  worldResourceName(wResName),
60  worldv(NULL),
61  mutex(QMutex::Recursive)
62 {
63  if ( textmap == NULL ) {
64  textmap = new QMap<QString, QImage>();
65  (*textmap)["tile1"].load( ":/tiles/16tile10.jpg" );
66  (*textmap)["tile2"].load( ":/tiles/16tile07.jpg" );
67  (*textmap)["white"].load( ":/white.jpg" );
68  (*textmap)["tile3"].load( ":/tiles/16tile11.jpg" );
69  (*textmap)["tile4"].load( ":/tiles/16tile-B.jpg" );
70  (*textmap)["tile5"].load( ":/tiles/16tile12.jpg" );
71  (*textmap)["tile6"].load( ":/tiles/16tile04.jpg" );
72  (*textmap)["tile7"].load( ":/tiles/tile01.jpg" );
73  (*textmap)["tile8"].load( ":/tiles/16tile02.jpg" );
74  (*textmap)["tile9"].load( ":/tiles/16tile05.jpg" );
75  (*textmap)["tile10"].load( ":/tiles/16tile08.jpg" );
76  (*textmap)["icub"].load( ":/tiles/16tile11.jpg" ); //.load( ":/metal/iron05.jpg" );
77  (*textmap)["icubFace"].load( ":/covers/face.jpg" );
78  (*textmap)["blueye"].load( ":/covers/eyep2_b.jpg" );
79  (*textmap)["metal"].load( ":/metal/iron05.jpg" );
80  (*textmap)["marXbot_12leds"].load( ":/covers/marxbot_12leds.jpg" );
81  //--- The order of the texture is:
82  // 0 => TOP
83  // 1 => BACK
84  // 2 => FRONT
85  // 3 => BOTTOM
86  // 4 => RIGHT
87  // 5 => LEFT
88  /* skyb[0].load( ":/skybox/sb_top.jpg" );
89  skyb[1].load( ":/skybox/sb_back.jpg" );
90  skyb[2].load( ":/skybox/sb_front.jpg" );
91  skyb[3].load( ":/skybox/sb_bottom.jpg" );
92  skyb[4].load( ":/skybox/sb_right.jpg" );
93  skyb[5].load( ":/skybox/sb_left.jpg" );*/
94  (*textmap)["skyb0"].load( ":/skybox/sb2_top.jpg" );
95  (*textmap)["skyb1"].load( ":/skybox/sb2_back.jpg" );
96  (*textmap)["skyb2"].load( ":/skybox/sb2_front.jpg" );
97  (*textmap)["skyb3"].load( ":/ground/cobbles01.jpg" );
98  (*textmap)["skyb4"].load( ":/skybox/sb2_right.jpg" );
99  (*textmap)["skyb5"].load( ":/skybox/sb2_left.jpg" );
100  }
101  textmapRefCounter++;
102 
103  usableResources(QStringList() << worldResourceName);
104 }
105 
106 RenderWObjectContainer::~RenderWObjectContainer() {
107  foreach( RenderWObject* ro, graphs ) {
108  delete ro;
109  }
110  textmapRefCounter--;
111  if (textmapRefCounter == 0) {
112  delete textmap;
113  textmap = NULL;
114  }
115 }
116 
117 bool RenderWObjectContainer::addTextureImage( QString filename, QString texturename ) {
118  QMutexLocker locker(&mutex);
119 
120  return ((*textmap)[texturename].load( filename ));
121 }
122 
123 void RenderWObjectContainer::setWorld( World* newworld ) {
124  // Here we simply declare a resource, the resourceChanged handler will take care of setting things up
125  declareResource( worldResourceName, newworld );
126 }
127 
128 RenderWObject* RenderWObjectContainer::operator[]( const WObject* obj ) {
129  QMutexLocker locker(&mutex);
130 
131  //--- not efficient !! OPTIMIZE ME
132  for( int i=0; i<graphs.size(); i++ ) {
133  if ( graphs[i]->object() == obj ) {
134  return graphs[i];
135  }
136  }
137  return NULL;
138 }
139 
140 RenderWObject* RenderWObjectContainer::createRenderWObjectFor( const WObject* obj, RenderWObjectContainer* container ) {
141  initFactory();
142  const QMetaObject* metaObj = obj->metaObject();
143  while( metaObj ) {
144  QString classname = metaObj->className();
145  if (fac->contains(classname)) {
146  return (*fac)[classname]->create( (WObject*)obj, container );
147  }
148  metaObj = metaObj->superClass();
149  }
150 
151  // Cannot find a specific renderer, using the generic one (which doesn't render anything...
152  // at least we save a crash)
153  return (*fac)["farsa::WObject"]->create( (WObject*)obj, container );
154 }
155 
156 void RenderWObjectContainer::addObject( WObject* wobj ) {
157  // Always take the resource mutex first and then our mutex to avoid deadlocks
158  ResourcesLocker resourceLocker(this); // Here because perhaps the RenderWObject need to access the world
159  QMutexLocker locker(&mutex);
160 
161  graphs.append( createRenderWObjectFor( wobj, this ) );
162 }
163 
164 void RenderWObjectContainer::removeObject( WObject* wobj ) {
165  // Always take the resource mutex first and then our mutex to avoid deadlocks
166  ResourcesLocker resourceLocker(this); // Here because perhaps the RenderWObject need to access the world
167  QMutexLocker locker(&mutex);
168 
169  for( int i=0; i<graphs.size(); i++ ) {
170  if ( graphs[i]->object() == wobj ) {
171  RenderWObject* ro = graphs[i];
172  graphs.remove( i );
173  ro->objectAlreadyDestroyed();
174  delete ro;
175  break;
176  }
177  }
178 }
179 
180 void RenderWObjectContainer::resourceChanged(QString name, ResourceChangeType changeType)
181 {
182  QMutexLocker locker(&mutex);
183 
184  // We ignore all resources except world
185  if (name != worldResourceName) {
186  return;
187  }
188 
189  // Removing all objects, they refer to the old world
190  foreach( RenderWObject* ro, graphs ) {
191  delete ro;
192  }
193  graphs.clear();
194 
195  // If world was deleted, we simply set the pointer to NULL, otherwise we set the pointer
196  // to the new world
197  if (changeType == Deleted) {
198  worldv = NULL;
199  } else {
200  worldv = getResource<World>();
201  }
202 
203  // Creating renderes for all objects
204  if (worldv != NULL) {
205  foreach( WObject* obj, worldv->objects() ) {
206  graphs.append( createRenderWObjectFor( obj, this ) );
207  }
208  }
209 }
210 
211 void RenderWObjectContainer::applyTexture( QGLContext* gw, QString texts ) {
212  QMutexLocker locker(&mutex);
213 
214  if ( textmap->contains( texts ) ) {
215  if ( !textGLId.contains( texts ) ) {
216  textGLId[texts] = gw->bindTexture( (*textmap)[texts], GL_TEXTURE_2D, GL_RGB );
217  }
218  glBindTexture( GL_TEXTURE_2D, textGLId[texts] );
219  glEnable( GL_TEXTURE_2D );
220  }/* else {
221  glDisable( GL_TEXTURE_2D );
222  }*/
223 }
224 
225 void RenderWObjectContainer::setupColorTexture( QGLContext* gw, RenderWObject* robj ) {
226  // Always take the resource mutex first and then our mutex to avoid deadlocks
227  ResourcesLocker resourceLocker(this);
228  QMutexLocker locker(&mutex);
229 
230  // set the color
231  glShadeModel( GL_SMOOTH );
232  glEnable( GL_BLEND );
233  glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );
234  WObject* obj = robj->object();
235  while ( obj->useColorTextureOfOwner() && ( obj->owner() != NULL ) ) {
236  WObject *const owner = dynamic_cast<WObject *>(obj->owner());
237  if ( owner != NULL ) {
238  obj = owner;
239  } else {
240  break;
241  }
242  }
243  QColor colorv = obj->color();
244  glColor4f( colorv.redF(), colorv.greenF(), colorv.blueF(), colorv.alphaF() );
245  applyTexture( gw, obj->texture() );
246 }
247 
248 void RenderWObjectContainer::drawSkyGroundBox( QGLContext* gw ) {
249  // Always take the resource mutex first and then our mutex to avoid deadlocks
250  ResourcesLocker resourceLocker(this);
251  QMutexLocker locker(&mutex);
252 
253  if (worldv == NULL) {
254  return;
255  }
256  wVector min, max;
257  worldv->size( min, max );
258  wVector m_size = wVector( fabs(max[0]-min[0]), fabs(max[1]-min[1]), fabs(max[1]-min[1]) );
259 
260  resourceLocker.unlock(); // We don't need it anymore
261 
262  glDisable( GL_LIGHTING );
263  glShadeModel( GL_FLAT );
264  glColor4f( 1.0f, 1.0f, 1.0f, 1.0f );
265  //glEnable(GL_TEXTURE_2D);
266  //glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
267  //glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
268  //glTexEnvf (GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_DECAL);
269  // the cube will just be drawn as six quads for the sake of simplicity
270  // for each face, we specify the quad's normal (for lighting), then
271  // specify the quad's 4 vertices's and associated texture coordinates
272  // TOP
273  applyTexture( gw, "skyb0" );
274  glBegin(GL_QUADS);
275  glTexCoord2f(0.0, 1.0); glVertex3f( min.x, max.y, max.z);
276  glTexCoord2f(0.0, 0.0); glVertex3f( max.x, max.y, max.z);
277  glTexCoord2f(1.0, 0.0); glVertex3f( max.x, min.y, max.z);
278  glTexCoord2f(1.0, 1.0); glVertex3f( min.x, min.y, max.z);
279  glEnd();
280  // BACK
281  applyTexture( gw, "skyb1" );
282  glBegin(GL_QUADS);
283  glTexCoord2f(1.0, 0.0); glVertex3f( min.x, max.y, min.z);
284  glTexCoord2f(1.0, 1.0); glVertex3f( min.x, max.y, max.z);
285  glTexCoord2f(0.0, 1.0); glVertex3f( min.x, min.y, max.z);
286  glTexCoord2f(0.0, 0.0); glVertex3f( min.x, min.y, min.z);
287  glEnd();
288  // FRONT
289  applyTexture( gw, "skyb2" );
290  glBegin(GL_QUADS);
291  glTexCoord2f(0.0, 1.0); glVertex3f(max.x, max.y, max.z);
292  glTexCoord2f(0.0, 0.0); glVertex3f(max.x, max.y, min.z);
293  glTexCoord2f(1.0, 0.0); glVertex3f(max.x, min.y, min.z);
294  glTexCoord2f(1.0, 1.0); glVertex3f(max.x, min.y, max.z);
295  glEnd();
296  // BOTTOM
297  applyTexture( gw, "skyb3" );
298  //--- suppose the bottom texture will represent 40x40 cm of ground
299  //--- and calculate repeating accordlying
300  float bfs = m_size[1]/0.4;
301  float bft = m_size[0]/0.4;
302  glBegin(GL_QUADS);
303  glTexCoord2f(0.0, bft); glVertex3f( max.x, max.y, min.z);
304  glTexCoord2f(0.0, 0.0); glVertex3f( min.x, max.y, min.z);
305  glTexCoord2f(bfs, 0.0); glVertex3f( min.x, min.y, min.z);
306  glTexCoord2f(bfs, bft); glVertex3f( max.x, min.y, min.z);
307  glEnd();
308  // RIGHT
309  applyTexture( gw, "skyb4" );
310  glBegin(GL_QUADS);
311  glTexCoord2f(1.0, 1.0); glVertex3f( min.x, min.y, max.z);
312  glTexCoord2f(0.0, 1.0); glVertex3f( max.x, min.y, max.z);
313  glTexCoord2f(0.0, 0.0); glVertex3f( max.x, min.y, min.z);
314  glTexCoord2f(1.0, 0.0); glVertex3f( min.x, min.y, min.z);
315  glEnd();
316  // LEFT
317  applyTexture( gw, "skyb5" );
318  glBegin(GL_QUADS);
319  glTexCoord2f(0.0, 0.0); glVertex3f( min.x, max.y, min.z);
320  glTexCoord2f(1.0, 0.0); glVertex3f( max.x, max.y, min.z);
321  glTexCoord2f(1.0, 1.0); glVertex3f( max.x, max.y, max.z);
322  glTexCoord2f(0.0, 1.0); glVertex3f( min.x, max.y, max.z);
323  glEnd();
324 }
325 
326 class StandardCamera : public qglviewer::Camera {
327 public :
328  StandardCamera() {
329  orthoSize = 1.0;
330  };
331  virtual float zNear() const {
332  return 0.001f;
333  };
334  virtual float zFar() const {
335  return 1000.0f;
336  };
337  void changeOrthoFrustumSize(int delta) {
338  if (delta > 0) {
339  orthoSize *= 1.1f;
340  } else {
341  orthoSize /= 1.1f;
342  }
343  };
344  virtual void getOrthoWidthHeight(GLdouble &halfWidth, GLdouble &halfHeight) const {
345  halfHeight = orthoSize;
346  halfWidth = aspectRatio() * orthoSize;
347  };
348 private :
349  float orthoSize;
350 };
351 
352 class MyCameraConstraint : public qglviewer::Constraint {
353 public:
355  MyCameraConstraint( qglviewer::Vec min, qglviewer::Vec max ) {
356  //--- the pad allow to avoid to reach exactly the bounding-box of the world
357  qglviewer::Vec pad(0.1,0.1,0.1);
358  minP = min + pad;
359  maxP = max - pad;
360  };
361  virtual void constrainTranslation( qglviewer::Vec& t, qglviewer::Frame* const fr ) {
362  //--- Local System
363  const qglviewer::Frame* ls = fr;
364  if ( fr->referenceFrame() != NULL ) {
365  qDebug() << "Using Reference";
366  ls = fr->referenceFrame();
367  }
368  //--- Convert t to world coordinate system
369  qglviewer::Vec tw = ls->inverseTransformOf( t );
370  qglviewer::Vec pos = fr->position();
371  if ( (pos.x + tw.x > maxP.x && t.x > 0) || (pos.x + tw.x < minP.x && t.x < 0) ||
372  (pos.y + tw.y > maxP.y && t.y > 0) || (pos.y + tw.y < minP.y && t.y < 0) ||
373  (pos.z + tw.z > maxP.z && t.z > 0) || (pos.z + tw.z < minP.z && t.z < 0) ) {
374  t.z = 0.0;
375  t.x = 0.0;
376  t.y = 0.0;
377  }
378  };
379  //--- boundings
380  qglviewer::Vec minP;
381  qglviewer::Vec maxP;
382 };
383 
384 namespace {
385  // A custom QEvent to force update of RenderWorldWrapperWidget
386  class ForceRenderWorldUpdateEvent : public QEvent
387  {
388  public:
389  ForceRenderWorldUpdateEvent() :
390  QEvent(QEvent::User)
391  {
392  }
393 
394  virtual ~ForceRenderWorldUpdateEvent()
395  {
396  }
397  };
398 }
399 
400 
401 RenderWorld::RenderWorld( QWidget* parent, QString wResName )
402  : QGLViewer( parent ), RenderWObjectContainer( wResName ) {
403  wiref = false;
404  showskygroundbox = true;
405  showobjs = true;
406  showjoints = false;
407  showaabbs = false;
408  showcontacts = true;
409  showforces = false;
410  currentSelected = -1;
411 
412  setStateFileName( QString() );
413 
414  setCamera( new StandardCamera() );
415 
416  setContextMenuPolicy( Qt::CustomContextMenu );
417 }
418 
419 RenderWorld::~RenderWorld() {
420  if ( ! stateFileName().isNull() ) {
421  saveStateToFile();
422  }
423 }
424 
425 void RenderWorld::slotRemoveObject( WObject* w ) {
426  // removeObject is already thread-safe, no need to lock mutex here
427  removeObject( w );
428 }
429 
430 void RenderWorld::slotAddObject( WObject* w ) {
431  // removeObject is already thread-safe, no need to lock mutex here
432  addObject( w );
433 }
434 
435 void RenderWorld::onWorldResize() {
436  // Always take the resource mutex first and then our mutex to avoid deadlocks
437  ResourcesLocker resourceLocker(this);
438  QMutexLocker locker(&mutex);
439 
440  if (world() == NULL) {
441  return;
442  }
443 
444  MyCameraConstraint* tmp = dynamic_cast<MyCameraConstraint*>( camera()->frame()->constraint() );
445  if ( tmp != NULL ) {
446  wVector min, max;
447  world()->size( min, max );
448  setSceneBoundingBox( qglviewer::Vec( min[0], min[1], min[2] ),
449  qglviewer::Vec( max[0], max[1], max[2] ) );
450  tmp->minP = qglviewer::Vec( min[0], min[1], min[2] );
451  tmp->maxP = qglviewer::Vec( max[0], max[1], max[2] );
452  }
453 }
454 
455 void RenderWorld::wireframe( bool b ) {
456  QMutexLocker locker(&mutex);
457  wiref = b;
458 }
459 
460 void RenderWorld::showSkyGround( bool b ) {
461  QMutexLocker locker(&mutex);
462  showskygroundbox = b;
463 }
464 
465 void RenderWorld::showObjects( bool b ) {
466  QMutexLocker locker(&mutex);
467  showobjs = b;
468 }
469 
470 void RenderWorld::showJoints( bool b ) {
471  QMutexLocker locker(&mutex);
472  showjoints = b;
473 }
474 
475 void RenderWorld::showAABBs( bool b ) {
476  QMutexLocker locker(&mutex);
477  showaabbs = b;
478 }
479 
480 void RenderWorld::showContacts( bool b ) {
481  QMutexLocker locker(&mutex);
482  showcontacts = b;
483 }
484 
485 void RenderWorld::showForces( bool b ) {
486  QMutexLocker locker(&mutex);
487  showforces = b;
488 }
489 
490 void RenderWorld::contextMenu( const QPoint& pos ) {
492  QList<QAction*> acts;
493  QAction* act;
494  if ( showskygroundbox ) {
495  act = new QAction( "Hide Sky-Ground", this );
496  } else {
497  act = new QAction( "Show Sky-Ground", this );
498  }
499  act->setCheckable( true );
500  act->setChecked( showskygroundbox );
501  connect( act, SIGNAL( toggled(bool) ),
502  this, SLOT( showSkyGround(bool) ) );
503  acts.append( act );
504  if ( wiref ) {
505  act = new QAction( "Hide Wireframe", this );
506  } else {
507  act = new QAction( "Show Wireframe", this );
508  }
509  act->setCheckable( true );
510  act->setChecked( wiref );
511  connect( act, SIGNAL( toggled(bool) ),
512  this, SLOT( wireframe(bool) ) );
513  acts.append( act );
514 
515  if ( showobjs ) {
516  act = new QAction( "Hide Objects", this );
517  } else {
518  act = new QAction( "Show Objects", this );
519  }
520  act->setCheckable( true );
521  act->setChecked( showobjs );
522  connect( act, SIGNAL( toggled(bool) ),
523  this, SLOT( showObjects(bool) ) );
524  acts.append( act );
525 
526  if ( showjoints ) {
527  act = new QAction( "Hide Joints", this );
528  } else {
529  act = new QAction( "Show Joints", this );
530  }
531  act->setCheckable( true );
532  act->setChecked( showjoints );
533  connect( act, SIGNAL( toggled(bool) ),
534  this, SLOT( showJoints(bool) ) );
535  acts.append( act );
536 
537  if ( showaabbs ) {
538  act = new QAction( "Hide AABBs", this );
539  } else {
540  act = new QAction( "Show AABBs", this );
541  }
542  act->setCheckable( true );
543  act->setChecked( showaabbs );
544  connect( act, SIGNAL( toggled(bool) ),
545  this, SLOT( showAABBs(bool) ) );
546  acts.append( act );
547 
548  if ( showcontacts ) {
549  act = new QAction( "Hide Contacts", this );
550  } else {
551  act = new QAction( "Show Contacts", this );
552  }
553  act->setCheckable( true );
554  act->setChecked( showcontacts );
555  connect( act, SIGNAL( toggled(bool) ),
556  this, SLOT( showContacts(bool) ) );
557  acts.append( act );
558 
559  if ( showforces ) {
560  act = new QAction( "Hide Forces", this );
561  } else {
562  act = new QAction( "Show Forces", this );
563  }
564  act->setCheckable( true );
565  act->setChecked( showforces );
566  connect( act, SIGNAL( toggled(bool) ),
567  this, SLOT( showForces(bool) ) );
568  acts.append( act );
569 
570  QMenu::exec( acts, pos );
571 }
572 
573 void RenderWorld::init() {
574  // Always take the resource mutex first and then our mutex to avoid deadlocks
575  ResourcesLocker resourceLocker(this);
576  QMutexLocker locker(&mutex);
577 
578  if (world() == NULL) {
579  return;
580  }
581 
582  if ( ! stateFileName().isNull() ) {
583  if ( !restoreStateFromFile() ) {
584  }
585  }
586  wVector min, max;
587  world()->size( min, max );
588  camera()->frame()->setConstraint( new MyCameraConstraint( qglviewer::Vec( min[0], min[1], min[2] ), qglviewer::Vec( max[0], max[1], max[2] ) ) );
589  // Light0 is the default ambient light
590  glEnable(GL_LIGHT0);
591 }
592 
593 void RenderWorld::draw() {
594  // Always take the resource mutex first and then our mutex to avoid deadlocks
595  ResourcesLocker resourceLocker(this);
596  QMutexLocker locker(&mutex);
597 
598  if (world() == NULL) {
599  return;
600  }
601 
602  if ( showskygroundbox ) {
603  glPushAttrib( GL_ALL_ATTRIB_BITS );
604  glPushMatrix();
605  drawSkyGroundBox( (QGLContext*)( QGLContext::currentContext() ) );
606  glPopMatrix();
607  glPopAttrib();
608  }
609 
610  //--- the light follow the movement of the camera
611  float pos[4] = {1.0, 0.5, 1.0, 0.0};
612  // Directionnal light
613  camera()->frame()->getPosition(pos[0], pos[1], pos[2]);
614  glLightfv( GL_LIGHT0, GL_POSITION, pos );
615 
616  //--- Display Contacts - Never in wireframe
617  if ( showcontacts ) {
618  glPushAttrib( GL_ALL_ATTRIB_BITS );
619  //--- wireframe is ignored when drawing Contacts and Joints
620  glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
621  glDisable( GL_LIGHTING );
622  // --- draws Contacts
623  contactMapIterator itera( world()->contacts() );
624  while( itera.hasNext() ) {
625  itera.next();
626  const contactVec& vec = itera.value();
627  for( int i=0; i<vec.size(); i++ ) {
628  drawSphere( vec[i].worldPos, 0.008f );
629  }
630  }
631  glPopAttrib();
632  }
633  //--- Display Joint and Kinematic chains
634  int alpha = 255;
635  if ( showjoints ) {
636  glPushAttrib( GL_ALL_ATTRIB_BITS );
637  //--- wireframe is ignored when drawing Contacts and Joints
638  glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
639  glDisable( GL_LIGHTING );
640  drawKineChains();
641  alpha = 130;
642  glPopAttrib();
643  }
644  //--- display objects
645  if ( showobjs ) {
646  glPushAttrib( GL_ALL_ATTRIB_BITS );
647  //--- wireframe only from this point to end
648  if ( wiref ) {
649  glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
650  } else {
651  glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
652  }
653  glEnable( GL_LIGHTING );
654  PhyObject* pr;
655  for( int i=0; i<graphics().size(); i++ ) {
656  RenderWObject* r = graphics()[i];
657  if ( r->object()->isInvisible() ) continue;
658  if ( showforces && ( pr=dynamic_cast<PhyObject*>(r->object()) ) ) {
659  glPushAttrib( GL_ALL_ATTRIB_BITS );
660  glColor4f( 1.0, 0.0, 0.0, 1.0 );
661  drawArrow( pr->matrix().w_pos, pr->matrix().w_pos + pr->force(), 0.1f );
662  glColor4f( 0.0, 1.0, 0.0, 1.0 );
663  drawArrow( pr->matrix().w_pos, pr->matrix().w_pos + pr->torque(), 0.1f );
664  alpha = 130;
665  glPopAttrib();
666  }
667  r->object()->setAlpha( alpha );
668  glPushAttrib( GL_ALL_ATTRIB_BITS );
669  r->render( (QGLContext*)( QGLContext::currentContext() ) );
670  glPopAttrib();
671  if ( showaabbs && currentSelected == i ) {
672  glPushAttrib( GL_ALL_ATTRIB_BITS );
673  r->renderAABB( this );
674  glPopAttrib();
675  wVector dims, minPoint, maxPoint;
676  r->calculateOBB( dims, minPoint, maxPoint );
677  glPushAttrib( GL_ALL_ATTRIB_BITS );
678  drawWireBox( minPoint, maxPoint, r->object()->matrix() );
679  glPopAttrib();
680  }
681  }
682  }
683 
684  //--- draw some text
685  glPushAttrib( GL_ALL_ATTRIB_BITS );
686  glColor4f( 1.0, 0.0, 0.0, 1.0 );
687  drawText( 80, 15, QString("time: %1 step: %2")
688  .arg(world()->elapsedTime())
689  .arg((int)(world()->elapsedTime()/world()->timeStep()))
690  );
691  glPopAttrib();
692 }
693 
694 void RenderWorld::drawWithNames() {
695  QMutexLocker locker(&mutex);
696 
697  for( int i=0; i< graphics().size(); i++ ) {
698  glPushName(i);
699  graphics()[i]->render( (QGLContext*)( QGLContext::currentContext() ) );
700  glPopName();
701  }
702 }
703 
704 void RenderWorld::postSelection(const QPoint& point) {
705  QMutexLocker locker(&mutex);
706 
707  UNUSED_PARAM( point );
708  int i = selectedName();
709  if ( i != -1 && i != currentSelected ) {
710  graphics()[qMax(0,currentSelected)]->object()->setColor( Qt::white );
711  graphics()[i]->object()->setColor( Qt::yellow );
712  currentSelected = i;
713  }
714 }
715 
716 void RenderWorld::keyPressEvent(QKeyEvent *e) {
717  QMutexLocker locker(&mutex);
718 
719  // Get event modifiers key
720 #if QT_VERSION < 0x040000
721  // Bug in Qt : use 0x0f00 instead of Qt::KeyButtonMask with Qt versions < 3.1
722  const Qt::ButtonState modifiers = (Qt::ButtonState)(e->state() & Qt::KeyButtonMask);
723 #else
724  const Qt::KeyboardModifiers modifiers = e->modifiers();
725 #endif
726 
727  // A simple switch on e->key() is not sufficient if we want to take state key into account.
728  // With a switch, it would have been impossible to separate 'F' from 'CTRL+F'.
729  // That's why we use imbricated if...else and a "handled" boolean.
730  bool handled = false;
731  if ((e->key()==Qt::Key_Left) && (modifiers==Qt::NoButton)) {
732  // rotate camera
733  Quaternion qcur = camera()->orientation();
734  Quaternion qnew = qcur * Quaternion( Vec(0,1,0), 3.1415/30 );
735  camera()->setOrientation( qnew );
736  handled = true;
737  updateGL();
738  } else if ((e->key()==Qt::Key_Right) && (modifiers==Qt::NoButton)) {
739  // rotate camera
740  Quaternion qcur = camera()->orientation();
741  Quaternion qnew = qcur * Quaternion( Vec(0,1,0), -3.1415/30 );
742  camera()->setOrientation( qnew );
743  handled = true;
744  updateGL();
745  }
746  if ((e->key()==Qt::Key_Up) && (modifiers==Qt::NoButton)) {
747  // rotate camera
748  Quaternion qcur = camera()->orientation();
749  Quaternion qnew = qcur * Quaternion( Vec(1,0,0), 3.1415/30 );
750  camera()->setOrientation( qnew );
751  handled = true;
752  updateGL();
753  } else if ((e->key()==Qt::Key_Down) && (modifiers==Qt::NoButton)) {
754  // rotate camera
755  Quaternion qcur = camera()->orientation();
756  Quaternion qnew = qcur * Quaternion( Vec(1,0,0), -3.1415/30 );
757  camera()->setOrientation( qnew );
758  handled = true;
759  updateGL();
760  }
761  //--- Context Menu (not accessible with right-click, because right button as different meaning)
762  if ((e->key()==Qt::Key_M) && (modifiers==Qt::NoButton)) {
763  contextMenu( mapToGlobal(QPoint(10,10)) );
764  handled = true;
765  }
766 
767  if (!handled) {
768  QGLViewer::keyPressEvent(e);
769  }
770 }
771 
772 void RenderWorld::resourceChanged(QString name, ResourceChangeType changeType)
773 {
774  QMutexLocker locker(&mutex);
775 
776  // Calling parent function
777  RenderWObjectContainer::resourceChanged(name, changeType);
778 
779  if (world() == NULL) {
780  return;
781  }
782 
783  // Connecting slots and updating world dimensions
784 
785  // These connections are direct because syncronization is done with Mutexes
786  connect( world(), SIGNAL( removedObject( WObject* ) ),
787  this, SLOT( slotRemoveObject( WObject* ) ), Qt::DirectConnection );
788  connect( world(), SIGNAL( addedObject( WObject* ) ),
789  this, SLOT( slotAddObject( WObject* ) ), Qt::DirectConnection );
790  connect( world(), SIGNAL( resized() ),
791  this, SLOT( onWorldResize() ), Qt::DirectConnection );
792 
793  connect( world(), SIGNAL( advanced() ),
794  this, SLOT( update() ) );
795 
796  // We can't call onWorldResize because that function tries to take a lock (and we can't from inside resourceChanged)
797  MyCameraConstraint* tmp = dynamic_cast<MyCameraConstraint*>( camera()->frame()->constraint() );
798  if ( tmp != NULL ) {
799  wVector min, max;
800  world()->size( min, max );
801  setSceneBoundingBox( qglviewer::Vec( min[0], min[1], min[2] ),
802  qglviewer::Vec( max[0], max[1], max[2] ) );
803  tmp->minP = qglviewer::Vec( min[0], min[1], min[2] );
804  tmp->maxP = qglviewer::Vec( max[0], max[1], max[2] );
805  }
806 
807  // Forcing renderworld update (we cannot call update() because here we could be in a different thread)
808  QCoreApplication::postEvent(this, new ForceRenderWorldUpdateEvent());
809 }
810 
811 void RenderWorld::drawDOF( PhyDOF* dof, bool drawAxes ) {
812  // Used inside draw(), no need to lock here
813 
814  if ( dof->joint() == NULL ) return;
815 
816  //--- FIX ME: this method handles only rotational joints :-(
817  if ( dof->translate() ) return;
818 
819  wMatrix mat;
820  mat.x_ax = dof->xAxis();
821  mat.y_ax = dof->yAxis();
822  mat.z_ax = dof->axis();
823  mat.w_pos = dof->centre();
824  mat.sanitifize();
825 
826  PhyJoint* joint = dof->joint();
827  PhyObject* child = joint->child();
828  PhyObject* parent= joint->parent();
829  RenderWorld& rw = *this;
830  wVector dims, minP, maxP;
831 // wVector lax = dof->axis();
832  rw[ child ]->calculateOBB( dims, minP, maxP );
833  real cube = (dims[0]+dims[1]+dims[2])/3.0;
834  if ( parent ) {
835  rw[ parent ]->calculateOBB( dims, minP, maxP );
836  real cube2 = (dims[0]+dims[1]+dims[2])/3.0;
837  cube = min( cube2, cube );
838  }
839  real len = cube * 0.70;
840  real rad = len * 0.25;
841 
842  mat.w_pos = dof->centre() - dof->axis().scale(len/2.0);
843  drawCylinder( mat, len, rad, QColor(Qt::cyan) );
844  if ( dof->isLimited() ) {
845  //--- draw indication about limits
846  real lo, hi;
847  dof->limits( lo, hi );
848  real ang = hi-lo;
849  mat.w_pos = wVector(0,0,0);
850  mat = mat * wMatrix( wQuaternion( dof->axis(), lo ), wVector(0,0,0) );
851  mat.w_pos = dof->centre() + dof->axis().scale(len/2.0);
852  drawTorus( rad, rad*0.6, mat, ang );
853  } else {
854  mat.w_pos = dof->centre() + dof->axis().scale(len/2.0);
855  drawTorus( rad, rad*0.6, mat );
856  }
857  //--- draw indication about current position
858  mat.x_ax = dof->xAxis();
859  mat.y_ax = dof->yAxis();
860  mat.z_ax = dof->axis();
861  mat.w_pos = wVector(0,0,0);
862  mat.sanitifize();
863  mat = mat * wMatrix( wQuaternion( dof->axis(), dof->position()-0.05 ), wVector(0,0,0) );
864  mat.w_pos = dof->centre() + dof->axis().scale(len/2.0);
865  drawTorus( rad, rad*0.55f, mat, 0.1f, Qt::green );
866 
867  if ( drawAxes ) {
868  drawArrow( dof->centre(), dof->centre()+dof->xAxis().scale(len*1.2), rad*0.4, 12, Qt::magenta );
869  drawArrow( dof->centre(), dof->centre()+dof->yAxis().scale(len*1.2), rad*0.5, 12, Qt::yellow );
870  }
871 }
872 
873 void RenderWorld::drawKineChains() {
874  // Used inside draw(), no need to lock here
875 
876  if (world() == NULL) {
877  return;
878  }
879 
880  foreach( PhyJoint* jn, world()->joints()) {
881  //if ( ! jn->isEnabled() ) continue;
882  float dist1 = 0.0f, dist2 = 0.0f;
883  wVector start = jn->centre();
884  wVector end1 = jn->child()->matrix().w_pos;
885  wVector end2 = start;
886  dist1 = wVector::distance( start, end1 );
887  if ( jn->parent() ) {
888  end2 = jn->parent()->matrix().w_pos;
889  dist2 = wVector::distance( start, end2 );
890  }
891  real rad = (dist1 + dist2) * 0.04;
892  drawCylinder( start, end1, rad*0.6 );
893  drawCylinder( start, end2, rad*0.6 );
894 
895  QVector<PhyDOF*> ds = jn->dofs();
896  for( int k=0; k<ds.size(); k++ ) {
897  drawDOF( ds[k], true );
898  }
899  }
900 }
901 
902 void RenderWorld::customEvent(QEvent* event)
903 {
904  if (event->type() == QEvent::User) {
905  // Forcing RenderWorld update
906  update();
907  }
908 }
909 
910 void RenderWObjectContainer::drawSphere( wVector pos, real radius ) {
911  GLUquadricObj *pObj;
912 
913  // set the color
914  glShadeModel( GL_SMOOTH );
915  glColor4f( 0.0f, 1.0f, 0.0f, 1.0f );
916 
917  wMatrix mat = wMatrix::identity();
918  mat.w_pos = pos;
919  mat.x_ax = mat.x_ax.scale( radius );
920  mat.y_ax = mat.y_ax.scale( radius );
921  mat.z_ax = mat.z_ax.scale( radius );
922  glPushMatrix();
923  GLMultMatrix(&mat[0][0]);
924 
925  // Get a new Quadric off the stack
926  pObj = gluNewQuadric();
927  // Get a new Quadric off the stack
928  gluQuadricTexture(pObj, true);
929  gluSphere(pObj, 1.0f, 20, 20);
930 
931  gluDeleteQuadric(pObj);
932  glPopMatrix();
933 }
934 
935 void RenderWObjectContainer::drawCylinder( wVector axis, wVector centre, float h, float rad, QColor c ) {
936  GLUquadricObj *pObj;
937 
938  glPushMatrix();
939 
940  // set the color
941  glShadeModel( GL_SMOOTH );
942  glColor4f( c.redF(), c.greenF(), c.blueF(), c.alphaF() );
943 
944  wVector recentre = axis.scale( -h*0.5 ) + centre;
945  glTranslatef( recentre[0], recentre[1], recentre[2] );
946 
947  Vec xg(0,0,1);
948  Vec ax( axis[0], axis[1], axis[2] );
949  Quaternion quad( xg, ax );
950  glMultMatrixd( quad.matrix() );
951 
952  // Get a new Quadric off the stack
953  pObj = gluNewQuadric();
954  gluQuadricTexture(pObj, true);
955  gluCylinder(pObj, rad, rad, h, 20, 2);
956 
957  // render the caps
958  gluQuadricOrientation(pObj, GLU_INSIDE);
959  gluDisk(pObj, 0.0f, rad, 20, 1);
960 
961  glTranslatef (0.0f, 0.0f, h);
962  gluQuadricOrientation(pObj, GLU_OUTSIDE);
963  gluDisk(pObj, 0.0f, rad, 20, 1);
964 
965  gluDeleteQuadric(pObj);
966  glPopMatrix();
967 }
968 
969 void RenderWObjectContainer::drawCylinder( wVector start, wVector end, float rad, QColor c ) {
970  float h = wVector::distance( start, end );
971  if ( h < 0.0001 ) return;
972 
973  GLUquadricObj *pObj;
974 
975  glPushMatrix();
976 
977  // set the color
978  glShadeModel( GL_SMOOTH );
979  glColor4f( c.redF(), c.greenF(), c.blueF(), c.alphaF() );
980 
981  wVector zaxis = end - start;
982  zaxis.normalize();
983  wMatrix tm = wMatrix::grammSchmidt( zaxis );
984  tm.w_pos = start;
985  GLMultMatrix( &tm[0][0] );
986 
987  // Get a new Quadric off the stack
988  pObj = gluNewQuadric();
989  gluQuadricTexture(pObj, true);
990  gluCylinder(pObj, rad, rad, h, 20, 2);
991 
992  // render the caps
993  gluQuadricOrientation(pObj, GLU_INSIDE);
994  gluDisk(pObj, 0.0f, rad, 20, 1);
995 
996  glTranslatef (0.0f, 0.0f, h);
997  gluQuadricOrientation(pObj, GLU_OUTSIDE);
998  gluDisk(pObj, 0.0f, rad, 20, 1);
999 
1000  gluDeleteQuadric(pObj);
1001  glPopMatrix();
1002 }
1003 
1004 void RenderWObjectContainer::drawCylinder( const wMatrix& mat, float h, float rad, QColor c ) {
1005  GLUquadricObj *pObj;
1006 
1007  glPushMatrix();
1008 
1009  // set the color
1010  glShadeModel( GL_SMOOTH );
1011  glColor4f( c.redF(), c.greenF(), c.blueF(), c.alphaF() );
1012 
1013  GLMultMatrix(&mat[0][0]);
1014 
1015  // Get a new Quadric off the stack
1016  pObj = gluNewQuadric();
1017  gluQuadricTexture(pObj, true);
1018  gluCylinder(pObj, rad, rad, h, 20, 2);
1019 
1020  // render the caps
1021  gluQuadricOrientation(pObj, GLU_INSIDE);
1022  gluDisk(pObj, 0.0f, rad, 20, 1);
1023 
1024  glTranslatef (0.0f, 0.0f, h);
1025  gluQuadricOrientation(pObj, GLU_OUTSIDE);
1026  gluDisk(pObj, 0.0f, rad, 20, 1);
1027 
1028  gluDeleteQuadric(pObj);
1029  glPopMatrix();
1030 }
1031 
1032 void RenderWObjectContainer::drawWireBox( wVector dims, wMatrix matrix ) {
1033  glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
1034  glPushMatrix();
1035 
1036  // set the color
1037  glShadeModel( GL_SMOOTH );
1038  glColor4f( 1.0f, 1.0f, 0.0f, 1.0f );
1039 
1040  float hdx = (dims[0]/2.0);
1041  float hdy = (dims[1]/2.0);
1042  float hdz = (dims[2]/2.0);
1043  GLMultMatrix(&matrix[0][0]);
1044 
1045  // the cube will just be drawn as six quads for the sake of simplicity
1046  // for each face, we specify the quad's normal (for lighting), then
1047  // specify the quad's 4 vertices and associated texture coordinates
1048  glBegin(GL_QUADS);
1049  // front
1050  glNormal3f(0.0, 0.0, 1.0);
1051  glVertex3f(-hdx, -hdy, hdz);
1052  glVertex3f( hdx, -hdy, hdz);
1053  glVertex3f( hdx, hdy, hdz);
1054  glVertex3f(-hdx, hdy, hdz);
1055 
1056  // back
1057  glNormal3f(0.0, 0.0, -1.0);
1058  glVertex3f( hdx, -hdy, -hdz);
1059  glVertex3f(-hdx, -hdy, -hdz);
1060  glVertex3f(-hdx, hdy, -hdz);
1061  glVertex3f( hdx, hdy, -hdz);
1062 
1063  // top
1064  glNormal3f(0.0, 1.0, 0.0);
1065  glVertex3f(-hdx, hdy, hdz);
1066  glVertex3f( hdx, hdy, hdz);
1067  glVertex3f( hdx, hdy, -hdz);
1068  glVertex3f(-hdx, hdy, -hdz);
1069 
1070  // bottom
1071  glNormal3f(0.0, -1.0, 0.0);
1072  glVertex3f(-hdx, -hdy, -hdz);
1073  glVertex3f( hdx, -hdy, -hdz);
1074  glVertex3f( hdx, -hdy, hdz);
1075  glVertex3f(-hdx, -hdy, hdz);
1076 
1077  // left
1078  glNormal3f(-1.0, 0.0, 0.0);
1079  glVertex3f(-hdx, -hdy, -hdz);
1080  glVertex3f(-hdx, -hdy, hdz);
1081  glVertex3f(-hdx, hdy, hdz);
1082  glVertex3f(-hdx, hdy, -hdz);
1083 
1084  // right
1085  glNormal3f(1.0, 0.0, 0.0);
1086  glVertex3f(hdx, -hdy, hdz);
1087  glVertex3f(hdx, -hdy, -hdz);
1088  glVertex3f(hdx, hdy, -hdz);
1089  glVertex3f(hdx, hdy, hdz);
1090  glEnd();
1091 
1092  glPopMatrix();
1093 }
1094 
1095 void RenderWObjectContainer::drawWireBox( wVector min, wVector max, const wMatrix& matrix ) {
1096  glPushMatrix();
1097  GLMultMatrix(&matrix[0][0]);
1098  drawWireBox( min, max );
1099  glPopMatrix();
1100 }
1101 
1102 void RenderWObjectContainer::drawWireBox( wVector min, wVector max ) {
1103  glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
1104  glPushMatrix();
1105 
1106  // set the color
1107  glShadeModel( GL_SMOOTH );
1108  glColor4f( 1.0f, 1.0f, 0.0f, 1.0f );
1109 
1110  float dx = fabs( max[0]-min[0] );
1111  float dy = fabs( max[1]-min[1] );
1112  float dz = fabs( max[2]-min[2] );
1113  float hdx = (dx/2.0);
1114  float hdy = (dy/2.0);
1115  float hdz = (dz/2.0);
1116  float minX = qMin(min[0], max[0]);
1117  float minY = qMin(min[1], max[1]);
1118  float minZ = qMin(min[2], max[2]);
1119  glTranslatef( minX+hdx, minY+hdy, minZ+hdz );
1120 
1121  // the cube will just be drawn as six quads for the sake of simplicity
1122  // for each face, we specify the quad's normal (for lighting), then
1123  // specify the quad's 4 vertices and associated texture coordinates
1124  glBegin(GL_QUADS);
1125  // front
1126  glNormal3f(0.0, 0.0, 1.0);
1127  glVertex3f(-hdx, -hdy, hdz);
1128  glVertex3f( hdx, -hdy, hdz);
1129  glVertex3f( hdx, hdy, hdz);
1130  glVertex3f(-hdx, hdy, hdz);
1131 
1132  // back
1133  glNormal3f(0.0, 0.0, -1.0);
1134  glVertex3f( hdx, -hdy, -hdz);
1135  glVertex3f(-hdx, -hdy, -hdz);
1136  glVertex3f(-hdx, hdy, -hdz);
1137  glVertex3f( hdx, hdy, -hdz);
1138 
1139  // top
1140  glNormal3f(0.0, 1.0, 0.0);
1141  glVertex3f(-hdx, hdy, hdz);
1142  glVertex3f( hdx, hdy, hdz);
1143  glVertex3f( hdx, hdy, -hdz);
1144  glVertex3f(-hdx, hdy, -hdz);
1145 
1146  // bottom
1147  glNormal3f(0.0, -1.0, 0.0);
1148  glVertex3f(-hdx, -hdy, -hdz);
1149  glVertex3f( hdx, -hdy, -hdz);
1150  glVertex3f( hdx, -hdy, hdz);
1151  glVertex3f(-hdx, -hdy, hdz);
1152 
1153  // left
1154  glNormal3f(-1.0, 0.0, 0.0);
1155  glVertex3f(-hdx, -hdy, -hdz);
1156  glVertex3f(-hdx, -hdy, hdz);
1157  glVertex3f(-hdx, hdy, hdz);
1158  glVertex3f(-hdx, hdy, -hdz);
1159 
1160  // right
1161  glNormal3f(1.0, 0.0, 0.0);
1162  glVertex3f(hdx, -hdy, hdz);
1163  glVertex3f(hdx, -hdy, -hdz);
1164  glVertex3f(hdx, hdy, -hdz);
1165  glVertex3f(hdx, hdy, hdz);
1166  glEnd();
1167 
1168  glPopMatrix();
1169 }
1170 
1171 void RenderWObjectContainer::drawTorus( real outRad, real innRad, const wMatrix& matrix, real angle, QColor c ) {
1172  glPushMatrix();
1173 
1174  // set the color
1175  glShadeModel( GL_SMOOTH );
1176  glColor4f( c.redF(), c.greenF(), c.blueF(), c.alphaF() );
1177 
1178  GLMultMatrix(&matrix[0][0]);
1179 
1180  int numc = 8;
1181  int numt = 25;
1182  int i, j, k;
1183  double s, t, x, y, z, twopi;
1184  real tubeRad = (outRad-innRad)/2.0;
1185  real toruRad = outRad - tubeRad;
1186  twopi = 2 * PI_GRECO;
1187  for (i = 0; i < numc; i++) {
1188  glBegin(GL_QUAD_STRIP);
1189  for (j = 0; j <= numt; j++) {
1190  for (k = 1; k >= 0; k--) {
1191  s = (i + k) % numc + 0.5;
1192  t = j; //% numt;
1193 
1194  x = (toruRad+tubeRad*cos(s*twopi/numc))*cos(t*(angle)/numt);
1195  y = (toruRad+tubeRad*cos(s*twopi/numc))*sin(t*(angle)/numt);
1196  z = tubeRad * sin(s * twopi / numc);
1197  glVertex3f(x, y, z);
1198  }
1199  }
1200  glEnd();
1201  }
1202 
1203  glPopMatrix();
1204 }
1205 
1206 void RenderWObjectContainer::drawTorus( wVector axis, wVector centre, real outRad, real innRad, real angle ) {
1207  glPushMatrix();
1208 
1209  // set the color
1210  glShadeModel( GL_SMOOTH );
1211  glColor4f( 1.0f, 0.0f, 0.0f, 1.0f );
1212 
1213  wMatrix tm = wMatrix::grammSchmidt( axis );
1214  tm.w_pos = centre;
1215  GLMultMatrix( &tm[0][0] );
1216 
1217  int numc = 8;
1218  int numt = 25;
1219  int i, j, k;
1220  double s, t, x, y, z, twopi;
1221  real tubeRad = (outRad-innRad)/2.0;
1222  real toruRad = outRad - tubeRad;
1223  twopi = 2 * PI_GRECO;
1224  for (i = 0; i < numc; i++) {
1225  glBegin(GL_QUAD_STRIP);
1226  for (j = 0; j <= numt; j++) {
1227  for (k = 1; k >= 0; k--) {
1228  s = (i + k) % numc + 0.5;
1229  t = j; //% numt;
1230 
1231  x = (toruRad+tubeRad*cos(s*twopi/numc))*cos(t*(angle)/numt);
1232  y = (toruRad+tubeRad*cos(s*twopi/numc))*sin(t*(angle)/numt);
1233  z = tubeRad * sin(s * twopi / numc);
1234  glVertex3f(x, y, z);
1235  }
1236  }
1237  glEnd();
1238  }
1239 
1240  glPopMatrix();
1241 }
1242 
1243 void RenderWorld::drawArrow( const wVector& start, const wVector& end, float radius, int nbSubdivisions, QColor c ) {
1244  glPushMatrix();
1245  glColor4f( c.redF(), c.greenF(), c.blueF(), c.alphaF() );
1246 
1247  wVector zaxis = end - start;
1248  real len = zaxis.norm();
1249  zaxis.normalize();
1250  wMatrix tm = wMatrix::grammSchmidt( zaxis );
1251  tm.w_pos = start;
1252  GLMultMatrix( &tm[0][0] );
1253 
1254  QGLViewer::drawArrow( len, radius, nbSubdivisions );
1255 
1256  glPopMatrix();
1257 }
1258 
1259 } // end namespace farsa