cis_config: /root/cis_interface/cis_interface/cis_interface/serialize/PlySerialize.h Source File

cis_config

PlySerialize.h
1 #ifndef CISPLYSERIALIZE_H_
2 #define CISPLYSERIALIZE_H_
3
4 #include <../tools.h>
5
6 #ifdef __cplusplus /* If this is a C++ compiler, use C linkage */
7 extern "C" {
8 #endif
9
11 typedef struct ply_t {
12 int nvert;
13 int nface;
14 float **vertices;
15 int **faces;
16 int **vertex_colors;
17 int *nvert_in_face;
18 } ply_t;
19
24 static inline
25 ply_t init_ply() {
26 ply_t x;
27 x.nvert = 0;
28 x.nface = 0;
29 x.vertices = NULL;
30 x.faces = NULL;
31 x.vertex_colors = NULL;
32 x.nvert_in_face = NULL;
33 return x;
34 };
35
40 static inline
41 void free_ply(ply_t *p) {
42 int i;
43 if (p->vertices != NULL) {
44 for (i = 0; i < p->nvert; i++) {
45 if (p->vertices[i] != NULL) {
46 free(p->vertices[i]);
47 p->vertices[i] = NULL;
48 }
49 }
50 free(p->vertices);
51 p->vertices = NULL;
52 }
53 if (p->vertex_colors != NULL) {
54 for (i = 0; i < p->nvert; i++) {
55 if (p->vertex_colors[i] != NULL) {
56 free(p->vertex_colors[i]);
57 p->vertex_colors[i] = NULL;
58 }
59 }
60 free(p->vertex_colors);
61 p->vertex_colors = NULL;
62 }
63 if (p->faces != NULL) {
64 for (i = 0; i < p->nface; i++) {
65 if (p->faces[i] != NULL) {
66 free(p->faces[i]);
67 p->faces[i] = NULL;
68 }
69 }
70 free(p->faces);
71 p->faces = NULL;
72 }
73 if (p->nvert_in_face != NULL) {
74 free(p->nvert_in_face);
75 p->nvert_in_face = NULL;
76 }
77 };
78
87 static inline
88 int alloc_ply(ply_t *p, int nvert, int nface, int do_color) {
89 int i;
90 free_ply(p); // Ensure that existing data is freed
91 p->nvert = nvert;
92 p->nface = nface;
93 // Allocate vertices
94 float **new_vert = (float**)malloc(p->nvert*sizeof(float*));
95 if (new_vert == NULL) {
96 cislog_error("alloc_ply: Failed to allocate vertices.");
97 free_ply(p);
98 return -1;
99 }
100 p->vertices = new_vert;
101 for (i = 0; i < p->nvert; i++) {
102 float *ivert = (float*)malloc(3*sizeof(float));
103 if (ivert == NULL) {
104 cislog_error("alloc_ply: Failed to allocate vertex %d.", i);
105 free_ply(p);
106 return -1;
107 }
108 p->vertices[i] = ivert;
109 }
110 cislog_debug("alloc_ply: Allocated %d vertices.", nvert);
111 // Allocate vertex colors
112 if (do_color) {
113 int **new_vert = (int**)malloc(p->nvert*sizeof(int*));
114 if (new_vert == NULL) {
115 cislog_error("alloc_ply: Failed to allocate vertex_colors.");
116 free_ply(p);
117 return -1;
118 }
119 p->vertex_colors = new_vert;
120 for (i = 0; i < p->nvert; i++) {
121 int *ivert = (int*)malloc(3*sizeof(int));
122 if (ivert == NULL) {
123 cislog_error("alloc_ply: Failed to allocate vertex color %d.", i);
124 free_ply(p);
125 return -1;
126 }
127 p->vertex_colors[i] = ivert;
128 }
129 cislog_debug("alloc_ply: Allocated %d vertex colors.", nvert);
130 }
131 // Allocate faces
132 int **new_face = (int**)malloc(p->nface*sizeof(int*));
133 if (new_face == NULL) {
134 cislog_error("alloc_ply: Failed to allocate faces.");
135 free_ply(p);
136 return -1;
137 }
138 p->faces = new_face;
139 for (i = 0; i < p->nface; i++) {
140 p->faces[i] = NULL;
141 /* int *iface = (int*)malloc(3*sizeof(int)); */
142 /* if (iface == NULL) { */
143 /* cislog_error("alloc_ply: Failed to allocate face %d.", i); */
144 /* free_ply(p); */
145 /* return -1; */
146 /* } */
147 }
148 cislog_debug("alloc_ply: Allocated %d faces.", nface);
149 // Allocate nvert_in_face
150 int *new_nvert = (int*)malloc(p->nface*sizeof(int));
151 if (new_nvert == NULL) {
152 cislog_error("alloc_ply: Failed to allocate nvert_in_face.");
153 free_ply(p);
154 return -1;
155 }
156 p->nvert_in_face = new_nvert;
157 for (i = 0; i < p->nface; i++) {
158 p->nvert_in_face[i] = 0;
159 }
160 cislog_debug("alloc_ply: Allocate for %d vertices and %d faces.",
161 p->nvert, p->nface);
162 return 0;
163 };
164
175 static inline
176 int serialize_ply(const seri_t s, char *buf, const size_t buf_size,
177 int *args_used, va_list ap) {
178 args_used[0] = 0;
179 int msg_len = 0;
180 int ilen;
181 // Get argument
182 ply_t p = va_arg(ap, ply_t);
183 args_used[0] = 1;
184 // Format header
185 char header_format[500] = "ply\n"
186 "format ascii 1.0\n"
187 "comment author cis_auto\n"
188 "comment File generated by cis_interface\n"
189 "element vertex %d\n"
190 "property float x\n"
191 "property float y\n"
192 "property float z\n";
193 if (p.vertex_colors != NULL) {
194 char header_format_colors[100] = "property uchar diffuse_red\n"
195 "property uchar diffuse_green\n"
196 "property uchar diffuse_blue\n";
197 strcat(header_format, header_format_colors);
198 }
199 char header_format2[100] = "element face %d\n"
200 "property list uchar int vertex_indices\n"
201 "end_header\n";
202 strcat(header_format, header_format2);
203 ilen = snprintf(buf, buf_size, header_format, p.nvert, p.nface);
204 if (ilen < 0) {
205 cislog_error("serialize_ply: Error formatting header.");
206 return -1;
207 } else if (ilen >= buf_size) {
208 cislog_error("serialize_ply: Buffer (size = %d) is not large "
209 "enough to contain the header (size = %d).", buf_size, ilen);
210 return msg_len + ilen;
211 }
212 msg_len = msg_len + ilen;
213 // Add vertex information
214 int i, j;
215 for (i = 0; i < p.nvert; i++) {
216 if (p.vertex_colors != NULL) {
217 ilen = snprintf(buf + msg_len, buf_size - msg_len, "%f %f %f %d %d %d\n",
218 p.vertices[i][0], p.vertices[i][1], p.vertices[i][2],
219 p.vertex_colors[i][0], p.vertex_colors[i][1], p.vertex_colors[i][2]);
220 } else {
221 ilen = snprintf(buf + msg_len, buf_size - msg_len, "%f %f %f\n",
222 p.vertices[i][0], p.vertices[i][1], p.vertices[i][2]);
223 }
224 if (ilen < 0) {
225 cislog_error("serialize_ply: Error formatting vertex %d.", i);
226 return -1;
227 } else if (ilen >= (buf_size - msg_len)) {
228 cislog_error("serialize_ply: Buffer (size = %d) is not large "
229 "enough to contain vertex %d (size = %d).",
230 buf_size, i, ilen + msg_len);
231 return msg_len + ilen;
232 }
233 msg_len = msg_len + ilen;
234 }
235 // Add face information
236 for (i = 0; i < p.nface; i++) {
237 ilen = snprintf(buf + msg_len, buf_size - msg_len, "%d", p.nvert_in_face[i]);
238 if (ilen < 0) {
239 cislog_error("serialize_ply: Error formatting number of verts for face %d.", i);
240 return -1;
241 } else if (ilen > (buf_size - msg_len)) {
242 cislog_error("serialize_ply: Buffer (size = %d) is not large "
243 "enough to contain number of verts for face %d (size = %d).",
244 buf_size, i, ilen + msg_len);
245 return msg_len + ilen;
246 }
247 msg_len = msg_len + ilen;
248 for (j = 0; j < p.nvert_in_face[i]; j++) {
249 ilen = snprintf(buf + msg_len, buf_size - msg_len, " %d", p.faces[i][j]);
250 if (ilen < 0) {
251 cislog_error("serialize_ply: Error formatting element %d of face %d.", j, i);
252 return -1;
253 } else if (ilen > (buf_size - msg_len)) {
254 cislog_error("serialize_ply: Buffer (size = %d) is not large "
255 "enough to contain element %d of face %d (size = %d).",
256 buf_size, j, i, ilen + msg_len);
257 return msg_len + ilen;
258 }
259 msg_len = msg_len + ilen;
260 }
261 ilen = snprintf(buf + msg_len, buf_size - msg_len, "\n");
262 if (ilen < 0) {
263 cislog_error("serialize_ply: Error formatting newline for face %d.", i);
264 return -1;
265 } else if (ilen > (buf_size - msg_len)) {
266 cislog_error("serialize_ply: Buffer (size = %d) is not large "
267 "enough to contain newline for face %d (size = %d).",
268 buf_size, i, ilen + msg_len);
269 return msg_len + ilen;
270 }
271 msg_len = msg_len + ilen;
272 }
273 return msg_len;
274 };
275
276
285 static inline
286 int deserialize_ply(const seri_t s, const char *buf, const size_t buf_siz,
287 va_list ap) {
288 int out = 1;
289 int do_colors = 0;
290 int n_sub_matches;
291 size_t *sind = NULL;
292 size_t *eind = NULL;
293 size_t *sind_body = NULL;
294 size_t *eind_body = NULL;
295 size_t value_size;
296 char value[100];
297 size_t begin_body = 0;
298 int nlines = 0;
299 int i, j;
300 int nvert = 0, nface = 0;
301 int line_no;
302 size_t line_size;
303 char iline[100];
304 // Get argument
305 ply_t *p = va_arg(ap, ply_t*);
306 // Get info from the header.
307 // Number of vertices
308 if (out > 0) {
309 n_sub_matches = find_matches("element vertex ([[:digit:]]+)\n", buf, &sind, &eind);
310 if (n_sub_matches < 2) {
311 cislog_error("deserialize_ply: Could not locate number of vertices in ply header.");
312 out = -1;
313 }
314 value_size = eind[1] - sind[1];
315 memcpy(value, buf + sind[1], value_size);
316 value[value_size] = '\0';
317 nvert = atoi(value);
318 }
319 // Number of faces
320 if (out > 0) {
321 n_sub_matches = find_matches("element face ([[:digit:]]+)\n", buf, &sind, &eind);
322 if (n_sub_matches < 2) {
323 cislog_error("deserialize_ply: Could not locate number of faces in ply header.");
324 out = -1;
325 }
326 value_size = eind[1] - sind[1];
327 memcpy(value, buf + sind[1], value_size);
328 value[value_size] = '\0';
329 nface = atoi(value);
330 }
331 // Color
332 if (out > 0) {
333 n_sub_matches = find_matches("green", buf, &sind, &eind);
334 if (n_sub_matches != 0) {
335 do_colors = 1;
336 }
337 }
338 // End of header
339 if (out > 0) {
340 n_sub_matches = find_matches("end_header\n", buf, &sind, &eind);
341 if (n_sub_matches < 1) {
342 cislog_error("deserialize_ply: Could not locate end of header.");
343 out = -1;
344 } else {
345 begin_body = eind[0];
346 }
347 }
348 // Locate lines
349 if (out > 0) {
350 int nlines_expected = nvert + nface;
351 nlines = 0;
352 sind_body = (size_t*)realloc(sind_body, (nlines_expected+1)*sizeof(size_t));
353 eind_body = (size_t*)realloc(eind_body, (nlines_expected+1)*sizeof(size_t));
354 size_t cur_pos = begin_body;
355 while (1) {
356 n_sub_matches = find_matches("([^\n]*)\n", buf + cur_pos, &sind, &eind);
357 if (n_sub_matches < 2) {
358 break;
359 }
360 if (nlines > nlines_expected) {
361 break;
362 // nlines_expected = nlines_expected + 50;
363 // sind_body = (size_t*)realloc(sind_body, (nlines_expected+1)*sizeof(size_t));
364 // eind_body = (size_t*)realloc(eind_body, (nlines_expected+1)*sizeof(size_t));
365 }
366 sind_body[nlines] = cur_pos + sind[1];
367 eind_body[nlines] = cur_pos + eind[1];
368 nlines++;
369 cur_pos = cur_pos + eind[0];
370 }
371 if ((nvert + nface) > nlines) {
372 cislog_error("deserialize_ply: Not enough lines (%d) for %d vertices "
373 "and %d faces.",
374 nlines, nvert, nface);
375 out = -1;
376 }
377 }
378 // Allocate
379 if (out > 0) {
380 int ret = alloc_ply(p, nvert, nface, do_colors);
381 if (ret < 0) {
382 cislog_error("deserialize_ply: Error allocating ply structure.");
383 out = -1;
384 }
385 }
386 // Get vertices
387 if (out > 0) {
388 int nexpected = 3;
389 if (do_colors) {
390 nexpected = 6;
391 }
392 char vert_re[80] = "([^ ]+) ([^ ]+) ([^ ]+)";
393 // Parse each line
394 for (i = 0; i < p->nvert; i++) {
395 line_no = i;
396 line_size = eind_body[line_no] - sind_body[line_no];
397 memcpy(iline, buf + sind_body[line_no], line_size);
398 iline[line_size] = '\0';
399 n_sub_matches = find_matches(vert_re, iline, &sind, &eind);
400 if (n_sub_matches != nexpected + 1) {
401 cislog_error("deserialize_ply: Vertex should contain %d entries. "
402 "%d were found", nexpected, n_sub_matches - 1);
403 out = -1;
404 break;
405 } else {
406 for (j = 0; j < 3; j++) {
407 p->vertices[i][j] = atof(iline + sind[j + 1]);
408 }
409 if (do_colors) {
410 for (j = 0; j < 3; j++) {
411 p->vertex_colors[i][j] = atoi(iline + sind[j + 4]);
412 }
413 }
414 }
415 }
416 }
417 // Get faces
418 if (out > 0) {
419 int nexpected = 0;
420 // Parse each line
421 for (i = 0; i < p->nface; i++) {
422 line_no = i + p->nvert;
423 line_size = eind_body[line_no] - sind_body[line_no];
424 memcpy(iline, buf + sind_body[line_no], line_size);
425 iline[line_size] = '\0';
426 nexpected = atoi(iline);
427 p->nvert_in_face[i] = nexpected;
428 char face_re[80] = "([^ ]+)";
429 for (j = 0; j < nexpected; j++) {
430 strcat(face_re, " ([^ ]+)");
431 }
432 n_sub_matches = find_matches(face_re, iline, &sind, &eind);
433 if (n_sub_matches < (nexpected + 2)) {
434 cislog_error("deserialize_ply: Face should contain %d entries. "
435 "%d were found.", nexpected, n_sub_matches - 2);
436 out = -1;
437 break;
438 } else {
439 int *iface = (int*)realloc(p->faces[i], nexpected*sizeof(int));
440 if (iface == NULL) {
441 cislog_error("deserialize_ply: Could not allocate face %d.", i);
442 out = -1;
443 break;
444 } else {
445 p->faces[i] = iface;
446 for (j = 0; j < nexpected; j++) {
447 p->faces[i][j] = atoi(iline + sind[j + 2]);
448 }
449 }
450 }
451 }
452 }
453 // Return
454 if (sind != NULL) free(sind);
455 if (eind != NULL) free(eind);
456 if (sind_body != NULL) free(sind_body);
457 if (eind_body != NULL) free(eind_body);
458 if (out < 0) {
459 free_ply(p);
460 }
461 return out;
462 };
463
464 #ifdef __cplusplus /* If this is a C++ compiler, end C linkage */
465 }
466 #endif
467
468 #endif /*CISPLYSERIALIZE_H_*/
ply_t::faces
int ** faces
Indices of the vertices composing each face.
Definition: PlySerialize.h:15
seri_t
Serializer structure.
Definition: SerializeBase.h:19
ply_t::nface
int nface
Number faces.
Definition: PlySerialize.h:13
ply_t::vertex_colors
int ** vertex_colors
RGB colors of each vertex.
Definition: PlySerialize.h:16
ply_t::nvert_in_face
int * nvert_in_face
Number of vertices in each face.
Definition: PlySerialize.h:17
ply_t
Ply structure.
Definition: PlySerialize.h:11
ply_t::nvert
int nvert
Number of vertices.
Definition: PlySerialize.h:12
ply_t::vertices
float ** vertices
X, Y, Z positions of vertices.
Definition: PlySerialize.h:14