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