Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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//
76static 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
83static 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
109SoSeparator *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 */
134static 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
174SoSeparator *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
224SoSeparator *createCameraObject(float zoomFactor = 1.0)
225{
226 vpDEBUG_TRACE(15, "# Entree.");
227
228 SoSeparator *cam = new SoSeparator;
229 cam->ref();
230
231 SoMaterial *myMaterial = new SoMaterial;
232 myMaterial->diffuseColor.setValue(1.0, 0.0, 0.0);
233 myMaterial->emissiveColor.setValue(0.5, 0.0, 0.0);
234
235 SoScale *taille = new SoScale;
236 {
237 float zoom = 0.1f * zoomFactor;
238 taille->scaleFactor.setValue(zoom, zoom, zoom);
239 }
240
241 SoMaterial *couleurBlanc = new SoMaterial;
242 couleurBlanc->diffuseColor.setValue(1.0, 1.0, 1.0);
243 couleurBlanc->emissiveColor.setValue(1.0, 1.0, 1.0);
244 SoDrawStyle *filDeFer = new SoDrawStyle;
245 filDeFer->style.setValue(SoDrawStyle::LINES);
246 filDeFer->lineWidth.setValue(1);
247
248 SoSeparator *cone = new SoSeparator;
249 cone->ref();
250 cone->addChild(makePyramide());
251 cone->addChild(couleurBlanc);
252 cone->addChild(filDeFer);
253 cone->addChild(makePyramide());
254 cone->unrefNoDelete();
255
256 cam->addChild(myMaterial);
257 cam->addChild(taille);
258 cam->addChild(cone);
259 cam->addChild(createFrame(2.0f, 0.1f, 0.01f));
260
261 // cam->unref() ;
262 vpDEBUG_TRACE(15, "# Sortie.");
263 return cam;
264}
265
266//--------------------------------------------------------------
268{
269 internal_width = 200;
270 internal_height = 200;
271 external_width = 200;
272 external_height = 200;
273
274 mainWindowInitialized = false;
275 internalView = NULL;
276 externalView = NULL;
277 image_background = NULL;
278
279 zoomFactor = 1;
281
282 // write image process
283 realtime = NULL;
284 offScreenRenderer = NULL;
285 bufferView = NULL;
286 get = 1;
288 mainThread = NULL;
289 scene = NULL;
290 internalRoot = NULL;
291 externalRoot = NULL;
292 internalCamera = NULL;
293 externalCamera = NULL;
297#if defined(VISP_HAVE_SOWIN)
298// mainWindow = ?;
299#elif defined(VISP_HAVE_SOQT)
300 mainWindow = NULL;
301#elif defined(VISP_HAVE_SOXT)
302// mainWindow = ?;
303#endif
304}
306{
307 if (internalView != NULL) {
308 delete internalView;
309 internalView = NULL;
310 }
311 if (externalView != NULL) {
312 delete externalView;
313 externalView = NULL;
314 }
315 if (bufferView != NULL) {
316 delete[] bufferView;
317 bufferView = NULL;
318 }
319 if (image_background != NULL) {
320 free(image_background);
321 image_background = NULL;
322 }
323}
324
326 :
327#if defined(VISP_HAVE_SOWIN)
328 mainWindow(),
329#elif defined(VISP_HAVE_SOQT)
330 mainWindow(NULL),
331#elif defined(VISP_HAVE_SOXT)
332 mainWindow(),
333#endif
334 mainWindowInitialized(false), typeImage(vpSimulator::grayImage), image_background(NULL), internalView(NULL),
335 externalView(NULL), mainThread(NULL), internal_width(0), internal_height(0), external_width(0), external_height(0),
336 scene(NULL), internalRoot(NULL), externalRoot(NULL), internalCamera(NULL), externalCamera(NULL),
337 internalCameraPosition(NULL), extrenalCameraPosition(NULL), internalCameraObject(NULL), zoomFactor(0.),
338 cameraPositionInitialized(false), cMf(), internalCameraParameters(), externalCameraParameters(), realtime(NULL),
339 offScreenRenderer(NULL), bufferView(NULL), get(0)
340{
342}
343
345
347{
348 mainWindow = vpViewer::init("");
350}
351
353{
354 this->scene = new SoSeparator;
355 this->internalRoot = new SoSeparator;
356 this->externalRoot = new SoSeparator;
357
358 this->scene->ref();
359 this->internalRoot->ref();
360 this->externalRoot->ref();
361
362 // define the camera SoPerspectiveCamera
363 this->internalCamera = new SoPerspectiveCamera;
364 this->externalCamera = new SoPerspectiveCamera;
365
366 this->internalCameraPosition = new SoTransform;
367 this->internalCameraObject = createCameraObject(zoomFactor);
368
369 internalCamera->farDistance.setValue(100);
370 internalCamera->nearDistance.setValue(0.0001f);
371
372 // link between camera and internal root
373 this->internalRoot->addChild(this->internalCamera);
374 this->internalRoot->addChild(this->scene);
375
376 this->externalRoot->addChild(this->externalCamera);
377 this->externalRoot->addChild(this->scene);
378
379 SoSeparator *camera = new SoSeparator;
380 camera->ref();
381 camera->addChild(this->internalCameraPosition);
382 camera->addChild(this->internalCameraObject);
383 this->externalRoot->addChild(camera);
384
385 // this->externalRoot->addChild (internalCameraPosition);
386 // this->externalRoot->addChild (internalCameraObject);
387 SoCube *cube = new SoCube;
388 cube->width = 0.01f;
389 cube->depth = 0.01f;
390 cube->height = 0.01f;
391
392 this->externalRoot->addChild(cube);
393
394 if (realtime == NULL) {
395
396 SoDB::enableRealTimeSensor(FALSE);
397 SoSceneManager::enableRealTimeUpdate(FALSE);
398 realtime = (SbTime *)SoDB::getGlobalField("realTime");
399 realtime->setValue(0.0);
400 }
401}
402
410{
411 zoomFactor = zoom;
412 static bool firstTime = true;
413 if (firstTime) {
414 SoScale *taille = new SoScale;
415 taille->scaleFactor.setValue(zoomFactor, zoomFactor, zoomFactor);
416 this->scene->addChild(taille);
417 firstTime = false;
418 } else {
419 SoScale *taille = (SoScale *)this->scene->getChild(0);
420 taille->scaleFactor.setValue(zoomFactor, zoomFactor, zoomFactor);
421 }
422}
423
442void vpSimulator::changeZoomFactor(float zoomFactor, int index)
443{
444 SoScale *taille = (SoScale *)this->scene->getChild(index);
445 taille->scaleFactor.setValue(zoomFactor, zoomFactor, zoomFactor);
446 // this->setZoomFactor(zoomFactor);
447}
448
449void vpSimulator::initInternalViewer(unsigned int width, unsigned int height)
450{
451 internal_width = width;
452 internal_height = height;
453
454 if (mainWindowInitialized == false) {
457 }
458
460
461 // set the scene to render from this view
462 internalView->setSceneGraph(internalRoot);
463
464 // set the title
465 internalView->setTitle("Internal camera view");
466
467 // If the view mode is on, user events will be caught and used to influence
468 // the camera position / orientation. in this viewer we do not want that,
469 // we set it to false
470 internalView->setViewing(false);
471
472 // Turn the viewer decorations
473 internalView->setDecoration(false);
474
475 internalView->resize((int)width, (int)height, true);
476
477 // open the window
478 internalView->show();
479
480 bufferView = new unsigned char[3 * width * height];
481}
482
483void vpSimulator::initExternalViewer(unsigned int width, unsigned int height)
484{
485
486 external_width = width;
487 external_height = height;
488
489 if (mainWindowInitialized == false) {
492 }
493
495
496 // set the scene to render this view
497 externalView->setSceneGraph(externalRoot);
498
499 // set the title
500 externalView->setTitle("External View");
501 externalView->resize((int)width, (int)height, false);
502 // the goal here is to see all the scene and not to determine
503 // a manual viewpoint
504 externalView->viewAll();
505
506 // open the window
507 externalView->show();
508}
509
511{
513
514 float px = (float)_cam.get_px();
515 float py = (float)_cam.get_py();
516 float v = internal_height / (2.f * py);
517
518 internalCamera->ref();
519 internalCamera->heightAngle = 2 * atan(v);
520 internalCamera->aspectRatio = (internal_width / internal_height) * (px / py);
521 internalCamera->nearDistance = 0.001f;
522
523 internalCamera->farDistance = 1000;
524 internalCamera->unrefNoDelete();
525}
526
528{
529 // SoPerspectiveCamera *camera ;
530 // camera = (SoPerspectiveCamera *)this->externalView->getCamera() ;
532
533 float px = (float)_cam.get_px();
534 float py = (float)_cam.get_py();
535 float v = external_height / (2 * py);
536
537 externalCamera->ref();
538 externalCamera->heightAngle = 2 * atan(v);
539 externalCamera->aspectRatio = (external_width / external_height) * (px / py);
540 externalCamera->nearDistance = 0.001f;
541 externalCamera->farDistance = 1000;
542 externalCamera->unrefNoDelete();
543}
544
546{
547 /* SoCamera *camera ;
548 camera = this->externalView->getCamera() ;*/
549 SoSFVec3f position = externalCamera->position;
550
551 // get the rotation
552 SoSFRotation orientation = externalCamera->orientation;
553 SbVec3f axis;
554 float angle;
555 orientation.getValue(axis, angle);
556 SbRotation rotation(axis, angle);
557
558 // get the translation
559 SbVec3f t;
560 t = position.getValue();
561
562 SbMatrix matrix;
563 matrix.setRotate(rotation);
564
566 SbMatrix rotX;
567 rotX.setRotate(SbRotation(SbVec3f(1.0f, 0.0f, 0.0f), (float)M_PI));
568 matrix.multLeft(rotX);
569 for (unsigned int i = 0; i < 4; i++)
570 for (unsigned int j = 0; j < 4; j++)
571 fMc[j][i] = matrix[(int)i][(int)j];
572 fMc[0][3] = t[0];
573 fMc[1][3] = t[1];
574 fMc[2][3] = t[2];
575
576 cMf = fMc.inverse();
577}
578
585{
586
587 SbMatrix matrix;
588 SbRotation rotCam;
589 SbMatrix rotX;
590 rotX.setRotate(SbRotation(SbVec3f(1.0f, 0.0f, 0.0f), (float)M_PI));
591 for (unsigned int i = 0; i < 4; i++)
592 for (unsigned int j = 0; j < 4; j++)
593 matrix[(int)j][(int)i] = (float)cMf[i][j];
594
595 matrix = matrix.inverse();
596 matrix.multLeft(rotX);
597 rotCam.setValue(matrix);
598
599 internalCamera->ref();
600 internalCamera->orientation.setValue(rotCam);
601 internalCamera->position.setValue(matrix[3][0], matrix[3][1], matrix[3][2]);
602 internalCamera->unref();
603
604 rotX.setRotate(SbRotation(SbVec3f(-1.0f, 0.0f, 0.0f), (float)M_PI));
605 matrix.multLeft(rotX);
606 rotCam.setValue(matrix);
608 internalCameraPosition->rotation.setValue(rotCam);
609 internalCameraPosition->translation.setValue(matrix[3][0], matrix[3][1], matrix[3][2]);
610 internalCameraPosition->unref();
611}
612
617{
618
619 // if (this->cameraPositionInitialized==true)
620 {
621 if (this->externalView != NULL) {
622 this->externalView->render(); // call actualRedraw()
623 // vpHomogeneousMatrix c ;
624 // getExternalCameraPosition(c) ;
625 }
626 if (this->internalView != NULL) {
627 this->moveInternalCamera(this->cMf);
628 this->internalView->render(); // call actualRedraw()
629 }
630 }
631}
632
633// This function is called 20 times each second.
634static void timerSensorCallback(void *data, SoSensor *)
635{
636 vpSimulator *simulator = (vpSimulator *)data;
637
638 simulator->redraw();
639}
640
642{
643 if (mainWindowInitialized == false) {
644 vpERROR_TRACE("main window is not opened ");
645 }
646
647 vpTime::wait(1000);
648
649 // Timer sensor
650 SoTimerSensor *timer = new SoTimerSensor(timerSensorCallback, (void *)this);
651 timer->setInterval(0.01);
652 timer->schedule();
653 vpViewer::mainLoop();
654}
655
656//-----------------------------------------------------------------
657// scene stuff
658//-----------------------------------------------------------------
659
661void vpSimulator::load(const char *file_name)
662{
663
664 SoInput input;
665 if (!input.openFile(file_name)) {
666 vpERROR_TRACE("Erreur cannot open file %s", file_name);
667 }
668
669 SoSeparator *newscene = SoDB::readAll(&input);
670 newscene->ref();
671 if (newscene == NULL) {
672 vpERROR_TRACE("Error while reading %s", file_name);
673 }
674
675 SoScale *taille = new SoScale;
676 taille->scaleFactor.setValue(zoomFactor, zoomFactor, zoomFactor);
677
678 // newscene->addChild(taille);
679
680 // std::cout << "this->scene->getNumChildren() = " <<
681 // this->scene->getNumChildren() << std::endl;
682
683 this->scene->addChild(taille);
684 this->scene->addChild(newscene);
685 newscene->unref();
686}
687
688void vpSimulator::save(const char *name, bool binary)
689{
690 // get a pointer to the object "name"
691 SoOutput output;
692 output.openFile(name);
693
694 if (binary == true)
695 output.setBinary(TRUE);
696
697 SoWriteAction writeAction(&output);
698 writeAction.apply(scene);
699 output.closeFile();
700}
701
707void vpSimulator::addFrame(const vpHomogeneousMatrix &fMo, float zoom)
708{
709
710 SoScale *taille = new SoScale;
711 taille->scaleFactor.setValue(zoom, zoom, zoom);
712
713 SoSeparator *frame = new SoSeparator;
714 frame->ref();
715 frame->addChild(taille);
716 frame->addChild(createFrame(LONGUEUR_FLECHE * zoom, PROPORTION_FLECHE * zoom, RAYON_FLECHE * zoom));
717 this->addObject(frame, fMo, externalRoot);
718 // frame->unref();
719}
720
727{
728 scene->addChild(createFrame(LONGUEUR_FLECHE * zoom, PROPORTION_FLECHE * zoom, RAYON_FLECHE * zoom));
729}
730
736void vpSimulator::load(const char *iv_filename, const vpHomogeneousMatrix &fMo)
737{
738
739 SoInput in;
740 SoSeparator *newObject;
741
742 if (!in.openFile(iv_filename)) {
743 vpERROR_TRACE("Erreur lors de la lecture du fichier %s.", iv_filename);
744 }
745
746 newObject = SoDB::readAll(&in);
747 if (NULL == newObject) {
748 vpERROR_TRACE("Problem reading data for file <%s>.", iv_filename);
749 }
750
751 try {
752 this->addObject(newObject, fMo);
753 } catch (...) {
754 vpERROR_TRACE("Error adding object from file <%s> ", iv_filename);
755 throw;
756 }
757}
758
764void vpSimulator::addObject(SoSeparator *newObject, const vpHomogeneousMatrix &fMo)
765{
766 try {
767 this->addObject(newObject, fMo, scene);
768 } catch (...) {
769 vpERROR_TRACE("Error adding object in scene graph ");
770 throw;
771 }
772}
773
781void vpSimulator::addObject(SoSeparator *object, const vpHomogeneousMatrix &fMo, SoSeparator *root)
782{
783
784 bool identity = true;
785 for (unsigned int i = 0; i < 4; i++) {
786 for (unsigned int j = 0; j < 4; j++) {
787 if (i == j) {
788 if (fabs(fMo[i][j] - 1) > 1e-6)
789 identity = false;
790 } else {
791 if (fabs(fMo[i][j]) > 1e-6)
792 identity = false;
793 }
794 }
795 }
796
797 if (identity == true) {
798 root->addChild(object);
799 } else {
800 SbMatrix matrix;
801 SbRotation rotation;
802 for (unsigned int i = 0; i < 4; i++)
803 for (unsigned int j = 0; j < 4; j++)
804 matrix[(int)j][(int)i] = (float)fMo[i][j];
805
806 // matrix= matrix.inverse();
807 rotation.setValue(matrix);
808
809 SoTransform *displacement = new SoTransform;
810 SoSeparator *newNode = new SoSeparator;
811
812 displacement->rotation.setValue(rotation);
813 displacement->translation.setValue(matrix[3][0], matrix[3][1], matrix[3][2]);
814
815 root->addChild(newNode);
816 newNode->addChild(displacement);
817 newNode->addChild(object);
818 }
819}
820
822void vpSimulator::initApplication(void *(*start_routine)(void *))
823{
824 // pthread_create (&mainThread, NULL, start_routine, (void *)this);
825 mainThread = SbThread::create(start_routine, (void *)this);
826}
827
837void vpSimulator::initApplication(void *(*start_routine)(void *), void *data)
838{
839 mainThread = SbThread::create(start_routine, (void *)data);
840}
841
845{
846 // pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL );
847 // pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
848 vpTime::wait(1000);
849}
853{
854 vpViewer::exitMainLoop();
855 // pthread_exit (NULL);
856}
857
858/* Initialise le SoOffScreenRenderer si necessaire, puis realise le rendu.
859 * Quand la fonction rend la main, le buffer est pret et n'a plus qu'a etre
860 * enregistre ou passe a l'utilisateur.
861 * INPUT:
862 * - vueInterne est vrai ssi il faut rendre la vue interne, faux ssi
863 * il faut rendre la vue externe.
864 * OUTPUT:
865 * - width : largeur de l'image dans le buffer.
866 * - height : hauteur de l'image dans le buffer.
867 */
868void vpSimulator::offScreenRendering(vpSimulatorViewType view, int *width, int *height)
869{
870
871 SbVec2s size(320, 200);
872 SoNode *thisroot;
873
874 {
875 if (view == vpSimulator::INTERNAL) {
876 size = this->internalView->getViewportRegion().getWindowSize();
877 thisroot = this->internalView->getSceneManager()->getSceneGraph();
878 } else {
879 size = this->externalView->getViewportRegion().getWindowSize();
880 thisroot = this->externalView->getSceneManager()->getSceneGraph();
881 }
882 }
883 SbViewportRegion myViewPort(size);
884
885 // Creation du rendu si necessaire.
886 if (NULL == this->offScreenRenderer) {
887 // Init du SoOffscreenRenderer
888 this->offScreenRenderer = new SoOffscreenRenderer(myViewPort);
889 } else {
890 // Redefini le view port
891 this->offScreenRenderer->setViewportRegion(myViewPort);
892 }
893
894 // Rendu offscreen
895 if (!this->offScreenRenderer->render(thisroot)) {
896 vpERROR_TRACE("La scene n'a pas pu etre rendue offscreen.");
897 delete this->offScreenRenderer;
898 this->offScreenRenderer = NULL;
899 } else {
900
901 /*
902 if (view==vpSimulator::INTERNAL)
903 {
904 //Recopie du buffer contenant l'image, dans bufferView
905 int length = 3*size [0]*size[1];
906 delete [] bufferView;
907 bufferView = new unsigned char [length];
908 for(int i=0; i<length; i++)
909 {
910 bufferView[i] = this ->offScreenRenderer->getBuffer()[i];
911 }
912 }*/
913 }
914
915 // exit(1) ;
916 if (NULL != width) {
917 *width = size[0];
918 }
919 if (NULL != height) {
920 *height = size[1];
921 }
922}
923
924/* Enregistre l'image de vue interne ou externe dans un fichier RGB.
925 * Effectue le rendu dans un buffer plutot qu'a l'ecran, puis sauvegarde
926 * ce buffer au format PS (copie directe).
927 * INPUT
928 * - fileName: nom du fichier dans lequel placer le resultat.
929 * OUTPUT
930 * - RETURN : Code d'erreur CODE_OK si tout s'est bien passe.
931 */
932
933#ifdef VISP_HAVE_MODULE_IO
934void vpSimulator::write(const char *fileName)
935{
936
937 while (get == 0) {
938 vpTRACE("%d ", get);
939 }
940 get = 2;
941 /* FILE *fp = fopen(fileName, "w");
942 fprintf(fp,"P6 \n %d %d \n 255",internal_width,internal_height) ;
943 fwrite(bufferView, sizeof(unsigned char),
944 internal_width*internal_height*3, fp) ;*/
946
947 for (unsigned int i = 0; i < internal_height; i++)
948 for (unsigned int j = 0; j < internal_width; j++) {
949 unsigned char r, g, b;
950 unsigned int index = 3 * ((internal_height - i - 1) * internal_width + j);
951 r = *(bufferView + index);
952 g = *(bufferView + index + 1);
953 b = *(bufferView + index + 2);
954 I[i][j].R = r;
955 I[i][j].G = g;
956 I[i][j].B = b;
957 }
958 vpImageIo::write(I, fileName);
959 // fclose (fp);
960 get = 1;
961}
962#endif
963
964void vpSimulator::getSizeInternalView(int &width, int &height)
965{
966 SbVec2s size = this->internalView->getViewportRegion().getWindowSize();
967 width = size[0];
968 height = size[1];
969}
970
977{
978 // while (get==0) {;}
979 get = 2;
982 get = 1;
983}
984
997
998#elif !defined(VISP_BUILD_SHARED_LIBS)
999// Work around to avoid warning: libvisp_ar.a(vpSimulator.cpp.o) has no
1000// symbols
1001void dummy_vpSimulator(){};
1002#endif
Generic class defining intrinsic camera parameters.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static void RGBToGrey(unsigned char *rgb, unsigned char *grey, unsigned int width, unsigned int height, bool flip=false)
static void RGBToRGBa(unsigned char *rgb, unsigned char *rgba, unsigned int size)
static void write(const vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition of the vpImage class member functions.
Definition vpImage.h:135
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition vpImage.h:795
Type * bitmap
points toward the bitmap
Definition vpImage.h:139
Implementation of a simulator based on Coin3d (www.coin3d.org).
Definition vpSimulator.h:99
SoPerspectiveCamera * internalCamera
internal camera
void save(const char *name, bool binary=false)
save the scene in an iv file
unsigned int internal_height
GLubyte * image_background
vpCameraParameters internalCameraParameters
internal camera parameters
void load(const char *file_name)
load an iv file
unsigned char * bufferView
image of the internal view
SoTransform * extrenalCameraPosition
external camera position
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
virtual void mainLoop()
activate the mainloop
unsigned int external_height
void setExternalCameraParameters(vpCameraParameters &cam)
set external camera parameters
unsigned int internal_width
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
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
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
void init()
perform some initialization
vpHomogeneousMatrix cMf
internal camera position
SoSeparator * internalCameraObject
representation of the camera in the external view
SoSeparator * scene
SbThread * mainThread
thread with the main program
vpViewer * externalView
view from an external camera
void setZoomFactor(float zoom)
set the size of the camera/frame
SoPerspectiveCamera * externalCamera
external camera
void setCameraPosition(vpHomogeneousMatrix &cMf)
set the camera position (from an homogeneous matrix)
HWND mainWindow
main Widget
void initExternalViewer(unsigned int nlig, unsigned int ncol)
initialize the external view
SoSeparator * externalRoot
root node of the external view
void write(const char *fileName)
virtual ~vpSimulator()
virtual void initInternalViewer(unsigned int nlig, unsigned int ncol)
initialize the camera view
bool mainWindowInitialized
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
vpSimulator()
constructor
void initSceneGraph()
initialize the scene graph
SoOffscreenRenderer * offScreenRenderer
SoTransform * internalCameraPosition
internal camera position
SoSeparator * internalRoot
root node of the internal view
void closeMainApplication()
SbTime * realtime
void addAbsoluteFrame(float zoom=1)
Add the representation of the absolute frame.
unsigned int external_width
void offScreenRendering(vpSimulatorViewType view=vpSimulator::EXTERNAL, int *width=NULL, int *height=NULL)
Viewer used by the simulator.
Definition vpViewer.h:120
void resize(int x, int y, bool fixed=false)
Definition vpViewer.cpp:127
@ externalView
Definition vpViewer.h:125
@ internalView
Definition vpViewer.h:125
#define vpTRACE
Definition vpDebug.h:411
#define vpDEBUG_TRACE
Definition vpDebug.h:482
#define vpERROR_TRACE
Definition vpDebug.h:388
VISP_EXPORT int wait(double t0, double t)