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