345 lines
11 KiB
C++
345 lines
11 KiB
C++
/*
|
|
* Copyright (C) 2012 Open Source Robotics Foundation
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*
|
|
*/
|
|
|
|
#include <string>
|
|
#include <gts.h>
|
|
|
|
#include "gazebo/common/Assert.hh"
|
|
#include "gazebo/common/CommonIface.hh"
|
|
#include "gazebo/common/Console.hh"
|
|
#include "gazebo/common/Exception.hh"
|
|
#include "gazebo/common/Mesh.hh"
|
|
#include "gazebo/common/MeshCSG.hh"
|
|
#include "gazebo/common/MeshManager.hh"
|
|
|
|
using namespace gazebo;
|
|
using namespace common;
|
|
|
|
//////////////////////////////////////////////////
|
|
MeshCSG::MeshCSG()
|
|
{
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
MeshCSG::~MeshCSG()
|
|
{
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
void MeshCSG::MergeVertices(GPtrArray * _vertices, double _epsilon)
|
|
{
|
|
GPtrArray *array;
|
|
GNode *kdtree;
|
|
GtsVertex **verticesData = reinterpret_cast<GtsVertex **>(_vertices->pdata);
|
|
array = g_ptr_array_new();
|
|
for (unsigned int i = 0; i < _vertices->len; ++i)
|
|
g_ptr_array_add(array, verticesData[i]);
|
|
kdtree = gts_kdtree_new(array, NULL);
|
|
g_ptr_array_free(array, true);
|
|
|
|
// for each vertex, do a bbox kdtree search to find nearby vertices within
|
|
// _epsilon, merge vertices if they are within the bbox
|
|
for (unsigned int i = 0; i < _vertices->len; ++i)
|
|
{
|
|
GtsVertex *v = reinterpret_cast<GtsVertex *>(verticesData[i]);
|
|
|
|
// make sure vertex v is active (because they could be marked inactive
|
|
// due to previous merge operation)
|
|
if (!GTS_OBJECT (v)->reserved)
|
|
{
|
|
GtsBBox *bbox;
|
|
GSList *selected, *j;
|
|
|
|
// build bounding box
|
|
bbox = gts_bbox_new(gts_bbox_class(),
|
|
v,
|
|
GTS_POINT(v)->x - _epsilon,
|
|
GTS_POINT(v)->y - _epsilon,
|
|
GTS_POINT(v)->z - _epsilon,
|
|
GTS_POINT(v)->x + _epsilon,
|
|
GTS_POINT(v)->y + _epsilon,
|
|
GTS_POINT(v)->z + _epsilon);
|
|
|
|
// select vertices which are inside bbox using kdtree
|
|
j = selected = gts_kdtree_range(kdtree, bbox, NULL);
|
|
while (j)
|
|
{
|
|
GtsVertex *sv = reinterpret_cast<GtsVertex *>(j->data);
|
|
// mark sv as inactive (merged)
|
|
if (sv != v && !GTS_OBJECT(sv)->reserved)
|
|
GTS_OBJECT(sv)->reserved = v;
|
|
j = j->next;
|
|
}
|
|
g_slist_free(selected);
|
|
gts_object_destroy(GTS_OBJECT(bbox));
|
|
}
|
|
}
|
|
|
|
gts_kdtree_destroy(kdtree);
|
|
|
|
// destroy inactive vertices
|
|
// we want to control vertex destruction
|
|
gts_allow_floating_vertices = true;
|
|
for (unsigned int i = 0; i < _vertices->len; ++i)
|
|
{
|
|
GtsVertex * v = reinterpret_cast<GtsVertex *>(verticesData[i]);
|
|
// v is inactive
|
|
if (GTS_OBJECT(v)->reserved)
|
|
{
|
|
verticesData[i] =
|
|
reinterpret_cast<GtsVertex *>(GTS_OBJECT(v)->reserved);
|
|
gts_object_destroy(GTS_OBJECT(v));
|
|
}
|
|
}
|
|
gts_allow_floating_vertices = false;
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
static void FillVertex(GtsPoint *_p, gpointer *_data)
|
|
{
|
|
// create a Gazebo vertex from GTS_POINT and add it to the submesh
|
|
SubMesh *subMesh = reinterpret_cast<SubMesh *>(_data[0]);
|
|
GHashTable* vIndex = reinterpret_cast<GHashTable *>(_data[2]);
|
|
subMesh->AddVertex(GTS_POINT(_p)->x, GTS_POINT(_p)->y, GTS_POINT(_p)->z);
|
|
// fill the hash table which will later be used for adding indices to the
|
|
// submesh in the FillFace function.
|
|
g_hash_table_insert(vIndex, _p,
|
|
GUINT_TO_POINTER((*(reinterpret_cast<guint *>(_data[1])))++));
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
static void FillFace(GtsTriangle *_t, gpointer *_data)
|
|
{
|
|
SubMesh *subMesh = reinterpret_cast<SubMesh *>(_data[0]);
|
|
GHashTable* vIndex = reinterpret_cast<GHashTable *>(_data[2]);
|
|
|
|
GtsVertex * v1, * v2, * v3;
|
|
gts_triangle_vertices(_t, &v1, &v2, &v3);
|
|
|
|
subMesh->AddIndex(GPOINTER_TO_UINT(g_hash_table_lookup(vIndex, v1)));
|
|
subMesh->AddIndex(GPOINTER_TO_UINT(g_hash_table_lookup(vIndex, v2)));
|
|
subMesh->AddIndex(GPOINTER_TO_UINT(g_hash_table_lookup(vIndex, v3)));
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
Mesh *MeshCSG::CreateBoolean(const Mesh *_m1, const Mesh *_m2, int _operation,
|
|
const ignition::math::Pose3d &_offset)
|
|
{
|
|
GtsSurface *s1, *s2, *s3;
|
|
GtsSurfaceInter *si;
|
|
GNode *tree1, *tree2;
|
|
|
|
gboolean closed = true;
|
|
bool isOpen1 = false;
|
|
bool isOpen2 = false;
|
|
|
|
s1 = gts_surface_new(gts_surface_class(), gts_face_class(), gts_edge_class(),
|
|
gts_vertex_class());
|
|
s2 = gts_surface_new(gts_surface_class(), gts_face_class(), gts_edge_class(),
|
|
gts_vertex_class());
|
|
s3 = gts_surface_new(gts_surface_class(), gts_face_class(), gts_edge_class(),
|
|
gts_vertex_class());
|
|
|
|
this->ConvertMeshToGTS(_m1, s1);
|
|
|
|
if (_offset != ignition::math::Pose3d::Zero)
|
|
{
|
|
Mesh *m2 = new Mesh();
|
|
for (unsigned int i = 0; i < _m2->GetSubMeshCount(); ++i)
|
|
{
|
|
SubMesh *m2SubMesh = new SubMesh();
|
|
m2->AddSubMesh(m2SubMesh);
|
|
|
|
const SubMesh *subMesh = _m2->GetSubMesh(i);
|
|
if (subMesh->GetVertexCount() <= 2)
|
|
continue;
|
|
for (unsigned int j = 0; j < subMesh->GetVertexCount(); ++j)
|
|
{
|
|
m2SubMesh->AddVertex(_offset.Pos() +
|
|
_offset.Rot()*subMesh->Vertex(j));
|
|
}
|
|
for (unsigned int j = 0; j < subMesh->GetIndexCount(); ++j)
|
|
{
|
|
m2SubMesh->AddIndex(subMesh->GetIndex(j));
|
|
}
|
|
}
|
|
this->ConvertMeshToGTS(m2, s2);
|
|
delete m2;
|
|
}
|
|
else
|
|
{
|
|
this->ConvertMeshToGTS(_m2, s2);
|
|
}
|
|
|
|
// build bounding box tree for first surface
|
|
tree1 = gts_bb_tree_surface(s1);
|
|
isOpen1 = gts_surface_volume(s1) < 0. ? true : false;
|
|
|
|
// build bounding box tree for second surface
|
|
tree2 = gts_bb_tree_surface(s2);
|
|
isOpen2 = gts_surface_volume(s2) < 0. ? true : false;
|
|
|
|
si = gts_surface_inter_new(gts_surface_inter_class(), s1, s2, tree1, tree2,
|
|
isOpen1, isOpen2);
|
|
GZ_ASSERT(gts_surface_inter_check(si, &closed),
|
|
"si is not an orientable manifold");
|
|
if (!closed)
|
|
{
|
|
gzerr << "the intersection of " << _m1->GetName() << " and "
|
|
<< _m2->GetName() << " is not a closed curve\n";
|
|
return NULL;
|
|
}
|
|
|
|
// FILE *output1 = fopen("output3.gts", "w");
|
|
// gts_surface_write(s1, output1);
|
|
// fclose(output1);
|
|
|
|
// FILE *output2 = fopen("output4.gts", "w");
|
|
// gts_surface_write(s2, output2);
|
|
// fclose(output2);
|
|
|
|
if (_operation == MeshCSG::UNION)
|
|
{
|
|
gts_surface_inter_boolean(si, s3, GTS_1_OUT_2);
|
|
gts_surface_inter_boolean(si, s3, GTS_2_OUT_1);
|
|
}
|
|
else if (_operation == MeshCSG::INTERSECTION)
|
|
{
|
|
gts_surface_inter_boolean(si, s3, GTS_1_IN_2);
|
|
gts_surface_inter_boolean(si, s3, GTS_2_IN_1);
|
|
}
|
|
else if (_operation == MeshCSG::DIFFERENCE)
|
|
{
|
|
gts_surface_inter_boolean(si, s3, GTS_1_OUT_2);
|
|
gts_surface_inter_boolean(si, s3, GTS_2_IN_1);
|
|
gts_surface_foreach_face(si->s2, (GtsFunc) gts_triangle_revert, NULL);
|
|
gts_surface_foreach_face(s2, (GtsFunc) gts_triangle_revert, NULL);
|
|
}
|
|
|
|
// FILE *output = fopen("output.gts", "w");
|
|
// gts_surface_write(s3, output);
|
|
// fclose(output);
|
|
|
|
// create the boolean mesh
|
|
Mesh *mesh = new Mesh();
|
|
SubMesh *subMesh = new SubMesh();
|
|
mesh->AddSubMesh(subMesh);
|
|
|
|
// fill the submesh with data generated by GTS
|
|
unsigned int n = 0;
|
|
gpointer data[3];
|
|
GHashTable *vIndex = g_hash_table_new(NULL, NULL);
|
|
|
|
data[0] = subMesh;
|
|
data[1] = &n;
|
|
data[2] = vIndex;
|
|
gts_surface_foreach_vertex(s3, (GtsFunc)FillVertex, data);
|
|
n = 0;
|
|
gts_surface_foreach_face(s3, (GtsFunc)FillFace, data);
|
|
g_hash_table_destroy(vIndex);
|
|
|
|
mesh->RecalculateNormals();
|
|
|
|
// destroy surfaces
|
|
gts_object_destroy(GTS_OBJECT(s1));
|
|
gts_object_destroy(GTS_OBJECT(s2));
|
|
gts_object_destroy(GTS_OBJECT(s3));
|
|
gts_object_destroy(GTS_OBJECT(si));
|
|
|
|
// destroy bounding box trees (including bounding boxes)
|
|
gts_bb_tree_destroy(tree1, true);
|
|
gts_bb_tree_destroy(tree2, true);
|
|
return mesh;
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
void MeshCSG::ConvertMeshToGTS(const Mesh *_mesh, GtsSurface *_surface)
|
|
{
|
|
if (!_surface)
|
|
{
|
|
gzerr << _mesh->GetName() << ": Surface is NULL\n";
|
|
// _surface = gts_surface_new(gts_surface_class(), gts_face_class(),
|
|
// gts_edge_class(), gts_vertex_class());
|
|
return;
|
|
}
|
|
|
|
GPtrArray *vertices = g_ptr_array_new();
|
|
|
|
for (unsigned int i = 0; i < _mesh->GetSubMeshCount(); ++i)
|
|
{
|
|
const SubMesh *subMesh = _mesh->GetSubMesh(i);
|
|
unsigned int indexCount = subMesh->GetIndexCount();
|
|
if (subMesh->GetVertexCount() <= 2)
|
|
continue;
|
|
|
|
for (unsigned int j = 0; j < subMesh->GetVertexCount(); ++j)
|
|
{
|
|
ignition::math::Vector3d vertex = subMesh->Vertex(j);
|
|
g_ptr_array_add(vertices, gts_vertex_new(gts_vertex_class(), vertex.X(),
|
|
vertex.Y(), vertex.Z()));
|
|
}
|
|
|
|
// merge duplicate vertices, otherwise gts produces undesirable results
|
|
this->MergeVertices(vertices, 0.01);
|
|
|
|
GtsVertex **verticesData =
|
|
reinterpret_cast<GtsVertex **>(vertices->pdata);
|
|
for (unsigned int j = 0; j < indexCount/3; ++j)
|
|
{
|
|
GtsEdge *e1 = GTS_EDGE(gts_vertices_are_connected(
|
|
verticesData[subMesh->GetIndex(3*j)],
|
|
verticesData[subMesh->GetIndex(3*j+1)]));
|
|
GtsEdge *e2 = GTS_EDGE(gts_vertices_are_connected(
|
|
verticesData[subMesh->GetIndex(3*j+1)],
|
|
verticesData[subMesh->GetIndex(3*j+2)]));
|
|
GtsEdge *e3 = GTS_EDGE(gts_vertices_are_connected(
|
|
verticesData[subMesh->GetIndex(3*j+2)],
|
|
verticesData[subMesh->GetIndex(3*j)]));
|
|
if (e1 == NULL && verticesData[subMesh->GetIndex(3*j)]
|
|
!= verticesData[subMesh->GetIndex(3*j+1)])
|
|
{
|
|
e1 = gts_edge_new(_surface->edge_class,
|
|
verticesData[subMesh->GetIndex(3*j)],
|
|
verticesData[subMesh->GetIndex(3*j+1)]);
|
|
}
|
|
if (e2 == NULL && verticesData[subMesh->GetIndex(3*j+1)]
|
|
!= verticesData[subMesh->GetIndex(3*j+2)])
|
|
{
|
|
e2 = gts_edge_new(_surface->edge_class,
|
|
verticesData[subMesh->GetIndex(3*j+1)],
|
|
verticesData[subMesh->GetIndex(3*j+2)]);
|
|
}
|
|
if (e3 == NULL && verticesData[subMesh->GetIndex(3*j+2)]
|
|
!= verticesData[subMesh->GetIndex(3*j)])
|
|
{
|
|
e3 = gts_edge_new(_surface->edge_class,
|
|
verticesData[subMesh->GetIndex(3*j+2)],
|
|
verticesData[subMesh->GetIndex(3*j)]);
|
|
}
|
|
if (e1 != NULL && e2 != NULL && e3 != NULL)
|
|
{
|
|
gts_surface_add_face(_surface, gts_face_new(_surface->face_class, e1,
|
|
e2, e3));
|
|
}
|
|
else
|
|
{
|
|
gzwarn << _mesh->GetName() << ": Ignoring degenerate facet!";
|
|
}
|
|
}
|
|
}
|
|
}
|