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
18 #include <gts.h>
19
20 #include <string>
21
22 #include "ignition/common/Console.hh"
23 #include "ignition/common/Mesh.hh"
24 #include "ignition/common/SubMesh.hh"
25 #include "ignition/common/MeshCSG.hh"
26
27 using namespace ignition;
28 using namespace common;
29
30 //////////////////////////////////////////////////
MeshCSG()31 MeshCSG::MeshCSG()
32 {
33 }
34
35 //////////////////////////////////////////////////
~MeshCSG()36 MeshCSG::~MeshCSG()
37 {
38 }
39
40 //////////////////////////////////////////////////
MergeVertices(GPtrArray * _vertices,double _epsilon)41 void MeshCSG::MergeVertices(GPtrArray * _vertices, double _epsilon)
42 {
43 GPtrArray *array;
44 GNode *kdtree;
45 GtsVertex **verticesData = reinterpret_cast<GtsVertex **>(_vertices->pdata);
46 array = g_ptr_array_new();
47 for (unsigned int i = 0; i < _vertices->len; ++i)
48 g_ptr_array_add(array, verticesData[i]);
49 kdtree = gts_kdtree_new(array, nullptr);
50 g_ptr_array_free(array, true);
51
52 // for each vertex, do a bbox kdtree search to find nearby vertices within
53 // _epsilon, merge vertices if they are within the bbox
54 for (unsigned int i = 0; i < _vertices->len; ++i)
55 {
56 GtsVertex *v = reinterpret_cast<GtsVertex *>(verticesData[i]);
57
58 // make sure vertex v is active (because they could be marked inactive
59 // due to previous merge operation)
60 if (!GTS_OBJECT (v)->reserved)
61 {
62 GtsBBox *bbox;
63 GSList *selected, *j;
64
65 // build bounding box
66 bbox = gts_bbox_new(gts_bbox_class(),
67 v,
68 GTS_POINT(v)->x - _epsilon,
69 GTS_POINT(v)->y - _epsilon,
70 GTS_POINT(v)->z - _epsilon,
71 GTS_POINT(v)->x + _epsilon,
72 GTS_POINT(v)->y + _epsilon,
73 GTS_POINT(v)->z + _epsilon);
74
75 // select vertices which are inside bbox using kdtree
76 j = selected = gts_kdtree_range(kdtree, bbox, nullptr);
77 while (j)
78 {
79 GtsVertex *sv = reinterpret_cast<GtsVertex *>(j->data);
80 // mark sv as inactive (merged)
81 if (sv != v && !GTS_OBJECT(sv)->reserved)
82 GTS_OBJECT(sv)->reserved = v;
83 j = j->next;
84 }
85 g_slist_free(selected);
86 gts_object_destroy(GTS_OBJECT(bbox));
87 }
88 }
89
90 gts_kdtree_destroy(kdtree);
91
92 // destroy inactive vertices
93 // we want to control vertex destruction
94 gts_allow_floating_vertices = true;
95 for (unsigned int i = 0; i < _vertices->len; ++i)
96 {
97 GtsVertex * v = reinterpret_cast<GtsVertex *>(verticesData[i]);
98 // v is inactive
99 if (GTS_OBJECT(v)->reserved)
100 {
101 verticesData[i] =
102 reinterpret_cast<GtsVertex *>(GTS_OBJECT(v)->reserved);
103 gts_object_destroy(GTS_OBJECT(v));
104 }
105 }
106 gts_allow_floating_vertices = false;
107 }
108
109 //////////////////////////////////////////////////
FillVertex(GtsPoint * _p,gpointer * _data)110 static int FillVertex(GtsPoint *_p, gpointer *_data)
111 {
112 // create a vertex from GTS_POINT and add it to the submesh
113 SubMesh *subMesh = reinterpret_cast<SubMesh *>(_data[0]);
114 GHashTable* vIndex = reinterpret_cast<GHashTable *>(_data[2]);
115 subMesh->AddVertex(GTS_POINT(_p)->x, GTS_POINT(_p)->y, GTS_POINT(_p)->z);
116 // fill the hash table which will later be used for adding indices to the
117 // submesh in the FillFace function.
118 g_hash_table_insert(vIndex, _p,
119 GUINT_TO_POINTER((*(reinterpret_cast<guint *>(_data[1])))++));
120 return 0;
121 }
122
123 //////////////////////////////////////////////////
FillFace(GtsTriangle * _t,gpointer * _data)124 static int FillFace(GtsTriangle *_t, gpointer *_data)
125 {
126 SubMesh *subMesh = reinterpret_cast<SubMesh *>(_data[0]);
127 GHashTable* vIndex = reinterpret_cast<GHashTable *>(_data[2]);
128
129 GtsVertex * v1, * v2, * v3;
130 gts_triangle_vertices(_t, &v1, &v2, &v3);
131
132 subMesh->AddIndex(GPOINTER_TO_UINT(g_hash_table_lookup(vIndex, v1)));
133 subMesh->AddIndex(GPOINTER_TO_UINT(g_hash_table_lookup(vIndex, v2)));
134 subMesh->AddIndex(GPOINTER_TO_UINT(g_hash_table_lookup(vIndex, v3)));
135 return 0;
136 }
137
138 //////////////////////////////////////////////////////////////////////////
TriangleRevert(GtsTriangle * _t,void *)139 static int TriangleRevert(GtsTriangle *_t, void *)
140 {
141 gts_triangle_revert(_t);
142 return 0;
143 }
144
145 //////////////////////////////////////////////////
CreateBoolean(const Mesh * _m1,const Mesh * _m2,int _operation,const ignition::math::Pose3d & _offset)146 Mesh *MeshCSG::CreateBoolean(const Mesh *_m1, const Mesh *_m2, int _operation,
147 const ignition::math::Pose3d &_offset)
148 {
149 GtsSurface *s1, *s2, *s3;
150 GtsSurfaceInter *si;
151 GNode *tree1, *tree2;
152
153 gboolean closed = true;
154 bool isOpen1 = false;
155 bool isOpen2 = false;
156
157 s1 = gts_surface_new(gts_surface_class(), gts_face_class(), gts_edge_class(),
158 gts_vertex_class());
159 s2 = gts_surface_new(gts_surface_class(), gts_face_class(), gts_edge_class(),
160 gts_vertex_class());
161 s3 = gts_surface_new(gts_surface_class(), gts_face_class(), gts_edge_class(),
162 gts_vertex_class());
163
164 this->ConvertMeshToGTS(_m1, s1);
165
166 if (_offset != ignition::math::Pose3d::Zero)
167 {
168 Mesh *m2 = new Mesh();
169 for (unsigned int i = 0; i < _m2->SubMeshCount(); ++i)
170 {
171 SubMesh m2SubMesh;
172
173 auto subMesh = _m2->SubMeshByIndex(i).lock();
174 if (subMesh->VertexCount() <= 2)
175 continue;
176 for (unsigned int j = 0; j < subMesh->VertexCount(); ++j)
177 {
178 m2SubMesh.AddVertex(_offset.Pos() +
179 _offset.Rot()*subMesh->Vertex(j));
180 }
181 for (unsigned int j = 0; j < subMesh->IndexCount(); ++j)
182 {
183 m2SubMesh.AddIndex(subMesh->Index(j));
184 }
185
186 m2->AddSubMesh(m2SubMesh);
187 }
188 this->ConvertMeshToGTS(m2, s2);
189 delete m2;
190 }
191 else
192 {
193 this->ConvertMeshToGTS(_m2, s2);
194 }
195
196 // build bounding box tree for first surface
197 tree1 = gts_bb_tree_surface(s1);
198 isOpen1 = gts_surface_volume(s1) < 0. ? true : false;
199
200 // build bounding box tree for second surface
201 tree2 = gts_bb_tree_surface(s2);
202 isOpen2 = gts_surface_volume(s2) < 0. ? true : false;
203
204 si = gts_surface_inter_new(gts_surface_inter_class(), s1, s2, tree1, tree2,
205 isOpen1, isOpen2);
206 if (!gts_surface_inter_check(si, &closed))
207 {
208 ignerr << "si is not an orientable manifold\n";
209 return nullptr;
210 }
211
212 if (!closed)
213 {
214 ignerr << "the intersection of " << _m1->Name() << " and "
215 << _m2->Name() << " is not a closed curve\n";
216 return nullptr;
217 }
218
219 // FILE *output1 = fopen("output3.gts", "w");
220 // gts_surface_write(s1, output1);
221 // fclose(output1);
222
223 // FILE *output2 = fopen("output4.gts", "w");
224 // gts_surface_write(s2, output2);
225 // fclose(output2);
226
227 if (_operation == MeshCSG::UNION)
228 {
229 gts_surface_inter_boolean(si, s3, GTS_1_OUT_2);
230 gts_surface_inter_boolean(si, s3, GTS_2_OUT_1);
231 }
232 else if (_operation == MeshCSG::INTERSECTION)
233 {
234 gts_surface_inter_boolean(si, s3, GTS_1_IN_2);
235 gts_surface_inter_boolean(si, s3, GTS_2_IN_1);
236 }
237 else if (_operation == MeshCSG::DIFFERENCE)
238 {
239 gts_surface_inter_boolean(si, s3, GTS_1_OUT_2);
240 gts_surface_inter_boolean(si, s3, GTS_2_IN_1);
241 gts_surface_foreach_face(si->s2, (GtsFunc) TriangleRevert, nullptr);
242 gts_surface_foreach_face(s2, (GtsFunc) TriangleRevert, nullptr);
243 }
244
245 // FILE *output = fopen("output.gts", "w");
246 // gts_surface_write(s3, output);
247 // fclose(output);
248
249 // create the boolean mesh
250 Mesh *mesh = new Mesh();
251 SubMesh subMesh;
252
253 // fill the submesh with data generated by GTS
254 unsigned int n = 0;
255 gpointer data[3];
256 GHashTable *vIndex = g_hash_table_new(nullptr, nullptr);
257
258 data[0] = &subMesh;
259 data[1] = &n;
260 data[2] = vIndex;
261 gts_surface_foreach_vertex(s3, (GtsFunc)FillVertex, data);
262 n = 0;
263 gts_surface_foreach_face(s3, (GtsFunc)FillFace, data);
264 g_hash_table_destroy(vIndex);
265
266 mesh->RecalculateNormals();
267
268 // destroy surfaces
269 gts_object_destroy(GTS_OBJECT(s1));
270 gts_object_destroy(GTS_OBJECT(s2));
271 gts_object_destroy(GTS_OBJECT(s3));
272 gts_object_destroy(GTS_OBJECT(si));
273
274 // destroy bounding box trees (including bounding boxes)
275 gts_bb_tree_destroy(tree1, true);
276 gts_bb_tree_destroy(tree2, true);
277
278 mesh->AddSubMesh(subMesh);
279 return mesh;
280 }
281
282 //////////////////////////////////////////////////
ConvertMeshToGTS(const Mesh * _mesh,GtsSurface * _surface)283 void MeshCSG::ConvertMeshToGTS(const Mesh *_mesh, GtsSurface *_surface)
284 {
285 if (!_surface)
286 {
287 ignerr << _mesh->Name() << ": Surface is null\n";
288 // _surface = gts_surface_new(gts_surface_class(), gts_face_class(),
289 // gts_edge_class(), gts_vertex_class());
290 return;
291 }
292
293 GPtrArray *vertices = g_ptr_array_new();
294
295 for (unsigned int i = 0; i < _mesh->SubMeshCount(); ++i)
296 {
297 auto subMesh = _mesh->SubMeshByIndex(i).lock();
298 unsigned int indexCount = subMesh->IndexCount();
299 if (subMesh->VertexCount() <= 2)
300 continue;
301
302 for (unsigned int j = 0; j < subMesh->VertexCount(); ++j)
303 {
304 ignition::math::Vector3d vertex = subMesh->Vertex(j);
305 g_ptr_array_add(vertices, gts_vertex_new(gts_vertex_class(), vertex.X(),
306 vertex.Y(), vertex.Z()));
307 }
308
309 // merge duplicate vertices, otherwise gts produces undesirable results
310 this->MergeVertices(vertices, 0.01);
311
312 GtsVertex **verticesData =
313 reinterpret_cast<GtsVertex **>(vertices->pdata);
314 for (unsigned int j = 0; j < indexCount/3; ++j)
315 {
316 GtsEdge *e1 = GTS_EDGE(gts_vertices_are_connected(
317 verticesData[subMesh->Index(3*j)],
318 verticesData[subMesh->Index(3*j+1)]));
319 GtsEdge *e2 = GTS_EDGE(gts_vertices_are_connected(
320 verticesData[subMesh->Index(3*j+1)],
321 verticesData[subMesh->Index(3*j+2)]));
322 GtsEdge *e3 = GTS_EDGE(gts_vertices_are_connected(
323 verticesData[subMesh->Index(3*j+2)],
324 verticesData[subMesh->Index(3*j)]));
325 if (e1 == nullptr && verticesData[subMesh->Index(3*j)]
326 != verticesData[subMesh->Index(3*j+1)])
327 {
328 e1 = gts_edge_new(_surface->edge_class,
329 verticesData[subMesh->Index(3*j)],
330 verticesData[subMesh->Index(3*j+1)]);
331 }
332 if (e2 == nullptr && verticesData[subMesh->Index(3*j+1)]
333 != verticesData[subMesh->Index(3*j+2)])
334 {
335 e2 = gts_edge_new(_surface->edge_class,
336 verticesData[subMesh->Index(3*j+1)],
337 verticesData[subMesh->Index(3*j+2)]);
338 }
339 if (e3 == nullptr && verticesData[subMesh->Index(3*j+2)]
340 != verticesData[subMesh->Index(3*j)])
341 {
342 e3 = gts_edge_new(_surface->edge_class,
343 verticesData[subMesh->Index(3*j+2)],
344 verticesData[subMesh->Index(3*j)]);
345 }
346 if (e1 != nullptr && e2 != nullptr && e3 != nullptr)
347 {
348 gts_surface_add_face(_surface, gts_face_new(_surface->face_class, e1,
349 e2, e3));
350 }
351 else
352 {
353 ignwarn << _mesh->Name() << ": Ignoring degenerate facet!";
354 }
355 }
356 }
357 }
358