Visual Servoing Platform version 3.5.0
vpSimulator.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Simulator based on Coin3d.
33 *
34 * Authors:
35 * Eric Marchand
36 * Anthony Saunier
37 *
38 *****************************************************************************/
45#include <visp3/core/vpConfig.h>
46
47#ifdef VISP_HAVE_COIN3D_AND_GUI
48
49#include <visp3/ar/vpSimulator.h>
50#include <visp3/core/vpTime.h>
51
52#include <visp3/core/vpImage.h>
53
54#ifdef VISP_HAVE_MODULE_IO
55#include <visp3/io/vpImageIo.h>
56#endif
57
58/* Objets OIV. */
59#include <Inventor/nodes/SoCone.h> /* Objet cone. */
60#include <Inventor/nodes/SoCoordinate3.h> /* Liste de points. */
61#include <Inventor/nodes/SoCylinder.h> /* Objet cylindre. */
62#include <Inventor/nodes/SoIndexedFaceSet.h> /* Liste de face. */
63#include <Inventor/nodes/SoPointLight.h> /* Objet lumiere ponctuelle. */
64#include <Inventor/nodes/SoRotationXYZ.h> /* Transfo rotation simple. */
65#include <Inventor/nodes/SoScale.h> /* Trasnfo mise a l'echelle. */
66#include <Inventor/nodes/SoTranslation.h> /* Trasnfo translation. */
67
68#include <Inventor/actions/SoWriteAction.h>
69#include <Inventor/nodes/SoDirectionalLight.h> /* Objet lumiere directionnelle*/
70#include <Inventor/nodes/SoDrawStyle.h> /* Style de rendu. */
71#include <Inventor/nodes/SoEnvironment.h> /* Eclairage ambiant. */
72#include <Inventor/nodes/SoGroup.h> /* Groupement de noeuds (sans separation)*/
73#include <Inventor/nodes/SoMaterial.h> /* Matiere (couleur) des objets. */
74
75// Positions of all of the vertices:
76//
77static float pyramidVertexes[5][3] = {{0.33f, 0.33f, 0.f},
78 {-0.33f, 0.33f, 0.f},
79 {-0.33f, -0.33f, 0.f},
80 {0.33f, -0.33f, 0.f},
81
82 {0.f, 0.f, -1.0f}};
83
84static int32_t pyramidFaces[] = {
85 0,
86 1,
87 2,
88 3,
89 SO_END_FACE_INDEX, // top face
90
91 0,
92 1,
93 4,
94 SO_END_FACE_INDEX, // 4 faces about top
95 1,
96 2,
97 4,
98 SO_END_FACE_INDEX,
99 2,
100 3,
101 4,
102 SO_END_FACE_INDEX,
103 3,
104 0,
105 4,
106 SO_END_FACE_INDEX,
107};
108
109// Routine to create a scene graph representing a dodecahedron
110SoSeparator *makePyramide()
111{
112 SoSeparator *result = new SoSeparator;
113 result->ref();
114
115 // Define coordinates for vertices
116 SoCoordinate3 *myCoords = new SoCoordinate3;
117 myCoords->point.setValues(0, 5, pyramidVertexes);
118 result->addChild(myCoords);
119
120 // Define the IndexedFaceSet, with indices into the vertices:
121 SoIndexedFaceSet *myFaceSet = new SoIndexedFaceSet;
122 myFaceSet->coordIndex.setValues(0, 21, (const int32_t *)pyramidFaces);
123 result->addChild(myFaceSet);
124
125 result->unrefNoDelete();
126 return result;
127}
128
129/* Cree une fleche composee d'un cylindre et d'un cone.
130 * La fleche a une hauteur total de <longueur>, dont
131 * <proportionFleche>% pour la fleche. Le rayon du cylindre
132 * est <radius>, et celui de la fleche <radius> * 5.
133 * La fleche est oriente selon l'axe Y.
134 */
135static SoSeparator *createArrow(float longueur, float proportionFleche, float radius)
136{
137 SoSeparator *fleche = new SoSeparator;
138 fleche->ref();
139
140 SoTranslation *poseCylindre = new SoTranslation;
141 SoCylinder *line = new SoCylinder;
142 SoTranslation *posePointe = new SoTranslation;
143 SoCone *pointe = new SoCone;
144
145 float l_cylindre = longueur * (1 - proportionFleche);
146 float l_cone = longueur * proportionFleche;
147 float radius_cylindre = radius;
148 float radius_cone = radius * 5;
149
150 line->radius.setValue(radius_cylindre);
151 line->height.setValue(l_cylindre);
152
153 poseCylindre->translation.setValue(0, l_cylindre / 2, 0);
154 posePointe->translation.setValue(0.0, l_cylindre / 2 + l_cone / 2, 0);
155
156 pointe->bottomRadius.setValue(radius_cone);
157 pointe->height.setValue(l_cone);
158
159 fleche->addChild(poseCylindre);
160 fleche->addChild(line);
161 fleche->addChild(posePointe);
162 fleche->addChild(pointe);
163
164 return fleche;
165}
166
167/*
168 Cree un objet repere dans un noeud separator, et le renvoie.
169 \return : code d'erreur, SIMU_CODE_OK si tout s'est bien passe.
170*/
171#define LONGUEUR_FLECHE 1.0f
172#define RAYON_FLECHE 0.002f
173#define PROPORTION_FLECHE 0.1f
174
175SoSeparator *createFrame(float longueurFleche = LONGUEUR_FLECHE, float proportionFleche = PROPORTION_FLECHE,
176 float radiusFleche = RAYON_FLECHE)
177{
178 vpDEBUG_TRACE(15, "# Entree.");
179
180 SoSeparator *frame = new SoSeparator;
181 frame->ref();
182
183 SoRotationXYZ *rotationY_X = new SoRotationXYZ;
184 rotationY_X->axis = SoRotationXYZ::Z;
185 rotationY_X->angle.setValue((float)(-M_PI / 2));
186
187 SoRotationXYZ *rotationX_Y = new SoRotationXYZ;
188 rotationX_Y->axis = SoRotationXYZ::Z;
189 rotationX_Y->angle.setValue((float)(M_PI / 2));
190
191 SoRotationXYZ *rotationY_Z = new SoRotationXYZ;
192 rotationY_Z->axis = SoRotationXYZ::X;
193 rotationY_Z->angle.setValue((float)(M_PI / 2));
194
195 SoMaterial *rouge = new SoMaterial;
196 rouge->diffuseColor.setValue(1.0, 0.0, 0.0);
197 rouge->emissiveColor.setValue(0.5, 0.0, 0.0);
198
199 SoMaterial *vert = new SoMaterial;
200 vert->diffuseColor.setValue(0.0, 1.0, 0.0);
201 vert->emissiveColor.setValue(0.0, 0.5, 0.0);
202
203 SoMaterial *bleu = new SoMaterial;
204 bleu->diffuseColor.setValue(0.0, 0.0, 1.0);
205 bleu->emissiveColor.setValue(0.0, 0.0, 0.5);
206
207 SoSeparator *fleche = createArrow(longueurFleche, proportionFleche, radiusFleche);
208
209 frame->addChild(rouge);
210 frame->addChild(rotationY_X);
211 frame->addChild(fleche);
212 frame->addChild(vert);
213 frame->addChild(rotationX_Y);
214 frame->addChild(fleche);
215 frame->addChild(bleu);
216 frame->addChild(rotationY_Z);
217 frame->addChild(fleche);
218
219 frame->unrefNoDelete();
220
221 vpDEBUG_TRACE(15, "# Sortie.");
222 return frame;
223}
224
225SoSeparator *createCameraObject(float zoomFactor = 1.0)
226{
227 vpDEBUG_TRACE(15, "# Entree.");
228
229 SoSeparator *cam = new SoSeparator;
230 cam->ref();
231
232 SoMaterial *myMaterial = new SoMaterial;
233 myMaterial->diffuseColor.setValue(1.0, 0.0, 0.0);
234 myMaterial->emissiveColor.setValue(0.5, 0.0, 0.0);
235
236 SoScale *taille = new SoScale;
237 {
238 float zoom = 0.1f * zoomFactor;
239 taille->scaleFactor.setValue(zoom, zoom, zoom);
240 }
241
242 SoMaterial *couleurBlanc = new SoMaterial;
243 couleurBlanc->diffuseColor.setValue(1.0, 1.0, 1.0);
244 couleurBlanc->emissiveColor.setValue(1.0, 1.0, 1.0);
245 SoDrawStyle *filDeFer = new SoDrawStyle;
246 filDeFer->style.setValue(SoDrawStyle::LINES);
247 filDeFer->lineWidth.setValue(1);
248
249 SoSeparator *cone = new SoSeparator;
250 cone->ref();
251 cone->addChild(makePyramide());
252 cone->addChild(couleurBlanc);
253 cone->addChild(filDeFer);
254 cone->addChild(makePyramide());
255 cone->unrefNoDelete();
256
257 cam->addChild(myMaterial);
258 cam->addChild(taille);
259 cam->addChild(cone);
260 cam->addChild(createFrame(2.0f, 0.1f, 0.01f));
261
262 // cam->unref() ;
263 vpDEBUG_TRACE(15, "# Sortie.");
264 return cam;
265}
266
267//--------------------------------------------------------------
269{
270 internal_width = 200;
271 internal_height = 200;
272 external_width = 200;
273 external_height = 200;
274
275 mainWindowInitialized = false;
276 internalView = NULL;
277 externalView = NULL;
278 image_background = NULL;
279
280 zoomFactor = 1;
282
283 // write image process
284 realtime = NULL;
285 offScreenRenderer = NULL;
286 bufferView = NULL;
287 get = 1;
289 mainThread = NULL;
290 scene = NULL;
291 internalRoot = NULL;
292 externalRoot = NULL;
293 internalCamera = NULL;
294 externalCamera = NULL;
298#if defined(VISP_HAVE_SOWIN)
299// mainWindow = ?;
300#elif defined(VISP_HAVE_SOQT)
301 mainWindow = NULL;
302#elif defined(VISP_HAVE_SOXT)
303// mainWindow = ?;
304#endif
305}
307{
308 if (internalView != NULL) {
309 delete internalView;
310 internalView = NULL;
311 }
312 if (externalView != NULL) {
313 delete externalView;
314 externalView = NULL;
315 }
316 if (bufferView != NULL) {
317 delete[] bufferView;
318 bufferView = NULL;
319 }
320 if (image_background != NULL) {
321 free(image_background);
322 image_background = NULL;
323 }
324}
325
327 :
328#if defined(VISP_HAVE_SOWIN)
329 mainWindow(),
330#elif defined(VISP_HAVE_SOQT)
331 mainWindow(NULL),
332#elif defined(VISP_HAVE_SOXT)
333 mainWindow(),
334#endif
335 mainWindowInitialized(false), typeImage(vpSimulator::grayImage), image_background(NULL), internalView(NULL),
336 externalView(NULL), mainThread(NULL), internal_width(0), internal_height(0), external_width(0), external_height(0),
337 scene(NULL), internalRoot(NULL), externalRoot(NULL), internalCamera(NULL), externalCamera(NULL),
338 internalCameraPosition(NULL), extrenalCameraPosition(NULL), internalCameraObject(NULL), zoomFactor(0.),
339 cameraPositionInitialized(false), cMf(), internalCameraParameters(), externalCameraParameters(), realtime(NULL),
340 offScreenRenderer(NULL), bufferView(NULL), get(0)
341{
343}
344
346
348{
349 mainWindow = vpViewer::init("");
351}
352
354{
355 this->scene = new SoSeparator;
356 this->internalRoot = new SoSeparator;
357 this->externalRoot = new SoSeparator;
358
359 this->scene->ref();
360 this->internalRoot->ref();
361 this->externalRoot->ref();
362
363 // define the camera SoPerspectiveCamera
364 this->internalCamera = new SoPerspectiveCamera;
365 this->externalCamera = new SoPerspectiveCamera;
366
367 this->internalCameraPosition = new SoTransform;
368 this->internalCameraObject = createCameraObject(zoomFactor);
369
370 internalCamera->farDistance.setValue(100);
371 internalCamera->nearDistance.setValue(0.0001f);
372
373 // link between camera and internal root
374 this->internalRoot->addChild(this->internalCamera);
375 this->internalRoot->addChild(this->scene);
376
377 this->externalRoot->addChild(this->externalCamera);
378 this->externalRoot->addChild(this->scene);
379
380 SoSeparator *camera = new SoSeparator;
381 camera->ref();
382 camera->addChild(this->internalCameraPosition);
383 camera->addChild(this->internalCameraObject);
384 this->externalRoot->addChild(camera);
385
386 // this->externalRoot->addChild (internalCameraPosition);
387 // this->externalRoot->addChild (internalCameraObject);
388 SoCube *cube = new SoCube;
389 cube->width = 0.01f;
390 cube->depth = 0.01f;
391 cube->height = 0.01f;
392
393 this->externalRoot->addChild(cube);
394
395 if (realtime == NULL) {
396
397 SoDB::enableRealTimeSensor(FALSE);
398 SoSceneManager::enableRealTimeUpdate(FALSE);
399 realtime = (SbTime *)SoDB::getGlobalField("realTime");
400 realtime->setValue(0.0);
401 }
402}
403
411{
412 zoomFactor = zoom;
413 static bool firstTime = true;
414 if (firstTime) {
415 SoScale *taille = new SoScale;
416 taille->scaleFactor.setValue(zoomFactor, zoomFactor, zoomFactor);
417 this->scene->addChild(taille);
418 firstTime = false;
419 } else {
420 SoScale *taille = (SoScale *)this->scene->getChild(0);
421 taille->scaleFactor.setValue(zoomFactor, zoomFactor, zoomFactor);
422 }
423}
424
443void 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
450void vpSimulator::initInternalViewer(unsigned int width, unsigned int height)
451{
452 internal_width = width;
453 internal_height = height;
454
455 if (mainWindowInitialized == false) {
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
484void vpSimulator::initExternalViewer(unsigned int width, unsigned int height)
485{
486
487 external_width = width;
488 external_height = height;
489
490 if (mainWindowInitialized == false) {
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);
609 internalCameraPosition->rotation.setValue(rotCam);
610 internalCameraPosition->translation.setValue(matrix[3][0], matrix[3][1], matrix[3][2]);
611 internalCameraPosition->unref();
612}
613
618{
619
620 // if (this->cameraPositionInitialized==true)
621 {
622 if (this->externalView != NULL) {
623 this->externalView->render(); // call actualRedraw()
624 // vpHomogeneousMatrix c ;
625 // getExternalCameraPosition(c) ;
626 }
627 if (this->internalView != NULL) {
628 this->moveInternalCamera(this->cMf);
629 this->internalView->render(); // call actualRedraw()
630 }
631 }
632}
633
634// This function is called 20 times each second.
635static 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
662void vpSimulator::load(const char *file_name)
663{
664
665 SoInput input;
666 if (!input.openFile(file_name)) {
667 vpERROR_TRACE("Erreur cannot open file %s", file_name);
668 }
669
670 SoSeparator *newscene = SoDB::readAll(&input);
671 newscene->ref();
672 if (newscene == NULL) {
673 vpERROR_TRACE("Error while reading %s", file_name);
674 }
675
676 SoScale *taille = new SoScale;
677 taille->scaleFactor.setValue(zoomFactor, zoomFactor, zoomFactor);
678
679 // newscene->addChild(taille);
680
681 // std::cout << "this->scene->getNumChildren() = " <<
682 // this->scene->getNumChildren() << std::endl;
683
684 this->scene->addChild(taille);
685 this->scene->addChild(newscene);
686 newscene->unref();
687}
688
689void 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
708void 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
737void vpSimulator::load(const char *iv_filename, const vpHomogeneousMatrix &fMo)
738{
739
740 SoInput in;
741 SoSeparator *newObject;
742
743 if (!in.openFile(iv_filename)) {
744 vpERROR_TRACE("Erreur lors de la lecture du fichier %s.", iv_filename);
745 }
746
747 newObject = SoDB::readAll(&in);
748 if (NULL == newObject) {
749 vpERROR_TRACE("Problem reading data for file <%s>.", iv_filename);
750 }
751
752 try {
753 this->addObject(newObject, fMo);
754 } catch (...) {
755 vpERROR_TRACE("Error adding object from file <%s> ", iv_filename);
756 throw;
757 }
758}
759
765void vpSimulator::addObject(SoSeparator *newObject, const vpHomogeneousMatrix &fMo)
766{
767 try {
768 this->addObject(newObject, fMo, scene);
769 } catch (...) {
770 vpERROR_TRACE("Error adding object in scene graph ");
771 throw;
772 }
773}
774
782void vpSimulator::addObject(SoSeparator *object, const vpHomogeneousMatrix &fMo, SoSeparator *root)
783{
784
785 bool identity = true;
786 for (unsigned int i = 0; i < 4; i++) {
787 for (unsigned int j = 0; j < 4; j++) {
788 if (i == j) {
789 if (fabs(fMo[i][j] - 1) > 1e-6)
790 identity = false;
791 } else {
792 if (fabs(fMo[i][j]) > 1e-6)
793 identity = false;
794 }
795 }
796 }
797
798 if (identity == true) {
799 root->addChild(object);
800 } else {
801 SbMatrix matrix;
802 SbRotation rotation;
803 for (unsigned int i = 0; i < 4; i++)
804 for (unsigned int j = 0; j < 4; j++)
805 matrix[(int)j][(int)i] = (float)fMo[i][j];
806
807 // matrix= matrix.inverse();
808 rotation.setValue(matrix);
809
810 SoTransform *displacement = new SoTransform;
811 SoSeparator *newNode = new SoSeparator;
812
813 displacement->rotation.setValue(rotation);
814 displacement->translation.setValue(matrix[3][0], matrix[3][1], matrix[3][2]);
815
816 root->addChild(newNode);
817 newNode->addChild(displacement);
818 newNode->addChild(object);
819 }
820}
821
823void vpSimulator::initApplication(void *(*start_routine)(void *))
824{
825 // pthread_create (&mainThread, NULL, start_routine, (void *)this);
826 mainThread = SbThread::create(start_routine, (void *)this);
827}
828
838void vpSimulator::initApplication(void *(*start_routine)(void *), void *data)
839{
840 mainThread = SbThread::create(start_routine, (void *)data);
841}
842
846{
847 // pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL );
848 // pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
849 vpTime::wait(1000);
850}
854{
855 vpViewer::exitMainLoop();
856 // pthread_exit (NULL);
857}
858
859/* Initialise le SoOffScreenRenderer si necessaire, puis realise le rendu.
860 * Quand la fonction rend la main, le buffer est pret et n'a plus qu'a etre
861 * enregistre ou passe a l'utilisateur.
862 * INPUT:
863 * - vueInterne est vrai ssi il faut rendre la vue interne, faux ssi
864 * il faut rendre la vue externe.
865 * OUTPUT:
866 * - width : largeur de l'image dans le buffer.
867 * - height : hauteur de l'image dans le buffer.
868 */
869void vpSimulator::offScreenRendering(vpSimulatorViewType view, int *width, int *height)
870{
871
872 SbVec2s size(320, 200);
873 SoNode *thisroot;
874
875 {
876 if (view == vpSimulator::INTERNAL) {
877 size = this->internalView->getViewportRegion().getWindowSize();
878 thisroot = this->internalView->getSceneManager()->getSceneGraph();
879 } else {
880 size = this->externalView->getViewportRegion().getWindowSize();
881 thisroot = this->externalView->getSceneManager()->getSceneGraph();
882 }
883 }
884 SbViewportRegion myViewPort(size);
885
886 // Creation du rendu si necessaire.
887 if (NULL == this->offScreenRenderer) {
888 // Init du SoOffscreenRenderer
889 this->offScreenRenderer = new SoOffscreenRenderer(myViewPort);
890 } else {
891 // Redefini le view port
892 this->offScreenRenderer->setViewportRegion(myViewPort);
893 }
894
895 // Rendu offscreen
896 if (!this->offScreenRenderer->render(thisroot)) {
897 vpERROR_TRACE("La scene n'a pas pu etre rendue offscreen.");
898 delete this->offScreenRenderer;
899 this->offScreenRenderer = NULL;
900 } else {
901
902 /*
903 if (view==vpSimulator::INTERNAL)
904 {
905 //Recopie du buffer contenant l'image, dans bufferView
906 int length = 3*size [0]*size[1];
907 delete [] bufferView;
908 bufferView = new unsigned char [length];
909 for(int i=0; i<length; i++)
910 {
911 bufferView[i] = this ->offScreenRenderer->getBuffer()[i];
912 }
913 }*/
914 }
915
916 // exit(1) ;
917 if (NULL != width) {
918 *width = size[0];
919 }
920 if (NULL != height) {
921 *height = size[1];
922 }
923}
924
925/* Enregistre l'image de vue interne ou externe dans un fichier RGB.
926 * Effectue le rendu dans un buffer plutot qu'a l'ecran, puis sauvegarde
927 * ce buffer au format PS (copie directe).
928 * INPUT
929 * - fileName: nom du fichier dans lequel placer le resultat.
930 * OUTPUT
931 * - RETURN : Code d'erreur CODE_OK si tout s'est bien passe.
932 */
933
934#ifdef VISP_HAVE_MODULE_IO
935void vpSimulator::write(const char *fileName)
936{
937
938 while (get == 0) {
939 vpTRACE("%d ", get);
940 }
941 get = 2;
942 /* FILE *fp = fopen(fileName, "w");
943 fprintf(fp,"P6 \n %d %d \n 255",internal_width,internal_height) ;
944 fwrite(bufferView, sizeof(unsigned char),
945 internal_width*internal_height*3, fp) ;*/
947
948 for (unsigned int i = 0; i < internal_height; i++)
949 for (unsigned int j = 0; j < internal_width; j++) {
950 unsigned char r, g, b;
951 unsigned int index = 3 * ((internal_height - i - 1) * internal_width + j);
952 r = *(bufferView + index);
953 g = *(bufferView + index + 1);
954 b = *(bufferView + index + 2);
955 I[i][j].R = r;
956 I[i][j].G = g;
957 I[i][j].B = b;
958 }
959 vpImageIo::write(I, fileName);
960 // fclose (fp);
961 get = 1;
962}
963#endif
964
965void vpSimulator::getSizeInternalView(int &width, int &height)
966{
967 SbVec2s size = this->internalView->getViewportRegion().getWindowSize();
968 width = size[0];
969 height = size[1];
970}
971
978{
979 // while (get==0) {;}
980 get = 2;
983 get = 1;
984}
985
991{
992 // while (get==0) {;}
993 get = 2;
996 get = 1;
997}
998
999#elif !defined(VISP_BUILD_SHARED_LIBS)
1000// Work arround to avoid warning: libvisp_ar.a(vpSimulator.cpp.o) has no
1001// symbols
1002void dummy_vpSimulator(){};
1003#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:293
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:800
Type * bitmap
points toward the bitmap
Definition: vpImage.h:143
Implementation of a simulator based on Coin3d (www.coin3d.org).
Definition: vpSimulator.h:100
SoPerspectiveCamera * internalCamera
internal camera
Definition: vpSimulator.h:192
void save(const char *name, bool binary=false)
save the scene in an iv file
unsigned int internal_height
Definition: vpSimulator.h:164
GLubyte * image_background
Definition: vpSimulator.h:131
vpCameraParameters internalCameraParameters
internal camera parameters
Definition: vpSimulator.h:241
void load(const char *file_name)
load an iv file
unsigned char * bufferView
image of the internal view
Definition: vpSimulator.h:279
SoTransform * extrenalCameraPosition
external camera position
Definition: vpSimulator.h:200
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:233
virtual void mainLoop()
activate the mainloop
unsigned int external_height
Definition: vpSimulator.h:166
void setExternalCameraParameters(vpCameraParameters &cam)
set external camera parameters
unsigned int internal_width
Definition: vpSimulator.h:163
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:237
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:129
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:137
void init()
perform some initialization
vpHomogeneousMatrix cMf
internal camera position
Definition: vpSimulator.h:239
SoSeparator * internalCameraObject
representation of the camera in the external view
Definition: vpSimulator.h:203
SoSeparator * scene
Definition: vpSimulator.h:185
SbThread * mainThread
thread with the main program
Definition: vpSimulator.h:149
vpViewer * externalView
view from an external camera
Definition: vpSimulator.h:139
void setZoomFactor(float zoom)
set the size of the camera/frame
SoPerspectiveCamera * externalCamera
external camera
Definition: vpSimulator.h:194
void setCameraPosition(vpHomogeneousMatrix &cMf)
set the camera position (from an homogeneous matrix)
HWND mainWindow
main Widget
Definition: vpSimulator.h:115
void initExternalViewer(unsigned int nlig, unsigned int ncol)
initialize the external view
SoSeparator * externalRoot
root node of the external view
Definition: vpSimulator.h:189
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:122
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:243
vpSimulator()
constructor
void initSceneGraph()
initialize the scene graph
SoOffscreenRenderer * offScreenRenderer
Definition: vpSimulator.h:274
SoTransform * internalCameraPosition
internal camera position
Definition: vpSimulator.h:197
SoSeparator * internalRoot
root node of the internal view
Definition: vpSimulator.h:187
void closeMainApplication()
SbTime * realtime
Definition: vpSimulator.h:273
void addAbsoluteFrame(float zoom=1)
Add the representation of the absolute frame.
unsigned int external_width
Definition: vpSimulator.h:165
void offScreenRendering(vpSimulatorViewType view=vpSimulator::EXTERNAL, int *width=NULL, int *height=NULL)
Viewer used by the simulator.
Definition: vpViewer.h:123
void resize(int x, int y, bool fixed=false)
Definition: vpViewer.cpp:130
@ externalView
Definition: vpViewer.h:128
@ internalView
Definition: vpViewer.h:128
#define vpTRACE
Definition: vpDebug.h:416
#define vpDEBUG_TRACE
Definition: vpDebug.h:487
#define vpERROR_TRACE
Definition: vpDebug.h:393
VISP_EXPORT int wait(double t0, double t)