1 /*
2 * Copyright (C) 2016 Open Source Robotics Foundation
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *
16 */
17 #include <string.h>
18 #include <ctype.h>
19 #include <stdio.h>
20 #include <memory>
21
22 #include "ignition/math/Helpers.hh"
23 #include "ignition/common/Console.hh"
24 #include "ignition/common/Mesh.hh"
25 #include "ignition/common/SubMesh.hh"
26 #include "ignition/common/STLLoader.hh"
27
28 using namespace ignition;
29 using namespace common;
30
31 //////////////////////////////////////////////////
STLLoader()32 STLLoader::STLLoader()
33 : MeshLoader()
34 {
35 }
36
37 //////////////////////////////////////////////////
~STLLoader()38 STLLoader::~STLLoader()
39 {
40 }
41
42 //////////////////////////////////////////////////
Load(const std::string & _filename)43 Mesh *STLLoader::Load(const std::string &_filename)
44 {
45 FILE *file = fopen(_filename.c_str(), "r");
46
47 if (!file)
48 {
49 ignerr << "Unable to open file[" << _filename << "]\n";
50 return nullptr;
51 }
52
53 Mesh *mesh = new Mesh();
54
55 // Try to read ASCII first. If that fails, try binary
56 if (!this->ReadAscii(file, mesh))
57 {
58 fclose(file);
59 file = fopen(_filename.c_str(), "rb");
60 if (!this->ReadBinary(file, mesh))
61 ignerr << "Unable to read STL[" << _filename << "]\n";
62 }
63
64 fclose(file);
65 return mesh;
66 }
67
68 //////////////////////////////////////////////////
ReadAscii(FILE * _filein,Mesh * _mesh)69 bool STLLoader::ReadAscii(FILE *_filein, Mesh *_mesh)
70 {
71 int count;
72 char *next;
73 float r1;
74 float r2;
75 float r3;
76 float r4;
77 char token[LINE_MAX_LEN];
78 int width;
79 char input[LINE_MAX_LEN];
80 bool result = true;
81
82 SubMesh subMesh;
83
84 // Read the next line of the file into INPUT.
85 while (fgets (input, LINE_MAX_LEN, _filein) != nullptr)
86 {
87 // Advance to the first nonspace character in INPUT.
88 for (next = input; *next != '\0' && iswspace(*next); ++next)
89 {
90 }
91
92 // Skip blank lines and comments.
93 if (*next == '\0' || *next == '#' || *next == '!' || *next == '$')
94 continue;
95
96 // Extract the first word in this line.
97 // cppcheck-suppress invalidscanf
98 sscanf(next, "%s%n", token, &width);
99
100 // Set NEXT to point to just after this token.
101 next = next + width;
102
103 // FACET
104 if (this->Leqi(token, const_cast<char*>("facet")))
105 {
106 ignition::math::Vector3d normal;
107
108 // Get the XYZ coordinates of the normal vector to the face.
109 sscanf(next, "%*s %e %e %e", &r1, &r2, &r3);
110
111 normal.X(r1);
112 normal.Y(r2);
113 normal.Z(r3);
114
115 if (fgets (input, LINE_MAX_LEN, _filein) == nullptr)
116 {
117 result = false;
118 break;
119 }
120
121 for (; result; )
122 {
123 ignition::math::Vector3d vertex;
124 if (fgets (input, LINE_MAX_LEN, _filein) == nullptr)
125 {
126 result = false;
127 break;
128 }
129
130 count = sscanf(input, "%*s %e %e %e", &r1, &r2, &r3);
131
132 if (count != 3)
133 break;
134
135 vertex.X(r1);
136 vertex.Y(r2);
137 vertex.Z(r3);
138
139 subMesh.AddVertex(vertex);
140 subMesh.AddNormal(normal);
141 subMesh.AddIndex(subMesh.IndexOfVertex(vertex));
142 }
143
144 if (fgets (input, LINE_MAX_LEN, _filein) == nullptr)
145 {
146 result = false;
147 break;
148 }
149 }
150 // COLOR
151 else if (this->Leqi (token, const_cast<char*>("color")))
152 {
153 sscanf(next, "%*s %f %f %f %f", &r1, &r2, &r3, &r4);
154 }
155 // SOLID
156 else if (this->Leqi (token, const_cast<char*>("solid")))
157 {
158 }
159 // ENDSOLID
160 else if (this->Leqi (token, const_cast<char*>("endsolid")))
161 {
162 }
163 // Unexpected or unrecognized.
164 else
165 {
166 /*printf("\n");
167 printf("stl - Fatal error!\n");
168 printf(" Unrecognized first word on line.\n");
169 */
170 result = false;
171 break;
172 }
173 }
174
175 result = subMesh.VertexCount() > 0;
176
177 if (result)
178 _mesh->AddSubMesh(subMesh);
179
180 return result;
181 }
182
183 //////////////////////////////////////////////////
ReadBinary(FILE * _filein,Mesh * _mesh)184 bool STLLoader::ReadBinary(FILE *_filein, Mesh *_mesh)
185 {
186 int i;
187 int iface;
188 int face_num;
189
190 SubMesh subMesh;
191
192 // 80 byte Header.
193 for (i = 0; i < 80; ++i)
194 fgetc(_filein);
195
196 // Number of faces.
197 face_num = this->LongIntRead(_filein);
198
199 ignition::math::Vector3d normal;
200 ignition::math::Vector3d vertex;
201
202 // For each (triangular) face,
203 // components of normal vector,
204 // coordinates of three vertices,
205 // 2 byte "attribute".
206 for (iface = 0; iface < face_num; iface++)
207 {
208 if (!this->FloatRead(_filein, normal.X()))
209 return false;
210 if (!this->FloatRead(_filein, normal.Y()))
211 return false;
212 if (!this->FloatRead(_filein, normal.Z()))
213 return false;
214
215 if (!this->FloatRead(_filein, vertex.X()))
216 return false;
217 if (!this->FloatRead(_filein, vertex.Y()))
218 return false;
219 if (!this->FloatRead(_filein, vertex.Z()))
220 return false;
221
222 subMesh.AddVertex(vertex);
223 subMesh.AddNormal(normal);
224 subMesh.AddIndex(subMesh.VertexCount()-1);
225
226 if (!this->FloatRead(_filein, vertex.X()))
227 return false;
228 if (!this->FloatRead(_filein, vertex.Y()))
229 return false;
230 if (!this->FloatRead(_filein, vertex.Z()))
231 return false;
232 subMesh.AddVertex(vertex);
233 subMesh.AddNormal(normal);
234 subMesh.AddIndex(subMesh.VertexCount()-1);
235
236 if (!this->FloatRead(_filein, vertex.X()))
237 return false;
238 if (!this->FloatRead(_filein, vertex.Y()))
239 return false;
240 if (!this->FloatRead(_filein, vertex.Z()))
241 return false;
242 subMesh.AddVertex(vertex);
243 subMesh.AddNormal(normal);
244 subMesh.AddIndex(subMesh.VertexCount()-1);
245
246 uint16_t shortTmp;
247 if (!ShortIntRead(_filein, shortTmp))
248 return false;
249 }
250
251 _mesh->AddSubMesh(subMesh);
252 return true;
253 }
254
255 //////////////////////////////////////////////////
Leqi(char * _string1,char * _string2)256 bool STLLoader::Leqi(char* _string1, char* _string2)
257 {
258 int i;
259 int nchar;
260 int nchar1;
261 int nchar2;
262
263 nchar1 = strlen(_string1);
264 nchar2 = strlen(_string2);
265
266 if (nchar1 < nchar2)
267 nchar = nchar1;
268 else
269 nchar = nchar2;
270
271 // The strings are not equal if they differ over their common length.
272 for (i = 0; i < nchar; ++i)
273 if (toupper (_string1[i]) != toupper (_string2[i]))
274 return false;
275
276 // The strings are not equal if the longer one includes nonblanks in the tail.
277 if (nchar1 > nchar)
278 {
279 for (i = nchar; i < nchar1; ++i)
280 if (_string1[i] != ' ')
281 return false;
282 }
283 else if (nchar2 > nchar)
284 {
285 for (i = nchar; i < nchar2; ++i)
286 if (_string2[i] != ' ')
287 return false;
288 }
289
290 return true;
291 }
292
293 //////////////////////////////////////////////////
RcolFind(float _a[][COR3_MAX],int _m,int _n,float _r[])294 int STLLoader::RcolFind(float _a[][COR3_MAX], int _m, int _n, float _r[])
295 {
296 int i;
297 int icol;
298 int j;
299
300 icol = -1;
301
302 for (j = 0; j < _n; ++j)
303 {
304 for (i = 0; i < _m; ++i)
305 {
306 if (!ignition::math::equal(_a[i][j], _r[i]))
307 break;
308 if (i == _m-1)
309 return j;
310 }
311 }
312
313 return icol;
314 }
315
316 //////////////////////////////////////////////////
FloatRead(FILE * _filein,double & _value)317 bool STLLoader::FloatRead(FILE *_filein, double &_value)
318 {
319 float v;
320 if (fread (&v, sizeof(v), 1, _filein) == 0)
321 return false;
322
323 _value = v;
324 return true;
325 }
326
327 //////////////////////////////////////////////////
LongIntRead(FILE * _filein)328 uint32_t STLLoader::LongIntRead(FILE *_filein)
329 {
330 union
331 {
332 uint32_t yint;
333 char ychar[4];
334 } y;
335
336 y.ychar[0] = fgetc(_filein);
337 y.ychar[1] = fgetc(_filein);
338 y.ychar[2] = fgetc(_filein);
339 y.ychar[3] = fgetc(_filein);
340
341 return y.yint;
342 }
343
344 //////////////////////////////////////////////////
ShortIntRead(FILE * _filein,uint16_t & _value)345 bool STLLoader::ShortIntRead(FILE *_filein, uint16_t &_value)
346 {
347 uint8_t c1;
348 uint8_t c2;
349
350 c1 = fgetc(_filein);
351 c2 = fgetc(_filein);
352
353 _value = c1 | (c2 << 8);
354
355 return true;
356 }
357
358
359