Visual Servoing Platform version 3.5.0
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*/
56Model_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*/
83void 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
118void 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
202void 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
229void 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
294void 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
302void 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*/
308void 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