ViSP  2.8.0
vpSimulator.cpp
1 /****************************************************************************
2  *
3  * $Id: vpSimulator.cpp 4323 2013-07-18 09:24:01Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Simulator based on Coin3d.
36  *
37  * Authors:
38  * Eric Marchand
39  * Anthony Saunier
40  *
41  *****************************************************************************/
48 #include <visp/vpConfig.h>
49 
50 #ifdef VISP_HAVE_COIN_AND_GUI
51 
52 #include <visp/vpSimulator.h>
53 #include <visp/vpTime.h>
54 
55 #include <visp/vpImage.h>
56 #include <visp/vpImageIo.h>
57 
58 /* Objets OIV. */
59 #include <Inventor/nodes/SoCone.h> /* Objet cone. */
60 #include <Inventor/nodes/SoCylinder.h> /* Objet cylindre. */
61 #include <Inventor/nodes/SoPointLight.h> /* Objet lumiere ponctuelle. */
62 #include <Inventor/nodes/SoCoordinate3.h> /* Liste de points. */
63 #include <Inventor/nodes/SoIndexedFaceSet.h> /* Liste de face. */
64 #include <Inventor/nodes/SoTranslation.h> /* Trasnfo translation. */
65 #include <Inventor/nodes/SoScale.h> /* Trasnfo mise a l'echelle. */
66 #include <Inventor/nodes/SoRotationXYZ.h> /* Transfo rotation simple. */
67 
68 #include <Inventor/nodes/SoDirectionalLight.h> /* Objet lumiere directionnelle*/
69 #include <Inventor/nodes/SoMaterial.h> /* Matiere (couleur) des objets. */
70 #include <Inventor/nodes/SoDrawStyle.h> /* Style de rendu. */
71 #include <Inventor/nodes/SoEnvironment.h> /* Eclairage ambiant. */
72 #include <Inventor/nodes/SoGroup.h> /* Groupement de noeuds (sans separation)*/
73 #include <Inventor/actions/SoWriteAction.h>
74 
75 
76 
77 
78 // Positions of all of the vertices:
79 //
80 static float pyramidVertexes [5][3] =
81  {
82  {0.33f, 0.33f, 0.f},
83  {-0.33f, 0.33f, 0.f},
84  {-0.33f, -0.33f, 0.f},
85  {0.33f, -0.33f, 0.f},
86 
87  {0.f, 0.f, -1.0f}
88  };
89 
90 
91 static int32_t pyramidFaces[] =
92  {
93  0, 1, 2, 3, SO_END_FACE_INDEX, // top face
94 
95  0, 1, 4, SO_END_FACE_INDEX, // 4 faces about top
96  1, 2, 4, SO_END_FACE_INDEX,
97  2, 3, 4, SO_END_FACE_INDEX,
98  3, 0, 4, SO_END_FACE_INDEX,
99  };
100 
101 
102 // Routine to create a scene graph representing a dodecahedron
103 SoSeparator *
104 makePyramide()
105 {
106  SoSeparator *result = new SoSeparator;
107  result->ref();
108 
109  // Define coordinates for vertices
110  SoCoordinate3 *myCoords = new SoCoordinate3;
111  myCoords->point.setValues(0, 5, pyramidVertexes);
112  result->addChild(myCoords);
113 
114  // Define the IndexedFaceSet, with indices into the vertices:
115  SoIndexedFaceSet *myFaceSet = new SoIndexedFaceSet;
116  myFaceSet->coordIndex.setValues (0, 21, (const int32_t*)pyramidFaces);
117  result->addChild (myFaceSet);
118 
119  result->unrefNoDelete();
120  return result;
121 }
122 
123 /* Cree une fleche composee d'un cylindre et d'un cone.
124  * La fleche a une hauteur total de <longueur>, dont
125  * <proportionFleche>% pour la fleche. Le rayon du cylindre
126  * est <radius>, et celui de la fleche <radius> * 5.
127  * La fleche est oriente selon l'axe Y.
128  */
129 static SoSeparator *
130 createArrow (float longueur,
131  float proportionFleche,
132  float radius)
133 {
134  SoSeparator *fleche = new SoSeparator;
135  fleche->ref();
136 
137  SoTranslation *poseCylindre = new SoTranslation;
138  SoCylinder *line = new SoCylinder;
139  SoTranslation *posePointe = new SoTranslation;
140  SoCone *pointe = new SoCone;
141 
142  float l_cylindre = longueur * ( 1 - proportionFleche);
143  float l_cone = longueur * proportionFleche;
144  float radius_cylindre = radius;
145  float radius_cone = radius * 5;
146 
147  line->radius.setValue (radius_cylindre);
148  line->height.setValue (l_cylindre);
149 
150  poseCylindre->translation.setValue (0, l_cylindre / 2, 0);
151  posePointe->translation.setValue (0.0, l_cylindre / 2 + l_cone / 2, 0);
152 
153  pointe->bottomRadius.setValue (radius_cone);
154  pointe->height.setValue (l_cone);
155 
156 
157  fleche->addChild (poseCylindre);
158  fleche->addChild (line);
159  fleche->addChild (posePointe);
160  fleche->addChild (pointe);
161 
162  return fleche;
163 }
164 
165 
166 /*
167  Cree un objet repere dans un noeud separator, et le renvoie.
168  \return : code d'erreur, SIMU_CODE_OK si tout s'est bien passe.
169 */
170 #define LONGUEUR_FLECHE 1.0f
171 #define RAYON_FLECHE 0.002f
172 #define PROPORTION_FLECHE 0.1f
173 
174 SoSeparator *
175 createFrame (float longueurFleche = LONGUEUR_FLECHE ,
176  float proportionFleche = PROPORTION_FLECHE,
177  float radiusFleche = RAYON_FLECHE)
178 {
179  vpDEBUG_TRACE (15, "# Entree.");
180 
181  SoSeparator *frame = new SoSeparator;
182  frame-> ref ();
183 
184  SoRotationXYZ *rotationY_X = new SoRotationXYZ;
185  rotationY_X->axis = SoRotationXYZ::Z;
186  rotationY_X->angle.setValue ((float)(- M_PI / 2));
187 
188  SoRotationXYZ *rotationX_Y = new SoRotationXYZ;
189  rotationX_Y->axis = SoRotationXYZ::Z;
190  rotationX_Y->angle.setValue ((float)(M_PI / 2));
191 
192  SoRotationXYZ *rotationY_Z = new SoRotationXYZ;
193  rotationY_Z->axis = SoRotationXYZ::X;
194  rotationY_Z->angle.setValue ((float)(M_PI / 2));
195 
196  SoMaterial *rouge = new SoMaterial;
197  rouge->diffuseColor.setValue(1.0, 0.0, 0.0);
198  rouge->emissiveColor.setValue(0.5, 0.0, 0.0);
199 
200  SoMaterial *vert = new SoMaterial;
201  vert->diffuseColor.setValue(0.0, 1.0, 0.0);
202  vert->emissiveColor.setValue(0.0, 0.5, 0.0);
203 
204  SoMaterial *bleu = new SoMaterial;
205  bleu->diffuseColor.setValue(0.0, 0.0, 1.0);
206  bleu->emissiveColor.setValue(0.0, 0.0, 0.5);
207 
208  SoSeparator *fleche = createArrow(longueurFleche,
209  proportionFleche,
210  radiusFleche);
211 
212  frame->addChild (rouge);
213  frame->addChild (rotationY_X);
214  frame->addChild (fleche);
215  frame->addChild (vert);
216  frame->addChild (rotationX_Y);
217  frame->addChild (fleche);
218  frame->addChild (bleu);
219  frame->addChild (rotationY_Z);
220  frame->addChild (fleche);
221 
222  frame-> unrefNoDelete ();
223 
224  vpDEBUG_TRACE (15, "# Sortie.");
225  return frame;
226 }
227 
228 SoSeparator *
229 createCameraObject (const float zoomFactor = 1.0)
230 {
231  vpDEBUG_TRACE (15, "# Entree.");
232 
233  SoSeparator * cam = new SoSeparator;
234  cam->ref ();
235 
236  SoMaterial *myMaterial = new SoMaterial;
237  myMaterial->diffuseColor.setValue(1.0, 0.0, 0.0);
238  myMaterial->emissiveColor.setValue(0.5, 0.0, 0.0);
239 
240  SoScale *taille = new SoScale;
241  {
242  float zoom = 0.1f * zoomFactor;
243  taille->scaleFactor.setValue (zoom, zoom, zoom);
244  }
245 
246  SoMaterial *couleurBlanc = new SoMaterial;
247  couleurBlanc->diffuseColor.setValue(1.0, 1.0, 1.0);
248  couleurBlanc->emissiveColor.setValue(1.0, 1.0, 1.0);
249  SoDrawStyle * filDeFer = new SoDrawStyle;
250  filDeFer->style.setValue (SoDrawStyle::LINES);
251  filDeFer->lineWidth.setValue (1);
252 
253  SoSeparator * cone = new SoSeparator;
254  cone->ref();
255  cone->addChild (makePyramide());
256  cone->addChild (couleurBlanc);
257  cone->addChild (filDeFer);
258  cone->addChild (makePyramide());
259  cone->unrefNoDelete();
260 
261  cam->addChild(myMaterial);
262  cam->addChild(taille);
263  cam->addChild(cone);
264  cam->addChild(createFrame(2.0f,0.1f,0.01f));
265 
266  // cam->unref() ;
267  vpDEBUG_TRACE (15, "# Sortie.");
268  return cam;
269 }
270 
271 
272 //--------------------------------------------------------------
273 void
275 {
276  internal_width = 200;
277  internal_height= 200;
278  external_width = 200;
279  external_height= 200;
280 
281  mainWindowInitialized = false ;
282  internalView = NULL ;
283  externalView = NULL ;
284  image_background = NULL ;
285 
286  zoomFactor = 1 ;
287  cameraPositionInitialized = false ;
288 
289  // write image process
290  realtime=NULL ;
291  offScreenRenderer = NULL ;
292  bufferView = NULL;
293  get = 1 ;
294 }
295 void
297 {
298  if (internalView !=NULL) {delete internalView ; internalView = NULL;}
299  if (externalView !=NULL) {delete externalView ; externalView = NULL;}
300  if (bufferView!=NULL) {delete[] bufferView ; bufferView = NULL;}
301  if (image_background != NULL) {
302  free (image_background);
303  image_background = NULL;
304  }
305 
306 }
307 
309 {
311 }
312 
314 {
316 }
317 
318 void
320 {
321  mainWindow = vpViewer::init("");
322  mainWindowInitialized = true ;
323 }
324 
325 void
327 {
328  this->scene = new SoSeparator;
329  this->internalRoot = new SoSeparator;
330  this->externalRoot = new SoSeparator;
331 
332  this->scene->ref();
333  this->internalRoot->ref();
334  this->externalRoot->ref();
335 
336  // define the camera SoPerspectiveCamera
337  this->internalCamera = new SoPerspectiveCamera ;
338  this->externalCamera = new SoPerspectiveCamera ;
339 
340 
341  this->internalCameraPosition = new SoTransform;
342  this->internalCameraObject = createCameraObject(zoomFactor);
343 
344  internalCamera->farDistance.setValue(100);
345  internalCamera->nearDistance.setValue(0.0001f);
346 
347 
348 
349  // link between camera and internal root
350  this->internalRoot->addChild (this->internalCamera);
351  this->internalRoot->addChild (this->scene);
352 
353  this->externalRoot->addChild (this->externalCamera);
354  this->externalRoot->addChild (this->scene);
355 
356 
357  SoSeparator * camera = new SoSeparator;
358  camera->ref();
359  camera->addChild (this->internalCameraPosition);
360  camera->addChild (this->internalCameraObject);
361  this->externalRoot->addChild (camera);
362 
363 
364  //this->externalRoot->addChild (internalCameraPosition);
365  // this->externalRoot->addChild (internalCameraObject);
366  SoCube *cube = new SoCube ;
367  cube->width=0.01f ;
368  cube->depth=0.01f ;
369  cube->height=0.01f ;
370 
371  this->externalRoot->addChild (cube);
372 
373  if (realtime==NULL)
374  {
375 
376  SoDB::enableRealTimeSensor(FALSE);
377  SoSceneManager::enableRealTimeUpdate(FALSE);
378  realtime = (SbTime *) SoDB::getGlobalField("realTime");
379  realtime->setValue(0.0);
380 
381  }
382 
383 
384 }
385 
386 
387 
394 void
395 vpSimulator::setZoomFactor (const float zoom)
396 {
397  zoomFactor = zoom;
398  static bool firstTime = true;
399  if(firstTime){
400  SoScale *taille = new SoScale;
401  taille->scaleFactor.setValue (zoomFactor, zoomFactor, zoomFactor);
402  this->scene->addChild(taille);
403  firstTime = false;
404  }
405  else{
406  SoScale * taille = (SoScale*)this->scene->getChild(0);
407  taille->scaleFactor.setValue (zoomFactor, zoomFactor, zoomFactor);
408  }
409 }
410 
428 void
429 vpSimulator::changeZoomFactor(const float zoomFactor, const int index)
430 {
431  SoScale * taille = (SoScale*)this->scene->getChild(index);
432  taille->scaleFactor.setValue (zoomFactor, zoomFactor, zoomFactor);
433 // this->setZoomFactor(zoomFactor);
434 }
435 
436 void
437 vpSimulator::initInternalViewer(const unsigned int width, const unsigned int height)
438 {
439  internal_width = width;
440  internal_height = height;
441 
442  if (mainWindowInitialized==false)
443  {
445  initSceneGraph() ;
446  }
447 
449 
450  // set the scene to render from this view
451  internalView->setSceneGraph(internalRoot);
452 
453  // set the title
454  internalView->setTitle("Internal camera view") ;
455 
456  //If the view mode is on, user events will be caught and used to influence
457  //the camera position / orientation. in this viewer we do not want that,
458  //we set it to false
459  internalView->setViewing(false);
460 
461  // Turn the viewer decorations
462  internalView->setDecoration(false) ;
463 
464  internalView->resize((int)width, (int)height, true) ;
465 
466  // open the window
467  internalView->show();
468 
469  bufferView = new unsigned char[3*width*height] ;
470 
471 }
472 
473 void
474 vpSimulator::initExternalViewer(const unsigned int width, const unsigned int height)
475 {
476 
477  external_width = width;
478  external_height = height;
479 
480  if (mainWindowInitialized==false)
481  {
483  initSceneGraph() ;
484  }
485 
487 
488  // set the scene to render this view
489  externalView->setSceneGraph(externalRoot);
490 
491  // set the title
492  externalView->setTitle("External View") ;
493  externalView->resize((int)width, (int)height, false) ;
494  // the goal here is to see all the scene and not to determine
495  // a manual viewpoint
496  externalView->viewAll ();
497 
498  // open the window
499  externalView->show();
500 }
501 
502 void
504 {
505  internalCameraParameters = _cam ;
506 
507 
508  float px = (float)_cam.get_px();
509  float py = (float)_cam.get_py();
510  float v = internal_height/(2.f*py);
511 
512  internalCamera->ref() ;
513  internalCamera->heightAngle = 2*atan(v);
514  internalCamera->aspectRatio=(internal_width/internal_height)*(px/py);
515  internalCamera->nearDistance = 0.001f ;
516 
517  internalCamera->farDistance = 1000;
518  internalCamera->unrefNoDelete() ;
519 }
520 
521 void
523 {
524 // SoPerspectiveCamera *camera ;
525 // camera = (SoPerspectiveCamera *)this->externalView->getCamera() ;
526  externalCameraParameters = _cam ;
527 
528  float px = (float)_cam.get_px();
529  float py = (float)_cam.get_py();
530  float v = external_height/(2*py);
531 
532  externalCamera->ref() ;
533  externalCamera->heightAngle = 2*atan(v);
534  externalCamera->aspectRatio=(external_width/external_height)*(px/py);
535  externalCamera->nearDistance = 0.001f ;
536  externalCamera->farDistance = 1000;
537  externalCamera->unrefNoDelete() ;
538 
539 }
540 
541 void
543 {
544 /* SoCamera *camera ;
545  camera = this->externalView->getCamera() ;*/
546  SoSFVec3f position = externalCamera->position ;
547 
548  // get the rotation
549  SoSFRotation orientation = externalCamera->orientation;
550  SbVec3f axis ; float angle ;
551  orientation.getValue(axis,angle) ;
552  SbRotation rotation(axis,angle) ;
553 
554  // get the translation
555  SbVec3f t ;
556  t = position.getValue() ;
557 
558  SbMatrix matrix ;
559  matrix.setRotate(rotation) ;
560 
561  vpHomogeneousMatrix fMc ;
562  SbMatrix rotX;
563  rotX.setRotate (SbRotation (SbVec3f(1.0f, 0.0f, 0.0f), (float)M_PI));
564  matrix.multLeft (rotX);
565  for(unsigned int i=0;i<4;i++)
566  for(unsigned int j=0;j<4;j++)
567  fMc[j][i]=matrix[(int)i][(int)j];
568  fMc[0][3] = t[0] ;
569  fMc[1][3] = t[1] ;
570  fMc[2][3] = t[2] ;
571 
572  cMf = fMc.inverse() ;
573 }
574 
575 
576 void
578 {
580  cMf = _cMf ;
581 }
582 void
584 {
585 
586  SbMatrix matrix;
587  SbRotation rotCam;
588  SbMatrix rotX;
589  rotX.setRotate (SbRotation (SbVec3f(1.0f, 0.0f, 0.0f), (float)M_PI));
590  for(unsigned int i=0;i<4;i++)
591  for(unsigned int j=0;j<4;j++)
592  matrix[(int)j][(int)i]=(float)cMf[i][j];
593 
594  matrix= matrix.inverse();
595  matrix.multLeft (rotX);
596  rotCam.setValue(matrix);
597 
598 
599  internalCamera->ref() ;
600  internalCamera->orientation.setValue(rotCam);
601  internalCamera->position.setValue(matrix[3][0],matrix[3][1],matrix[3][2]);
602  internalCamera->unref() ;
603 
604  rotX.setRotate (SbRotation (SbVec3f(-1.0f, 0.0f, 0.0f), (float)M_PI));
605  matrix.multLeft (rotX);
606  rotCam.setValue(matrix);
607  internalCameraPosition->ref() ;
608  internalCameraPosition->rotation.setValue(rotCam);
609  internalCameraPosition->translation.setValue(matrix[3][0],matrix[3][1],matrix[3][2]);
610  internalCameraPosition->unref() ;
611 }
612 
613 
617 void
619 {
620 
621  // if (this->cameraPositionInitialized==true)
622  {
623  if (this->externalView != NULL)
624  {
625  this->externalView->render() ; //call actualRedraw()
626  // vpHomogeneousMatrix c ;
627  // getExternalCameraPosition(c) ;
628  }
629  if (this->internalView != NULL)
630  {
631  this->moveInternalCamera(this->cMf) ;
632  this->internalView->render() ; //call actualRedraw()
633  }
634  }
635 }
636 
637 // This function is called 20 times each second.
638 static void
639 timerSensorCallback(void *data , SoSensor *)
640 {
641  vpSimulator * simulator = (vpSimulator *)data ;
642 
643  simulator->redraw() ;
644 
645 }
646 
647 
648 void
650 {
651  if (mainWindowInitialized==false)
652  {
653  vpERROR_TRACE("main window is not opened ") ;
654  }
655 
656  vpTime::wait(1000) ;
657 
658  // Timer sensor
659  SoTimerSensor * timer = new SoTimerSensor(timerSensorCallback, (void *)this);
660  timer->setInterval(0.01);
661  timer->schedule();
662  vpViewer::mainLoop() ;
663 }
664 
665 
666 //-----------------------------------------------------------------
667 // scene stuff
668 //-----------------------------------------------------------------
669 
671 void
672 vpSimulator::load(const char *file_name)
673 {
674 
675  SoInput input;
676  if (!input.openFile(file_name))
677  {
678  vpERROR_TRACE("Erreur cannot open file %s",file_name);
679  }
680 
681  SoSeparator *newscene=SoDB::readAll(&input);
682  newscene->ref() ;
683  if (newscene==NULL)
684  {
685  vpERROR_TRACE("Error while reading %s",file_name);
686  }
687 
688  SoScale *taille = new SoScale;
689  taille->scaleFactor.setValue (zoomFactor, zoomFactor, zoomFactor);
690 
691 // newscene->addChild(taille);
692 
693 // std::cout << "this->scene->getNumChildren() = " << this->scene->getNumChildren() << std::endl;
694 
695  this->scene->addChild(taille);
696  this->scene->addChild(newscene);
697  newscene->unref() ;
698 
699 }
700 
701 
702 void
703 vpSimulator::save(const char *name,bool binary)
704 {
705  // get a pointer to the object "name"
706  SoOutput output ;
707  output.openFile(name) ;
708 
709  if (binary==true) output.setBinary(TRUE) ;
710 
711  SoWriteAction writeAction(&output) ;
712  writeAction.apply(scene) ;
713  output.closeFile() ;
714 
715 }
716 
722 void
724 {
725 
726  SoScale *taille = new SoScale;
727  taille->scaleFactor.setValue (zoom, zoom, zoom);
728 
729  SoSeparator * frame = new SoSeparator;
730  frame->ref();
731  frame->addChild(taille);
732  frame->addChild(createFrame (LONGUEUR_FLECHE*zoom, PROPORTION_FLECHE*zoom, RAYON_FLECHE*zoom));
733  this->addObject(frame, fMo, externalRoot) ;
734  // frame->unref();
735 }
736 
742 void
744 {
745  scene->addChild(createFrame (LONGUEUR_FLECHE*zoom, PROPORTION_FLECHE*zoom, RAYON_FLECHE*zoom)) ;
746 }
747 
753 void
754 vpSimulator::load(const char * iv_filename,const vpHomogeneousMatrix &fMo)
755 {
756 
757  SoInput in;
758  SoSeparator * newObject;
759 
760  if (! in.openFile (iv_filename))
761  {
762  vpERROR_TRACE ("Erreur lors de la lecture du fichier %s.", iv_filename);
763  }
764 
765  newObject = SoDB::readAll (&in);
766  if (NULL == newObject)
767  {
768  vpERROR_TRACE ("Problem reading data for file <%s>.", iv_filename);
769  }
770 
771  try
772  {
773  this->addObject (newObject, fMo) ;
774  }
775  catch(...)
776  {
777  vpERROR_TRACE("Error adding object from file <%s> ",iv_filename) ;
778  throw ;
779  }
780 
781 }
782 
783 
789 void
790 vpSimulator::addObject(SoSeparator * newObject, const vpHomogeneousMatrix &fMo)
791 {
792  try
793  {
794  this->addObject(newObject, fMo , scene);
795  }
796  catch(...)
797  {
798  vpERROR_TRACE("Error adding object in scene graph ") ;
799  throw ;
800  }
801 }
802 
803 
804 
805 
806 
814 void
815 vpSimulator::addObject(SoSeparator * object,
816  const vpHomogeneousMatrix &fMo,
817  SoSeparator * root)
818 {
819 
820  bool identity = true ;
821  for (unsigned int i=0 ; i <4 ;i++){
822  for (unsigned int j=0 ; j < 4 ; j++){
823  if (i==j){
824  if (fabs(fMo[i][j] -1) > 1e-6) identity=false ;
825  }
826  else{
827  if (fabs(fMo[i][j]) > 1e-6) identity=false ;
828  }
829  }
830  }
831 
832  if (identity==true)
833  {
834  vpTRACE("identity ") ;
835  root->addChild (object);
836  }
837  else
838  {
839  SbMatrix matrix;
840  SbRotation rotation;
841  for(unsigned int i=0;i<4;i++)
842  for(unsigned int j=0;j<4;j++)
843  matrix[(int)j][(int)i]=(float)fMo[i][j];
844 
845  // matrix= matrix.inverse();
846  rotation.setValue(matrix);
847 
848  SoTransform *displacement = new SoTransform;
849  SoSeparator *newNode = new SoSeparator;
850 
851  displacement->rotation.setValue(rotation);
852  displacement->translation.setValue(matrix[3][0],
853  matrix[3][1],
854  matrix[3][2]);
855 
856  root->addChild (newNode);
857  newNode->addChild (displacement);
858  newNode->addChild (object);
859 
860  }
861 }
862 
863 
865 void
866 vpSimulator::initApplication(void *(*start_routine)(void *))
867 {
868  //pthread_create (&mainThread, NULL, start_routine, (void *)this);
869  mainThread = SbThread::create (start_routine, (void *)this);
870 }
871 
881 void
882 vpSimulator::initApplication(void *(*start_routine)(void *), void* data)
883 {
884  mainThread = SbThread::create (start_routine, (void *)data);
885 }
886 
889 void
891 {
892  //pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL );
893  //pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
894  vpTime::wait(1000) ;
895 }
898 void
900 {
901  vpViewer::exitMainLoop() ;
902  //pthread_exit (NULL);
903 }
904 
905 
906 
907 
908 
909 /* Initialise le SoOffScreenRenderer si necessaire, puis realise le rendu.
910  * Quand la fonction rend la main, le buffer est pret et n'a plus qu'a etre
911  * enregistre ou passe a l'utilisateur.
912  * INPUT:
913  * - vueInterne est vrai ssi il faut rendre la vue interne, faux ssi
914  * il faut rendre la vue externe.
915  * OUTPUT:
916  * - width : largeur de l'image dans le buffer.
917  * - height : hauteur de l'image dans le buffer.
918  */
919 void
920 vpSimulator::offScreenRendering(vpSimulatorViewType view, int * width, int * height)
921 {
922 
923  SbVec2s size(320,200);
924  SoNode * thisroot;
925 
926  {
927  if (view==vpSimulator::INTERNAL)
928  {
929  size = this ->internalView ->getViewportRegion().getWindowSize();
930  thisroot = this ->internalView->getSceneManager()->getSceneGraph() ;
931  }
932  else
933  {
934  size = this ->externalView ->getViewportRegion().getWindowSize();
935  thisroot = this ->externalView->getSceneManager()->getSceneGraph() ;
936  }
937  }
938  SbViewportRegion myViewPort(size);
939 
940  // Creation du rendu si necessaire.
941  if (NULL == this ->offScreenRenderer)
942  {
943  //Init du SoOffscreenRenderer
944  this ->offScreenRenderer = new SoOffscreenRenderer(myViewPort);
945  }
946  else
947  {
948  // Redefini le view port
949  this ->offScreenRenderer ->setViewportRegion (myViewPort);
950  }
951 
952  // Rendu offscreen
953  if (! this ->offScreenRenderer ->render(thisroot))
954  {
955  vpERROR_TRACE("La scene n'a pas pu etre rendue offscreen.");
956  delete this ->offScreenRenderer;
957  this ->offScreenRenderer = NULL;
958  }
959  else
960  {
961 
962 
963  /*
964  if (view==vpSimulator::INTERNAL)
965  {
966  //Recopie du buffer contenant l'image, dans bufferView
967  int length = 3*size [0]*size[1];
968  delete [] bufferView;
969  bufferView = new unsigned char [length];
970  for(int i=0; i<length; i++)
971  {
972  bufferView[i] = this ->offScreenRenderer->getBuffer()[i];
973  }
974  }*/
975 
976  }
977 
978  // exit(1) ;
979  if (NULL != width) { * width = size [0]; }
980  if (NULL != height) { * height = size [1]; }
981 
982 
983 }
984 
985 
986 /* Enregistre l'image de vue interne ou externe dans un fichier RGB.
987  * Effectue le rendu dans un buffer plutot qu'a l'ecran, puis sauvegarde
988  * ce buffer au format PS (copie directe).
989  * INPUT
990  * - fileName: nom du fichier dans lequel placer le resultat.
991  * OUTPUT
992  * - RETURN : Code d'erreur CODE_OK si tout s'est bien passe.
993  */
994 
995 void
996 vpSimulator::write (const char * fileName)
997 {
998 
999  while (get==0) { vpTRACE("%d ",get); }
1000  get =2 ;
1001  /* FILE *fp = fopen(fileName, "w");
1002  fprintf(fp,"P6 \n %d %d \n 255",internal_width,internal_height) ;
1003  fwrite(bufferView, sizeof(unsigned char), internal_width*internal_height*3, fp) ;*/
1005 
1006 
1007  for(unsigned int i=0 ; i < internal_height ; i++)
1008  for(unsigned int j=0 ; j < internal_width ; j++)
1009  {
1010  unsigned char r,g,b ;
1011  unsigned int index = 3*((internal_height-i-1)* internal_width + j );
1012  r = *(bufferView+index);
1013  g = *(bufferView+index+1);
1014  b = *(bufferView+index+2);
1015  I[i][j].R =r ;
1016  I[i][j].G =g ;
1017  I[i][j].B =b ;
1018  }
1019  vpImageIo::write(I,fileName) ;
1020  // fclose (fp);
1021  get =1 ;
1022 }
1023 
1024 void
1025 vpSimulator::getSizeInternalView(int& width, int& height)
1026 {
1027  SbVec2s size = this ->internalView ->getViewportRegion().getWindowSize();
1028  width = size [0];
1029  height = size[1];
1030 }
1031 
1037 void
1039 {
1040  //while (get==0) {;}
1041  get =2 ;
1044  get =1 ;
1045 }
1046 
1051 void
1053 {
1054  //while (get==0) {;}
1055  get =2 ;
1058  get =1 ;
1059 }
1060 
1061 #endif
1062 
1063 /*
1064  * Local variables:
1065  * c-basic-offset: 2
1066  * End:
1067  */
HWND mainWindow
main Widget
Definition: vpSimulator.h:118
SoSeparator * externalRoot
root node of the external view
Definition: vpSimulator.h:195
static void write(const vpImage< unsigned char > &I, const char *filename)
Definition: vpImageIo.cpp:442
#define vpDEBUG_TRACE
Definition: vpDebug.h:454
virtual void initInternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the camera view
void resize(int x, int y, bool fixed=false)
Definition: vpViewer.cpp:148
void changeZoomFactor(const float zoom, const int index)
Change the zoom factor associated to the child given by index. In order to create multiple zoom facto...
SoPerspectiveCamera * internalCamera
internal camera
Definition: vpSimulator.h:198
bool cameraPositionInitialized
Definition: vpSimulator.h:244
static void RGBToGrey(unsigned char *rgb, unsigned char *grey, unsigned int size)
void write(const char *fileName)
SoTransform * internalCameraPosition
internal camera position
Definition: vpSimulator.h:203
void kill()
perform some destruction
void setExternalCameraParameters(vpCameraParameters &cam)
set external camera parameters
void setCameraPosition(vpHomogeneousMatrix &cMf)
set the camera position (from an homogeneous matrix)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpERROR_TRACE
Definition: vpDebug.h:379
void addObject(SoSeparator *object, const vpHomogeneousMatrix &fMo, SoSeparator *root)
Add a new object in the scene graph ad a given location.
static void RGBToRGBa(unsigned char *rgb, unsigned char *rgba, unsigned int size)
#define vpTRACE
Definition: vpDebug.h:401
Type * bitmap
points toward the bitmap
Definition: vpImage.h:120
Implementation of a simulator based on Coin3d (www.coin3d.org).
Definition: vpSimulator.h:102
vpCameraParameters internalCameraParameters
internal camera parameters
Definition: vpSimulator.h:248
void closeMainApplication()
void resize(const unsigned int height, const unsigned int width)
set the size of the image
Definition: vpImage.h:535
void getSizeInternalView(int &width, int &height)
get the size of the internal view
void addAbsoluteFrame(float zoom=1)
Add the representation of the absolute frame.
unsigned int external_height
Definition: vpSimulator.h:172
void getExternalCameraPosition(vpHomogeneousMatrix &cMf)
get the external camera position
double get_py() const
void init()
perform some initialization
virtual void mainLoop()
activate the mainloop
static int wait(double t0, double t)
Definition: vpTime.cpp:149
bool mainWindowInitialized
Definition: vpSimulator.h:125
vpViewer * internalView
view from the camera
Definition: vpSimulator.h:143
void moveInternalCamera(vpHomogeneousMatrix &cMf)
modify the position of the camera in the scene graph
SoSeparator * scene
Definition: vpSimulator.h:191
void getInternalImage(vpImage< unsigned char > &I)
get an Image of the internal view
SoSeparator * internalRoot
root node of the internal view
Definition: vpSimulator.h:193
vpCameraParameters externalCameraParameters
internal camera parameters
Definition: vpSimulator.h:250
SbTime * realtime
Definition: vpSimulator.h:277
void initApplication(void *(*start_routine)(void *))
begin the main program
GLubyte * image_background
Definition: vpSimulator.h:137
void setInternalCameraParameters(vpCameraParameters &cam)
set internal camera parameters
Viewer used by the simulator.
Definition: vpViewer.h:120
Generic class defining intrinsic camera parameters.
void initSceneGraph()
initialize the scene graph
void load(const char *file_name)
load an iv file
void offScreenRendering(vpSimulatorViewType view=vpSimulator::EXTERNAL, int *width=NULL, int *height=NULL)
double get_px() const
vpViewer * externalView
view from an external camera
Definition: vpSimulator.h:145
void initSoApplication()
open the SoGui application
SoPerspectiveCamera * externalCamera
external camera
Definition: vpSimulator.h:200
unsigned int external_width
Definition: vpSimulator.h:171
vpHomogeneousMatrix cMf
internal camera position
Definition: vpSimulator.h:246
void initMainApplication()
perform some initialization in the main program thread
void addFrame(const vpHomogeneousMatrix &fMo, float zoom=1)
Add the representation of a frame.
unsigned char * bufferView
image of the internal view
Definition: vpSimulator.h:284
unsigned int internal_width
Definition: vpSimulator.h:169
vpHomogeneousMatrix inverse() const
vpSimulator()
constructor
void redraw()
display the scene (handle with care)
void initExternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the external view
SoOffscreenRenderer * offScreenRenderer
Definition: vpSimulator.h:278
void setZoomFactor(const float zoom)
set the size of the camera/frame
virtual ~vpSimulator()
unsigned int internal_height
Definition: vpSimulator.h:170
void save(const char *name, bool binary=false)
save the scene in an iv file
float zoomFactor
Definition: vpSimulator.h:240
SoSeparator * internalCameraObject
representation of the camera in the external view
Definition: vpSimulator.h:209
SbThread * mainThread
thread with the main program
Definition: vpSimulator.h:155