src/shape/convex.cpp
Go to the documentation of this file.
1 #include <hpp/fcl/shape/convex.h>
2 
3 #ifdef HPP_FCL_HAS_QHULL
10 #include <libqhullcpp/Qhull.h>
11 
12 using orgQhull::Qhull;
19 #endif
20 
21 namespace hpp {
22 namespace fcl {
23 
24 // Reorders `tri` such that the dot product between the normal of triangle and
25 // the vector `triangle barycentre - convex_tri.center` is positive.
26 void reorderTriangle(const Convex<Triangle>* convex_tri, Triangle& tri) {
27  Vec3f p0, p1, p2;
28  p0 = convex_tri->points[tri[0]];
29  p1 = convex_tri->points[tri[1]];
30  p2 = convex_tri->points[tri[2]];
31 
32  Vec3f barycentre_tri, center_barycenter;
33  barycentre_tri = (p0 + p1 + p2) / 3;
34  center_barycenter = barycentre_tri - convex_tri->center;
35 
36  Vec3f edge_tri1, edge_tri2, n_tri;
37  edge_tri1 = p1 - p0;
38  edge_tri2 = p2 - p1;
39  n_tri = edge_tri1.cross(edge_tri2);
40 
41  if (center_barycenter.dot(n_tri) < 0) {
42  tri.set(tri[1], tri[0], tri[2]);
43  }
44 }
45 
46 ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points,
47  bool keepTriangles,
48  const char* qhullCommand) {
49 #ifdef HPP_FCL_HAS_QHULL
50  if (num_points <= 3) {
51  throw std::invalid_argument(
52  "You shouldn't use this function with less than"
53  " 4 points.");
54  }
55  assert(pts[0].data() + 3 == pts[1].data());
56 
57  Qhull qh;
58  const char* command =
59  qhullCommand ? qhullCommand : (keepTriangles ? "Qt" : "");
60  qh.runQhull("", 3, static_cast<int>(num_points), pts[0].data(), command);
61 
62  if (qh.qhullStatus() != qh_ERRnone) {
63  if (qh.hasQhullMessage()) std::cerr << qh.qhullMessage() << std::endl;
64  throw std::logic_error("Qhull failed");
65  }
66 
67  typedef std::size_t index_type;
68  typedef int size_type;
69 
70  // Map index in pts to index in vertices. -1 means not used
71  std::vector<int> pts_to_vertices(num_points, -1);
72 
73  // Initialize the vertices
74  int nvertex = (qh.vertexCount());
75  Vec3f* vertices = new Vec3f[nvertex];
76  QhullVertexList vertexList(qh.vertexList());
77  int i_vertex = 0;
78  for (QhullVertexList::const_iterator v = vertexList.begin();
79  v != vertexList.end(); ++v) {
80  QhullPoint pt((*v).point());
81  pts_to_vertices[(size_t)pt.id()] = (int)i_vertex;
82  vertices[i_vertex] = Vec3f(pt[0], pt[1], pt[2]);
83  ++i_vertex;
84  }
85  assert(i_vertex == nvertex);
86 
87  Convex<Triangle>* convex_tri(NULL);
88  ConvexBase* convex(NULL);
89  if (keepTriangles)
90  convex = convex_tri = new Convex<Triangle>();
91  else
92  convex = new ConvexBase;
93  convex->initialize(true, vertices, static_cast<unsigned int>(nvertex));
94 
95  // Build the neighbors
96  convex->neighbors = new Neighbors[nvertex];
97  std::vector<std::set<index_type> > nneighbors(static_cast<size_t>(nvertex));
98  if (keepTriangles) {
99  convex_tri->num_polygons = static_cast<unsigned int>(qh.facetCount());
100  convex_tri->polygons = new Triangle[convex_tri->num_polygons];
101  convex_tri->computeCenter();
102  }
103 
104  unsigned int c_nneighbors = 0;
105  unsigned int i_polygon = 0;
106 
107  // Compute the neighbors from the edges of the faces.
108  for (QhullFacet facet = qh.beginFacet(); facet != qh.endFacet();
109  facet = facet.next()) {
110  if (facet.isSimplicial()) {
111  // In 3D, simplicial faces have 3 vertices. We mark them as neighbors.
112  QhullVertexSet f_vertices(facet.vertices());
113  size_t n = static_cast<size_t>(f_vertices.count());
114  assert(n == 3);
115  Triangle tri(
116  static_cast<size_t>(
117  pts_to_vertices[static_cast<size_t>(f_vertices[0].point().id())]),
118  static_cast<size_t>(
119  pts_to_vertices[static_cast<size_t>(f_vertices[1].point().id())]),
120  static_cast<size_t>(pts_to_vertices[static_cast<size_t>(
121  f_vertices[2].point().id())]));
122  if (keepTriangles) {
123  reorderTriangle(convex_tri, tri);
124  convex_tri->polygons[i_polygon++] = tri;
125  }
126  for (size_t j = 0; j < n; ++j) {
127  size_t i = (j == 0) ? n - 1 : j - 1;
128  size_t k = (j == n - 1) ? 0 : j + 1;
129  // Update neighbors of pj;
130  if (nneighbors[tri[j]].insert(tri[i]).second) c_nneighbors++;
131  if (nneighbors[tri[j]].insert(tri[k]).second) c_nneighbors++;
132  }
133  } else {
134  if (keepTriangles) { // TODO I think there is a memory leak here.
135  throw std::invalid_argument(
136  "You requested to keep triangles so you "
137  "must pass option \"Qt\" to qhull via the qhull command argument.");
138  }
139  // Non-simplicial faces have more than 3 vertices and contains a list of
140  // rigdes. Ridges are (3-1)D simplex (i.e. one edge). We mark the two
141  // vertices of each ridge as neighbors.
142  QhullRidgeSet f_ridges(facet.ridges());
143  for (size_type j = 0; j < f_ridges.count(); ++j) {
144  assert(f_ridges[j].vertices().count() == 2);
145  int pi = pts_to_vertices[static_cast<size_t>(
146  f_ridges[j].vertices()[0].point().id())],
147  pj = pts_to_vertices[static_cast<size_t>(
148  f_ridges[j].vertices()[1].point().id())];
149  // Update neighbors of pi and pj;
150  if (nneighbors[static_cast<size_t>(pj)]
151  .insert(static_cast<size_t>(pi))
152  .second)
153  c_nneighbors++;
154  if (nneighbors[static_cast<size_t>(pi)]
155  .insert(static_cast<size_t>(pj))
156  .second)
157  c_nneighbors++;
158  }
159  }
160  }
161  assert(!keepTriangles || i_polygon == qh.facetCount());
162 
163  // Fill the neighbor attribute of the returned object.
164  convex->nneighbors_ = new unsigned int[c_nneighbors];
165  unsigned int* p_nneighbors = convex->nneighbors_;
166  for (size_t i = 0; i < static_cast<size_t>(nvertex); ++i) {
167  Neighbors& n = convex->neighbors[i];
168  if (nneighbors[i].size() >= (std::numeric_limits<unsigned char>::max)())
169  throw std::logic_error("Too many neighbors.");
170  n.count_ = (unsigned char)nneighbors[i].size();
171  n.n_ = p_nneighbors;
172  p_nneighbors =
173  std::copy(nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors);
174  }
175  assert(p_nneighbors == convex->nneighbors_ + c_nneighbors);
176  return convex;
177 #else
178  throw std::logic_error(
179  "Library built without qhull. Cannot build object of this type.");
181  HPP_FCL_UNUSED_VARIABLE(num_points);
182  HPP_FCL_UNUSED_VARIABLE(keepTriangles);
183  HPP_FCL_UNUSED_VARIABLE(qhullCommand);
184 #endif
185 }
186 } // namespace fcl
187 } // namespace hpp
static ConvexBase * convexHull(const Vec3f *points, unsigned int num_points, bool keepTriangles, const char *qhullCommand=NULL)
Build a convex hull based on Qhull library and store the vertices and optionally the triangles...
tuple p1
Definition: octree.py:55
void reorderTriangle(const Convex< Triangle > *convex_tri, Triangle &tri)
Main namespace.
A QhullFacet is the C++ equivalent to Qhull&#39;s facetT*.
Definition: QhullFacet.h:37
unsigned int num_polygons
Definition: shape/convex.h:77
ConvexBase()
Construct an uninitialized convex object Initialization is done with ConvexBase::initialize.
data
list v
Definition: obb.py:45
PolygonT * polygons
An array of PolygonT object. PolygonT should contains a list of vertices for each polygon...
Definition: shape/convex.h:76
#define qh
Definition: libqhull.h:457
#define HPP_FCL_UNUSED_VARIABLE(var)
QhullSet< QhullRidge > QhullRidgeSet
Definition: QhullFacet.h:33
Interface to Qhull from C++.
Definition: Qhull.h:43
void initialize(bool ownStorage, Vec3f *points_, unsigned int num_points_)
Initialize the points of the convex shape This also initializes the ConvexBase::center.
void set(index_type p1, index_type p2, index_type p3)
Set the vertex indices of the triangle.
Definition: data_types.h:108
Triangle with 3 indices for points.
Definition: data_types.h:96
Base for convex polytope.
#define qh_ERRnone
Definition: libqhull.h:193
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
Convex polytope.
Definition: shape/convex.h:50
QhullLinkedList< QhullVertex > QhullVertexList
Definition: QhullVertex.h:28
Vec3f center
center of the convex polytope, this is used for collision: center is guaranteed in the internal of th...
Vec3f * points
An array of the points of the polygon.


hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:00