Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
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 = nullptr;
276  externalView = nullptr;
277  image_background = nullptr;
278 
279  zoomFactor = 1;
281 
282  // write image process
283  realtime = nullptr;
284  offScreenRenderer = nullptr;
285  bufferView = nullptr;
286  get = 1;
288  mainThread = nullptr;
289  scene = nullptr;
290  internalRoot = nullptr;
291  externalRoot = nullptr;
292  internalCamera = nullptr;
293  externalCamera = nullptr;
294  internalCameraPosition = nullptr;
295  extrenalCameraPosition = nullptr;
296  internalCameraObject = nullptr;
297 #if defined(VISP_HAVE_SOWIN)
298 // mainWindow = ?;
299 #elif defined(VISP_HAVE_SOQT)
300  mainWindow = nullptr;
301 #elif defined(VISP_HAVE_SOXT)
302 // mainWindow = ?;
303 #endif
304 }
306 {
307  if (internalView != nullptr) {
308  delete internalView;
309  internalView = nullptr;
310  }
311  if (externalView != nullptr) {
312  delete externalView;
313  externalView = nullptr;
314  }
315  if (bufferView != nullptr) {
316  delete[] bufferView;
317  bufferView = nullptr;
318  }
319  if (image_background != nullptr) {
320  free(image_background);
321  image_background = nullptr;
322  }
323 }
324 
326  :
327 #if defined(VISP_HAVE_SOWIN)
328  mainWindow(),
329 #elif defined(VISP_HAVE_SOQT)
330  mainWindow(nullptr),
331 #elif defined(VISP_HAVE_SOXT)
332  mainWindow(),
333 #endif
334  mainWindowInitialized(false), typeImage(vpSimulator::grayImage), image_background(nullptr), internalView(nullptr),
335  externalView(nullptr), mainThread(nullptr), internal_width(0), internal_height(0), external_width(0), external_height(0),
336  scene(nullptr), internalRoot(nullptr), externalRoot(nullptr), internalCamera(nullptr), externalCamera(nullptr),
337  internalCameraPosition(nullptr), extrenalCameraPosition(nullptr), internalCameraObject(nullptr), zoomFactor(0.),
338  cameraPositionInitialized(false), cMf(), internalCameraParameters(), externalCameraParameters(), realtime(nullptr),
339  offScreenRenderer(nullptr), bufferView(nullptr), 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 == nullptr) {
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  }
419  else {
420  SoScale *taille = (SoScale *)this->scene->getChild(0);
421  taille->scaleFactor.setValue(zoomFactor, zoomFactor, zoomFactor);
422  }
423 }
424 
443 void vpSimulator::changeZoomFactor(float zoomFactor, int index)
444 {
445  SoScale *taille = (SoScale *)this->scene->getChild(index);
446  taille->scaleFactor.setValue(zoomFactor, zoomFactor, zoomFactor);
447  // this->setZoomFactor(zoomFactor);
448 }
449 
450 void vpSimulator::initInternalViewer(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 != nullptr) {
623  this->externalView->render(); // call actualRedraw()
624  // vpHomogeneousMatrix c ;
625  // getExternalCameraPosition(c) ;
626  }
627  if (this->internalView != nullptr) {
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 == nullptr) {
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 (nullptr == newObject) {
749  vpERROR_TRACE("Problem reading data for file <%s>.", iv_filename);
750  }
751 
752  try {
753  this->addObject(newObject, fMo);
754  }
755  catch (...) {
756  vpERROR_TRACE("Error adding object from file <%s> ", iv_filename);
757  throw;
758  }
759 }
760 
766 void vpSimulator::addObject(SoSeparator *newObject, const vpHomogeneousMatrix &fMo)
767 {
768  try {
769  this->addObject(newObject, fMo, scene);
770  }
771  catch (...) {
772  vpERROR_TRACE("Error adding object in scene graph ");
773  throw;
774  }
775 }
776 
784 void vpSimulator::addObject(SoSeparator *object, const vpHomogeneousMatrix &fMo, SoSeparator *root)
785 {
786 
787  bool identity = true;
788  for (unsigned int i = 0; i < 4; i++) {
789  for (unsigned int j = 0; j < 4; j++) {
790  if (i == j) {
791  if (fabs(fMo[i][j] - 1) > 1e-6)
792  identity = false;
793  }
794  else {
795  if (fabs(fMo[i][j]) > 1e-6)
796  identity = false;
797  }
798  }
799  }
800 
801  if (identity == true) {
802  root->addChild(object);
803  }
804  else {
805  SbMatrix matrix;
806  SbRotation rotation;
807  for (unsigned int i = 0; i < 4; i++)
808  for (unsigned int j = 0; j < 4; j++)
809  matrix[(int)j][(int)i] = (float)fMo[i][j];
810 
811  // matrix= matrix.inverse();
812  rotation.setValue(matrix);
813 
814  SoTransform *displacement = new SoTransform;
815  SoSeparator *newNode = new SoSeparator;
816 
817  displacement->rotation.setValue(rotation);
818  displacement->translation.setValue(matrix[3][0], matrix[3][1], matrix[3][2]);
819 
820  root->addChild(newNode);
821  newNode->addChild(displacement);
822  newNode->addChild(object);
823  }
824 }
825 
827 void vpSimulator::initApplication(void *(*start_routine)(void *))
828 {
829  // pthread_create (&mainThread, nullptr, start_routine, (void *)this);
830  mainThread = SbThread::create(start_routine, (void *)this);
831 }
832 
842 void vpSimulator::initApplication(void *(*start_routine)(void *), void *data)
843 {
844  mainThread = SbThread::create(start_routine, (void *)data);
845 }
846 
850 {
851  // pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, nullptr );
852  // pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, nullptr);
853  vpTime::wait(1000);
854 }
858 {
859  vpViewer::exitMainLoop();
860  // pthread_exit (nullptr);
861 }
862 
863 /* Initialise le SoOffScreenRenderer si necessaire, puis realise le rendu.
864  * Quand la fonction rend la main, le buffer est pret et n'a plus qu'a etre
865  * enregistre ou passe a l'utilisateur.
866  * INPUT:
867  * - vueInterne est vrai ssi il faut rendre la vue interne, faux ssi
868  * il faut rendre la vue externe.
869  * OUTPUT:
870  * - width : largeur de l'image dans le buffer.
871  * - height : hauteur de l'image dans le buffer.
872  */
873 void vpSimulator::offScreenRendering(vpSimulatorViewType view, int *width, int *height)
874 {
875 
876  SbVec2s size(320, 200);
877  SoNode *thisroot;
878 
879  {
880  if (view == vpSimulator::INTERNAL) {
881  size = this->internalView->getViewportRegion().getWindowSize();
882  thisroot = this->internalView->getSceneManager()->getSceneGraph();
883  }
884  else {
885  size = this->externalView->getViewportRegion().getWindowSize();
886  thisroot = this->externalView->getSceneManager()->getSceneGraph();
887  }
888  }
889  SbViewportRegion myViewPort(size);
890 
891  // Creation du rendu si necessaire.
892  if (nullptr == this->offScreenRenderer) {
893  // Init du SoOffscreenRenderer
894  this->offScreenRenderer = new SoOffscreenRenderer(myViewPort);
895  }
896  else {
897  // Redefini le view port
898  this->offScreenRenderer->setViewportRegion(myViewPort);
899  }
900 
901  // Rendu offscreen
902  if (!this->offScreenRenderer->render(thisroot)) {
903  vpERROR_TRACE("La scene n'a pas pu etre rendue offscreen.");
904  delete this->offScreenRenderer;
905  this->offScreenRenderer = nullptr;
906  }
907  else {
908 
909  /*
910  if (view==vpSimulator::INTERNAL)
911  {
912  //Recopie du buffer contenant l'image, dans bufferView
913  int length = 3*size [0]*size[1];
914  delete [] bufferView;
915  bufferView = new unsigned char [length];
916  for(int i=0; i<length; i++)
917  {
918  bufferView[i] = this ->offScreenRenderer->getBuffer()[i];
919  }
920  }*/
921  }
922 
923  // exit(1) ;
924  if (nullptr != width) {
925  *width = size[0];
926  }
927  if (nullptr != height) {
928  *height = size[1];
929  }
930 }
931 
932 /* Enregistre l'image de vue interne ou externe dans un fichier RGB.
933  * Effectue le rendu dans un buffer plutot qu'a l'ecran, puis sauvegarde
934  * ce buffer au format PS (copie directe).
935  * INPUT
936  * - fileName: nom du fichier dans lequel placer le resultat.
937  * OUTPUT
938  * - RETURN : Code d'erreur CODE_OK si tout s'est bien passe.
939  */
940 
941 #ifdef VISP_HAVE_MODULE_IO
942 void vpSimulator::write(const char *fileName)
943 {
944 
945  while (get == 0) {
946  vpTRACE("%d ", get);
947  }
948  get = 2;
949  /* FILE *fp = fopen(fileName, "w");
950  fprintf(fp,"P6 \n %d %d \n 255",internal_width,internal_height) ;
951  fwrite(bufferView, sizeof(unsigned char),
952  internal_width*internal_height*3, fp) ;*/
954 
955  for (unsigned int i = 0; i < internal_height; i++)
956  for (unsigned int j = 0; j < internal_width; j++) {
957  unsigned char r, g, b;
958  unsigned int index = 3 * ((internal_height - i - 1) * internal_width + j);
959  r = *(bufferView + index);
960  g = *(bufferView + index + 1);
961  b = *(bufferView + index + 2);
962  I[i][j].R = r;
963  I[i][j].G = g;
964  I[i][j].B = b;
965  }
966  vpImageIo::write(I, fileName);
967  // fclose (fp);
968  get = 1;
969 }
970 #endif
971 
972 void vpSimulator::getSizeInternalView(int &width, int &height)
973 {
974  SbVec2s size = this->internalView->getViewportRegion().getWindowSize();
975  width = size[0];
976  height = size[1];
977 }
978 
985 {
986  // while (get==0) {;}
987  get = 2;
990  get = 1;
991 }
992 
998 {
999  // while (get==0) {;}
1000  get = 2;
1003  get = 1;
1004 }
1005 
1006 #elif !defined(VISP_BUILD_SHARED_LIBS)
1007 // Work around to avoid warning: libvisp_ar.a(vpSimulator.cpp.o) has no symbols
1008 void dummy_vpSimulator() { };
1009 #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:780
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 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: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
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:405
#define vpDEBUG_TRACE
Definition: vpDebug.h:473
#define vpERROR_TRACE
Definition: vpDebug.h:382
VISP_EXPORT int wait(double t0, double t)