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