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