Visual Servoing Platform  version 3.4.0
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(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 
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 
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(unsigned int width, 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(unsigned int width, 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
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:173
void resize(int x, int y, bool fixed=false)
Definition: vpViewer.cpp:130
SoPerspectiveCamera * internalCamera
internal camera
Definition: vpSimulator.h:192
bool cameraPositionInitialized
Definition: vpSimulator.h:237
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)
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:800
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:143
#define vpERROR_TRACE
Definition: vpDebug.h:393
Implementation of a simulator based on Coin3d (www.coin3d.org).
Definition: vpSimulator.h:99
void setZoomFactor(float zoom)
set the size of the camera/frame
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:445
SoSeparator * internalRoot
root node of the internal view
Definition: vpSimulator.h:187
vpCameraParameters externalCameraParameters
internal camera parameters
Definition: vpSimulator.h:243
static void RGBToGrey(unsigned char *rgb, unsigned char *grey, unsigned int width, unsigned int height, bool flip=false)
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.
virtual void initInternalViewer(unsigned int nlig, unsigned int ncol)
initialize the camera view
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 changeZoomFactor(float zoom, int index)
Change the zoom factor associated to the child given by index. In order to create multiple zoom facto...
void initExternalViewer(unsigned int nlig, unsigned int ncol)
initialize the external view
SoOffscreenRenderer * offScreenRenderer
Definition: vpSimulator.h:274
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