Visual Servoing Platform  version 3.0.1
vpScene.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  *
32  * Authors:
33  * Jean-Luc CORRE
34  *
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 "vpScene.h"
46 #include "vpKeyword.h"
47 #include "vpLex.h"
48 #include "vpParser.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
57 getExtension(const char* file)
58 {
59  std::string sfilename(file);
60 
61  size_t bnd = sfilename.find("bnd");
62  size_t BND = sfilename.find("BND");
63  size_t wrl = sfilename.find("wrl");
64  size_t WRL = sfilename.find("WRL");
65 
66  size_t size = sfilename.size();
67 
68  if ((bnd>0 && bnd<size ) || (BND>0 && BND<size))
69  return BND_MODEL;
70  else if ((wrl>0 && wrl<size) || ( WRL>0 && WRL<size))
71  {
72 #if defined(VISP_HAVE_COIN3D)
73  return WRL_MODEL;
74 #else
75  std::cout << "Coin not installed, cannot read VRML files" << std::endl;
76  throw std::string("Coin not installed, cannot read VRML files");
77 #endif
78  }
79  return UNKNOWN_MODEL;
80 }
81 
82 /*
83  Enable to initialize the scene
84 */
85 void set_scene (const char* str, Bound_scene *sc, float factor)
86 {
87  FILE *fd;
88 
89  //if ((fd = fopen (str, 0)) == -1)
90  if ((fd = fopen (str, "r")) == NULL)
91  {
92  std::string error = "The file " + std::string(str) + " can not be opened";
93 
94  throw(vpException(vpException::ioError, error.c_str())) ;
95  }
96  open_keyword (keyword_tbl);
97  open_lex ();
98  open_source (fd, str);
99  malloc_Bound_scene (sc, str,(Index)BOUND_NBR);
100  parser (sc);
101 
102  //if (factor != 1)
103  if (std::fabs(factor) > std::numeric_limits<double>::epsilon())
104  {
105  for (int i = 0; i < sc->bound.nbr; i++)
106  {
107  for (int j = 0; j < sc->bound.ptr[i].point.nbr; j++)
108  {
109  sc->bound.ptr[i].point.ptr[j].x = sc->bound.ptr[i].point.ptr[j].x*factor;
110  sc->bound.ptr[i].point.ptr[j].y = sc->bound.ptr[i].point.ptr[j].y*factor;
111  sc->bound.ptr[i].point.ptr[j].z = sc->bound.ptr[i].point.ptr[j].z*factor;
112  }
113  }
114  }
115 
116  close_source ();
117  close_lex ();
118  close_keyword ();
119  fclose(fd);
120 }
121 
122 #if defined(VISP_HAVE_COIN3D)
123 
124 void set_scene_wrl (const char* str, Bound_scene *sc, float factor)
125 {
126  //Load the sceneGraph
127  SoDB::init();
128  SoInput in;
129  SbBool ok = in.openFile(str);
130  SoVRMLGroup *sceneGraphVRML2;
131 
132  if (!ok) {
133  throw(vpException(vpException::fatalError, "Can't open file \"%s\". Please check the Marker_Less.ini file", str));
134  }
135 
136  if(!in.isFileVRML2())
137  {
138  SoSeparator *sceneGraph = SoDB::readAll(&in);
139  if (sceneGraph == NULL) { /*return -1;*/ }
140  sceneGraph->ref();
141 
142  SoToVRML2Action tovrml2;
143  tovrml2.apply(sceneGraph);
144  sceneGraphVRML2 =tovrml2.getVRML2SceneGraph();
145  sceneGraphVRML2->ref();
146  sceneGraph->unref();
147  }
148  else
149  {
150  sceneGraphVRML2 = SoDB::readAllVRML(&in);
151  if (sceneGraphVRML2 == NULL) {
152  /*return -1;*/
153  throw(vpException(vpException::notInitialized, "Cannot read VRML file"));
154  }
155  sceneGraphVRML2->ref();
156  }
157 
158  in.closeFile();
159 
160  int nbShapes = sceneGraphVRML2->getNumChildren();
161 
162  SoNode * child;
163 
164  malloc_Bound_scene (sc, str,(Index)BOUND_NBR);
165 
166  int iterShapes = 0;
167  for (int i = 0; i < nbShapes; i++)
168  {
169  child = sceneGraphVRML2->getChild(i);
170  if (child->getTypeId() == SoVRMLShape::getClassTypeId())
171  {
172  int nbFaces = 0;
173  std::list<indexFaceSet*> ifs_list;
174  SoChildList * child2list = child->getChildren();
175  for (int j = 0; j < child2list->getLength(); j++)
176  {
177  if (((SoNode*)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId())
178  {
179  indexFaceSet *ifs = new indexFaceSet;
180  SoVRMLIndexedFaceSet * face_set;
181  face_set = (SoVRMLIndexedFaceSet*)child2list->get(j);
182  extractFaces(face_set,ifs);
183  ifs_list.push_back(ifs);
184  nbFaces++;
185  }
186 // if (((SoNode*)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId())
187 // {
188 // std::cout << "> We found a line" << std::endl;
189 // SoVRMLIndexedLineSet * line_set;
190 // line_set = (SoVRMLIndexedLineSet*)child2list->get(j);
191 // extractLines(line_set);
192 // }
193  }
194  sc->bound.nbr++;
195  ifsToBound (&(sc->bound.ptr[iterShapes]), ifs_list);
196  destroyIfs(ifs_list);
197  iterShapes++;
198  }
199  }
200 
201  //if (factor != 1)
202  if (std::fabs(factor) > std::numeric_limits<double>::epsilon())
203  {
204  for (int i = 0; i < sc->bound.nbr; i++)
205  {
206  for (int j = 0; j < sc->bound.ptr[i].point.nbr; j++)
207  {
208  sc->bound.ptr[i].point.ptr[j].x = sc->bound.ptr[i].point.ptr[j].x*factor;
209  sc->bound.ptr[i].point.ptr[j].y = sc->bound.ptr[i].point.ptr[j].y*factor;
210  sc->bound.ptr[i].point.ptr[j].z = sc->bound.ptr[i].point.ptr[j].z*factor;
211  }
212  }
213  }
214 }
215 
216 
217 void extractFaces(SoVRMLIndexedFaceSet* face_set, indexFaceSet *ifs)
218 {
219 // vpList<vpPoint> pointList;
220 // pointList.kill();
221  SoVRMLCoordinate *coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
222  int coordSize = coord->point.getNum();
223 
224  ifs->nbPt = coordSize;
225  for (int i = 0; i < coordSize; i++)
226  {
227  SbVec3f point(0,0,0);
228  point[0]=coord->point[i].getValue()[0];
229  point[1]=coord->point[i].getValue()[1];
230  point[2]=coord->point[i].getValue()[2];
231  vpPoint pt(point[0],point[1],point[2]);
232  ifs->pt.push_back(pt);
233  }
234 
235  SoMFInt32 indexList = face_set->coordIndex;
236  int indexListSize = indexList.getNum();
237 
238  ifs->nbIndex = indexListSize;
239  for (int i = 0; i < indexListSize; i++)
240  {
241  int index = face_set->coordIndex[i];
242  ifs->index.push_back(index);
243  }
244 }
245 
246 void ifsToBound (Bound* bptr, std::list<indexFaceSet*> &ifs_list)
247 {
248  int nbPt = 0;
249  for(std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it){
250  nbPt += (*it)->nbPt;
251  }
252  bptr->point.nbr = (Index)nbPt;
253  bptr->point.ptr = (Point3f *) malloc ((unsigned int)nbPt * sizeof (Point3f));
254 
255  ifs_list.front();
256  unsigned int iter = 0;
257  for (std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it)
258  {
259  indexFaceSet* ifs = *it;
260  for (unsigned int j = 0; j < (unsigned int)ifs->nbPt; j++)
261  {
262  bptr->point.ptr[iter].x = (float)ifs->pt[j].get_oX();
263  bptr->point.ptr[iter].y = (float)ifs->pt[j].get_oY();
264  bptr->point.ptr[iter].z = (float)ifs->pt[j].get_oZ();
265  iter++;
266  }
267  }
268 
269  unsigned int nbFace = 0;
270  ifs_list.front();
271  std::list<int> indSize;
272  int indice = 0;
273  for (std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it)
274  {
275  indexFaceSet* ifs = *it;
276  for (unsigned int j = 0; j < (unsigned int)ifs->nbIndex; j++)
277  {
278  if(ifs->index[j] == -1)
279  {
280  nbFace++;
281  indSize.push_back(indice);
282  indice = 0;
283  }
284  else indice++;
285  }
286  }
287 
288  bptr->face.nbr = (Index)nbFace;
289  bptr->face.ptr = (Face *) malloc (nbFace * sizeof (Face));
290 
291  std::list<int>::const_iterator iter_indSize = indSize.begin();
292  for (unsigned int i = 0; i < indSize.size(); i++)
293  {
294  bptr->face.ptr[i].vertex.nbr = (Index)*iter_indSize;
295  bptr->face.ptr[i].vertex.ptr = (Index *) malloc ((unsigned int)*iter_indSize * sizeof (Index));
296  ++iter_indSize;
297  }
298 
299  int offset = 0;
300  indice = 0;
301  for (std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it)
302  {
303  indexFaceSet* ifs = *it;
304  iter = 0;
305  for (unsigned int j = 0; j < (unsigned int)ifs->nbIndex; j++)
306  {
307  if(ifs->index[j] != -1)
308  {
309  bptr->face.ptr[indice].vertex.ptr[iter] = (Index)(ifs->index[j] + offset);
310  iter++;
311  }
312  else
313  {
314  iter = 0;
315  indice++;
316  }
317  }
318  offset += ifs->nbPt;
319  }
320 }
321 
322 void destroyIfs(std::list<indexFaceSet*> &ifs_list)
323 {
324  for(std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it){
325  delete *it;
326  }
327  ifs_list.clear();
328 }
329 #else
330 void set_scene_wrl (const char* /*str*/, Bound_scene* /*sc*/, float /*factor*/)
331 {
332 }
333 #endif
334 
335 /*
336  Convert the matrix format to deal with the one in the simulator
337 */
338 void vp2jlc_matrix (const vpHomogeneousMatrix vpM, Matrix &jlcM)
339 {
340  for (unsigned int i = 0; i < 4; i++) {
341  for (unsigned int j = 0; j < 4; j++)
342  jlcM[j][i] = (float)vpM[i][j];
343  }
344 }
345 
346 #endif
Implementation of an homogeneous matrix and operations on such kind of matrices.
error that can be emited by ViSP classes.
Definition: vpException.h:73
Class that defines what is a point.
Definition: vpPoint.h:59