Visual Servoing Platform  version 3.5.1 under development (2022-09-25)
vpScene.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  *
33  * Authors:
34  * Jean-Luc CORRE
35  *
36  *****************************************************************************/
37 
38 #include <visp3/core/vpConfig.h>
39 
40 #ifndef DOXYGEN_SHOULD_SKIP_THIS
41 
42 #include <cmath>
43 #include <limits>
44 
45 #include "vpKeyword.h"
46 #include "vpLex.h"
47 #include "vpParser.h"
48 #include "vpScene.h"
49 
50 #include <visp3/core/vpException.h>
51 #include <visp3/core/vpPoint.h>
52 
53 /*
54  Get the extension of the file and return it
55 */
56 Model_3D getExtension(const char *file)
57 {
58  std::string sfilename(file);
59 
60  size_t bnd = sfilename.find("bnd");
61  size_t BND = sfilename.find("BND");
62  size_t wrl = sfilename.find("wrl");
63  size_t WRL = sfilename.find("WRL");
64 
65  size_t size = sfilename.size();
66 
67  if ((bnd > 0 && bnd < size) || (BND > 0 && BND < size))
68  return BND_MODEL;
69  else if ((wrl > 0 && wrl < size) || (WRL > 0 && WRL < size)) {
70 #if defined(VISP_HAVE_COIN3D)
71  return WRL_MODEL;
72 #else
73  std::cout << "Coin not installed, cannot read VRML files" << std::endl;
74  throw std::string("Coin not installed, cannot read VRML files");
75 #endif
76  }
77  return UNKNOWN_MODEL;
78 }
79 
80 /*
81  Enable to initialize the scene
82 */
83 void set_scene(const char *str, Bound_scene *sc, float factor)
84 {
85  FILE *fd;
86 
87  // if ((fd = fopen (str, 0)) == -1)
88  if ((fd = fopen(str, "r")) == NULL) {
89  std::string error = "The file " + std::string(str) + " can not be opened";
90 
91  throw(vpException(vpException::ioError, error.c_str()));
92  }
93  open_keyword(keyword_tbl);
94  open_lex();
95  open_source(fd, str);
96  malloc_Bound_scene(sc, str, (Index)BOUND_NBR);
97  parser(sc);
98 
99  // if (factor != 1)
100  if (std::fabs(factor) > std::numeric_limits<double>::epsilon()) {
101  for (int i = 0; i < sc->bound.nbr; i++) {
102  for (int j = 0; j < sc->bound.ptr[i].point.nbr; j++) {
103  sc->bound.ptr[i].point.ptr[j].x = sc->bound.ptr[i].point.ptr[j].x * factor;
104  sc->bound.ptr[i].point.ptr[j].y = sc->bound.ptr[i].point.ptr[j].y * factor;
105  sc->bound.ptr[i].point.ptr[j].z = sc->bound.ptr[i].point.ptr[j].z * factor;
106  }
107  }
108  }
109 
110  close_source();
111  close_lex();
112  close_keyword();
113  fclose(fd);
114 }
115 
116 #if defined(VISP_HAVE_COIN3D)
117 
118 void set_scene_wrl(const char *str, Bound_scene *sc, float factor)
119 {
120  // Load the sceneGraph
121  SoDB::init();
122  SoInput in;
123  SbBool ok = in.openFile(str);
124  SoVRMLGroup *sceneGraphVRML2;
125 
126  if (!ok) {
127  throw(vpException(vpException::fatalError, "Can't open file \"%s\". Please check the Marker_Less.ini file", str));
128  }
129 
130  if (!in.isFileVRML2()) {
131  SoSeparator *sceneGraph = SoDB::readAll(&in);
132  if (sceneGraph == NULL) { /*return -1;*/
133  }
134  sceneGraph->ref();
135 
136  SoToVRML2Action tovrml2;
137  tovrml2.apply(sceneGraph);
138  sceneGraphVRML2 = tovrml2.getVRML2SceneGraph();
139  sceneGraphVRML2->ref();
140  sceneGraph->unref();
141  } else {
142  sceneGraphVRML2 = SoDB::readAllVRML(&in);
143  if (sceneGraphVRML2 == NULL) {
144  /*return -1;*/
145  throw(vpException(vpException::notInitialized, "Cannot read VRML file"));
146  }
147  sceneGraphVRML2->ref();
148  }
149 
150  in.closeFile();
151 
152  int nbShapes = sceneGraphVRML2->getNumChildren();
153 
154  SoNode *child;
155 
156  malloc_Bound_scene(sc, str, (Index)BOUND_NBR);
157 
158  int iterShapes = 0;
159  for (int i = 0; i < nbShapes; i++) {
160  child = sceneGraphVRML2->getChild(i);
161  if (child->getTypeId() == SoVRMLShape::getClassTypeId()) {
162  int nbFaces = 0;
163  std::list<indexFaceSet *> ifs_list;
164  SoChildList *child2list = child->getChildren();
165  for (int j = 0; j < child2list->getLength(); j++) {
166  if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId()) {
167  indexFaceSet *ifs = new indexFaceSet;
168  SoVRMLIndexedFaceSet *face_set;
169  face_set = (SoVRMLIndexedFaceSet *)child2list->get(j);
170  extractFaces(face_set, ifs);
171  ifs_list.push_back(ifs);
172  nbFaces++;
173  }
174  // if (((SoNode*)child2list->get(j))->getTypeId() ==
175  // SoVRMLIndexedLineSet::getClassTypeId())
176  // {
177  // std::cout << "> We found a line" << std::endl;
178  // SoVRMLIndexedLineSet * line_set;
179  // line_set = (SoVRMLIndexedLineSet*)child2list->get(j);
180  // extractLines(line_set);
181  // }
182  }
183  sc->bound.nbr++;
184  ifsToBound(&(sc->bound.ptr[iterShapes]), ifs_list);
185  destroyIfs(ifs_list);
186  iterShapes++;
187  }
188  }
189 
190  // if (factor != 1)
191  if (std::fabs(factor) > std::numeric_limits<double>::epsilon()) {
192  for (int i = 0; i < sc->bound.nbr; i++) {
193  for (int j = 0; j < sc->bound.ptr[i].point.nbr; j++) {
194  sc->bound.ptr[i].point.ptr[j].x = sc->bound.ptr[i].point.ptr[j].x * factor;
195  sc->bound.ptr[i].point.ptr[j].y = sc->bound.ptr[i].point.ptr[j].y * factor;
196  sc->bound.ptr[i].point.ptr[j].z = sc->bound.ptr[i].point.ptr[j].z * factor;
197  }
198  }
199  }
200 }
201 
202 void extractFaces(SoVRMLIndexedFaceSet *face_set, indexFaceSet *ifs)
203 {
204  // vpList<vpPoint> pointList;
205  // pointList.kill();
206  SoVRMLCoordinate *coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
207  int coordSize = coord->point.getNum();
208 
209  ifs->nbPt = coordSize;
210  for (int i = 0; i < coordSize; i++) {
211  SbVec3f point(0, 0, 0);
212  point[0] = coord->point[i].getValue()[0];
213  point[1] = coord->point[i].getValue()[1];
214  point[2] = coord->point[i].getValue()[2];
215  vpPoint pt(point[0], point[1], point[2]);
216  ifs->pt.push_back(pt);
217  }
218 
219  SoMFInt32 indexList = face_set->coordIndex;
220  int indexListSize = indexList.getNum();
221 
222  ifs->nbIndex = indexListSize;
223  for (int i = 0; i < indexListSize; i++) {
224  int index = face_set->coordIndex[i];
225  ifs->index.push_back(index);
226  }
227 }
228 
229 void ifsToBound(Bound *bptr, std::list<indexFaceSet *> &ifs_list)
230 {
231  int nbPt = 0;
232  for (std::list<indexFaceSet *>::const_iterator it = ifs_list.begin(); it != ifs_list.end(); ++it) {
233  nbPt += (*it)->nbPt;
234  }
235  bptr->point.nbr = (Index)nbPt;
236  bptr->point.ptr = (Point3f *)malloc((unsigned int)nbPt * sizeof(Point3f));
237 
238  ifs_list.front();
239  unsigned int iter = 0;
240  for (std::list<indexFaceSet *>::const_iterator it = ifs_list.begin(); it != ifs_list.end(); ++it) {
241  indexFaceSet *ifs = *it;
242  for (unsigned int j = 0; j < (unsigned int)ifs->nbPt; j++) {
243  bptr->point.ptr[iter].x = (float)ifs->pt[j].get_oX();
244  bptr->point.ptr[iter].y = (float)ifs->pt[j].get_oY();
245  bptr->point.ptr[iter].z = (float)ifs->pt[j].get_oZ();
246  iter++;
247  }
248  }
249 
250  unsigned int nbFace = 0;
251  ifs_list.front();
252  std::list<int> indSize;
253  int indice = 0;
254  for (std::list<indexFaceSet *>::const_iterator it = ifs_list.begin(); it != ifs_list.end(); ++it) {
255  indexFaceSet *ifs = *it;
256  for (unsigned int j = 0; j < (unsigned int)ifs->nbIndex; j++) {
257  if (ifs->index[j] == -1) {
258  nbFace++;
259  indSize.push_back(indice);
260  indice = 0;
261  } else
262  indice++;
263  }
264  }
265 
266  bptr->face.nbr = (Index)nbFace;
267  bptr->face.ptr = (Face *)malloc(nbFace * sizeof(Face));
268 
269  std::list<int>::const_iterator iter_indSize = indSize.begin();
270  for (unsigned int i = 0; i < indSize.size(); i++) {
271  bptr->face.ptr[i].vertex.nbr = (Index)*iter_indSize;
272  bptr->face.ptr[i].vertex.ptr = (Index *)malloc((unsigned int)*iter_indSize * sizeof(Index));
273  ++iter_indSize;
274  }
275 
276  int offset = 0;
277  indice = 0;
278  for (std::list<indexFaceSet *>::const_iterator it = ifs_list.begin(); it != ifs_list.end(); ++it) {
279  indexFaceSet *ifs = *it;
280  iter = 0;
281  for (unsigned int j = 0; j < (unsigned int)ifs->nbIndex; j++) {
282  if (ifs->index[j] != -1) {
283  bptr->face.ptr[indice].vertex.ptr[iter] = (Index)(ifs->index[j] + offset);
284  iter++;
285  } else {
286  iter = 0;
287  indice++;
288  }
289  }
290  offset += ifs->nbPt;
291  }
292 }
293 
294 void destroyIfs(std::list<indexFaceSet *> &ifs_list)
295 {
296  for (std::list<indexFaceSet *>::const_iterator it = ifs_list.begin(); it != ifs_list.end(); ++it) {
297  delete *it;
298  }
299  ifs_list.clear();
300 }
301 #else
302 void set_scene_wrl(const char * /*str*/, Bound_scene * /*sc*/, float /*factor*/) {}
303 #endif
304 
305 /*
306  Convert the matrix format to deal with the one in the simulator
307 */
308 void vp2jlc_matrix(const vpHomogeneousMatrix &vpM, Matrix &jlcM)
309 {
310  for (unsigned int i = 0; i < 4; i++) {
311  for (unsigned int j = 0; j < 4; j++)
312  jlcM[j][i] = (float)vpM[i][j];
313  }
314 }
315 
316 #endif
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ ioError
I/O error.
Definition: vpException.h:91
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition: vpException.h:98
@ fatalError
Fatal error.
Definition: vpException.h:96
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82