convex-inl.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
39 #ifndef FCL_SHAPE_CONVEX_INL_H
40 #define FCL_SHAPE_CONVEX_INL_H
41 
42 #include <iomanip>
43 #include <map>
44 #include <set>
45 #include <sstream>
46 #include <utility>
47 
50 
51 namespace fcl
52 {
53 
54 //==============================================================================
55 extern template
56 class FCL_EXPORT Convex<double>;
57 
58 //==============================================================================
59 template <typename S>
61  const std::shared_ptr<const std::vector<Vector3<S>>>& vertices,
62  int num_faces, const std::shared_ptr<const std::vector<int>>& faces,
63  bool throw_if_invalid)
64  : ShapeBase<S>(),
65  vertices_(vertices),
66  num_faces_(num_faces),
67  faces_(faces),
68  throw_if_invalid_(throw_if_invalid),
69  find_extreme_via_neighbors_{vertices->size() >
70  kMinVertCountForEdgeWalking} {
71  assert(vertices != nullptr);
72  assert(faces != nullptr);
73  // Compute an interior point. We're computing the mean point and *not* some
74  // alternative such as the centroid or bounding box center.
76  for (const auto& vertex : *vertices_) {
77  sum += vertex;
78  }
79  interior_point_ = sum * (S)(1.0 / vertices_->size());
80  FindVertexNeighbors();
81  ValidateMesh(throw_if_invalid);
82 }
83 
84 //==============================================================================
85 template <typename S>
87  this->aabb_local.min_.setConstant(std::numeric_limits<S>::max());
88  this->aabb_local.max_.setConstant(-std::numeric_limits<S>::max());
89 
90  for (const auto& v : *vertices_) {
91  this->aabb_local += v;
92  }
93 
94  this->aabb_center = this->aabb_local.center();
95  this->aabb_radius = (this->aabb_local.min_ - this->aabb_center).norm();
96 }
97 
98 //==============================================================================
99 template <typename S>
101  return GEOM_CONVEX;
102 }
103 
104 //==============================================================================
105 // TODO(SeanCurtis-TRI): When revisiting these, consider the following
106 // resources:
107 // https://www.geometrictools.com/Documentation/PolyhedralMassProperties.pdf
108 // http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.56.127&rep=rep1&type=pdf
109 // http://number-none.com/blow/inertia/bb_inertia.doc
110 template <typename S>
112  const std::vector<Vector3<S>>& vertices = *vertices_;
113  const std::vector<int>& faces = *faces_;
115 
116  Matrix3<S> C_canonical;
117  C_canonical << 1/ 60.0, 1/120.0, 1/120.0,
118  1/120.0, 1/ 60.0, 1/120.0,
119  1/120.0, 1/120.0, 1/ 60.0;
120 
121  S vol_times_six = 0;
122  int face_index = 0;
123  for (int i = 0; i < num_faces_; ++i) {
124  const int vertex_count = faces[face_index];
125  Vector3<S> face_center = Vector3<S>::Zero();
126 
127  // Compute the center of the face.
128  for (int j = 1; j <= vertex_count; ++j) {
129  face_center += vertices[faces[face_index + j]];
130  }
131  face_center = face_center * (1.0 / vertex_count);
132 
133  // Compute the volume of tetrahedron formed by the vertices on one of the
134  // polygon's edges, the center point, and the shape's frame's origin.
135  const Vector3<S>& v3 = face_center;
136  const int vertex_base = face_index + 1;
137  for (int j = 0; j < vertex_count; ++j) {
138  int e_first = faces[vertex_base + j];
139  int e_second = faces[vertex_base + (j + 1) % vertex_count];
140  const Vector3<S>& v1 = vertices[e_first];
141  const Vector3<S>& v2 = vertices[e_second];
142  S d_six_vol = (v1.cross(v2)).dot(v3);
143  Matrix3<S> A; // this is A' in the original document
144  A.row(0) = v1;
145  A.row(1) = v2;
146  A.row(2) = v3;
147  C += A.transpose() * C_canonical * A * d_six_vol; // change accordingly
148  vol_times_six += d_six_vol;
149  }
150 
151  face_index += vertex_count + 1;
152  }
153 
154  S trace_C = C(0, 0) + C(1, 1) + C(2, 2);
155 
156  Matrix3<S> m;
157  m << trace_C - C(0, 0), -C(0, 1), -C(0, 2),
158  -C(1, 0), trace_C - C(1, 1), -C(1, 2),
159  -C(2, 0), -C(2, 1), trace_C - C(2, 2);
160 
161  return m * (6 / vol_times_six);
162 }
163 
164 //==============================================================================
165 template <typename S>
167  const std::vector<Vector3<S>>& vertices = *vertices_;
168  const std::vector<int>& faces = *faces_;
170  S vol = 0;
171  int face_index = 0;
172  for (int i = 0; i < num_faces_; ++i) {
173  const int vertex_count = faces[face_index];
174  Vector3<S> face_center = Vector3<S>::Zero();
175 
176  // TODO(SeanCurtis-TRI): See note in computeVolume() on the efficiency of
177  // this approach.
178 
179  // Compute the center of the polygon.
180  for (int j = 1; j <= vertex_count; ++j) {
181  face_center += vertices[faces[face_index + j]];
182  }
183  face_center = face_center * (1.0 / vertex_count);
184 
185  // Compute the volume of tetrahedron formed by the vertices on one of the
186  // polygon's edges, the center point, and the shape's frame's origin.
187  const Vector3<S>& v3 = face_center;
188  for (int j = 1; j <= vertex_count; ++j) {
189  int e_first = faces[face_index + j];
190  int e_second = faces[face_index + (j % vertex_count) + 1];
191  const Vector3<S>& v1 = vertices[e_first];
192  const Vector3<S>& v2 = vertices[e_second];
193  S d_six_vol = (v1.cross(v2)).dot(v3);
194  vol += d_six_vol;
195  com += (v1 + v2 + face_center) * d_six_vol;
196  }
197 
198  face_index += vertex_count + 1;
199  }
200 
201  return com / (vol * 4); // here we choose zero as the reference
202 }
203 
204 //==============================================================================
205 template <typename S> S Convex<S>::computeVolume() const {
206  const std::vector<Vector3<S>>& vertices = *vertices_;
207  const std::vector<int>& faces = *faces_;
208  S vol = 0;
209  int face_index = 0;
210  for (int i = 0; i < num_faces_; ++i) {
211  const int vertex_count = faces[face_index];
212  Vector3<S> face_center = Vector3<S>::Zero();
213 
214  // TODO(SeanCurtis-TRI): While this is general, this is inefficient. If the
215  // face happens to be a triangle, this does 3X the requisite work.
216  // If the face is a 4-gon, then this does 2X the requisite work.
217  // As N increases in the N-gon this approach's inherent relative penalty
218  // shrinks. Ideally, this should at least key on 3-gon and 4-gon before
219  // falling through to this.
220 
221  // Compute the center of the polygon.
222  for (int j = 1; j <= vertex_count; ++j) {
223  face_center += vertices[faces[face_index + j]];
224  }
225  face_center = face_center * (1.0 / vertex_count);
226 
227  // TODO(SeanCurtis-TRI): Because volume serves as the weights for
228  // center-of-mass and inertia computations, it should be refactored into its
229  // own function that can be invoked by providing three vertices (the fourth
230  // being the origin).
231 
232  // Compute the volume of tetrahedron formed by the vertices on one of the
233  // polygon's edges, the center point, and the shape's frame's origin.
234  const Vector3<S>& v3 = face_center;
235  for (int j = 1; j <= vertex_count; ++j) {
236  int e_first = faces[face_index + j];
237  int e_second = faces[face_index + (j % vertex_count) + 1];
238  const Vector3<S>& v1 = vertices[e_first];
239  const Vector3<S>& v2 = vertices[e_second];
240  S d_six_vol = (v1.cross(v2)).dot(v3);
241  vol += d_six_vol;
242  }
243 
244  face_index += vertex_count + 1;
245  }
246 
247  return vol / 6;
248 }
249 
250 //==============================================================================
251 template <typename S>
252 std::vector<Vector3<S>> Convex<S>::getBoundVertices(
253  const Transform3<S>& tf) const {
254  std::vector<Vector3<S>> result;
255  result.reserve(vertices_->size());
256 
257  for (const auto& v : *vertices_) {
258  result.push_back(tf * v);
259  }
260 
261  return result;
262 }
263 
264 //==============================================================================
265 template <typename S>
267  // TODO(SeanCurtis-TRI): Create an override of this that seeds the search with
268  // the last extremal vertex index (assuming some kind of coherency in the
269  // evaluation sequence).
270  const std::vector<Vector3<S>>& vertices = *vertices_;
271  // Note: A small amount of empirical testing suggests that a vector of int8_t
272  // yields a slight performance improvement over int and bool. This is *not*
273  // definitive.
274  std::vector<int8_t> visited(vertices.size(), 0);
275  int extreme_index = 0;
276  S extreme_value = v_C.dot(vertices[extreme_index]);
277 
278  if (find_extreme_via_neighbors_) {
279  bool keep_searching = true;
280  while (keep_searching) {
281  keep_searching = false;
282  const int neighbor_start = neighbors_[extreme_index];
283  const int neighbor_count = neighbors_[neighbor_start];
284  for (int n_index = neighbor_start + 1;
285  n_index <= neighbor_start + neighbor_count; ++n_index) {
286  const int neighbor_index = neighbors_[n_index];
287  if (visited[neighbor_index]) continue;
288  visited[neighbor_index] = 1;
289  const S neighbor_value = v_C.dot(vertices[neighbor_index]);
290  // N.B. Testing >= (instead of >) protects us from the (very rare) case
291  // where the *starting* vertex is co-planar with all of its neighbors
292  // *and* the query direction v_C is perpendicular to that plane. With >
293  // we wouldn't walk away from the start vertex. With >= we will walk
294  // away and continue around (although it won't necessarily be the
295  // fastest walk down).
296  if (neighbor_value >= extreme_value) {
297  keep_searching = true;
298  extreme_index = neighbor_index;
299  extreme_value = neighbor_value;
300  }
301  }
302  }
303  } else {
304  // Simple linear search.
305  for (int i = 1; i < static_cast<int>(vertices.size()); ++i) {
306  S value = v_C.dot(vertices[i]);
307  if (value > extreme_value) {
308  extreme_index = i;
309  extreme_value = value;
310  }
311  }
312  }
313  return vertices[extreme_index];
314 }
315 
316 //==============================================================================
317 template <typename S>
318 std::string Convex<S>::representation(int precision) const {
319  const char* S_str = detail::ScalarRepr<S>::value();
320  std::stringstream ss;
321  ss << std::setprecision(precision);
322  ss << "Convex<" << S_str << ">("
323  << "\n std::make_shared<std::vector<Vector3<" << S_str << ">>>("
324  << "\n std::initializer_list<Vector3<" << S_str << ">>{";
325  for (const Vector3<S>& p_GV : *vertices_) {
326  ss << "\n Vector3<" << S_str << ">(" << p_GV[0] << ", " << p_GV[1]
327  << ", " << p_GV[2] << "),";
328  }
329  ss << "}),";
330  ss << "\n " << num_faces_ << ",";
331  ss << "\n std::make_shared<std::vector<int>>("
332  << "\n std::initializer_list<int>{"
333  << "\n ";
334  const std::vector<int>& faces = *faces_;
335  int face_index = 0;
336  for (int i = 0; i < num_faces_; ++i) {
337  const int vertex_count = faces[face_index];
338  ss << " " << vertex_count << ",";
339  for (int j = 1; j <= vertex_count; ++j) {
340  ss << " " << faces[face_index + j] << ",";
341  }
342  face_index += vertex_count + 1;
343  }
344  ss << "}),"
345  << "\n " << std::boolalpha << throw_if_invalid_ << ");";
346 
347  return ss.str();
348 }
349 
350 //==============================================================================
351 template <typename S>
352 void Convex<S>::ValidateMesh(bool throw_on_error) {
353  ValidateTopology(throw_on_error);
354  // TODO(SeanCurtis-TRI) Implement the missing "all-faces-are-planar" test.
355  // TODO(SeanCurtis-TRI) Implement the missing "really-is-convex" test.
356 }
357 
358 //==============================================================================
359 template <typename S>
360 void Convex<S>::ValidateTopology(bool throw_on_error) {
361  // Computing the vertex neighbors is a pre-requisite to determining validity.
362  assert(neighbors_.size() > vertices_->size());
363 
364  std::stringstream ss;
365  ss << "Found errors in the Convex mesh:";
366 
367  // To simplify the code, we define an edge as a pair of ints (A, B) such that
368  // A < B must be true.
369  auto make_edge = [](int v0, int v1) {
370  if (v0 > v1) std::swap(v0, v1);
371  return std::make_pair(v0, v1);
372  };
373 
374  bool all_connected = true;
375  // Build a map from each unique edge to the _number_ of adjacent faces (see
376  // the definition of make_edge for the encoding of an edge).
377  std::map<std::pair<int, int>, int> per_edge_face_count;
378  // First, pre-populate all the edges found in the vertex neighbor calculation.
379  for (int v = 0; v < static_cast<int>(vertices_->size()); ++v) {
380  const int neighbor_start = neighbors_[v];
381  const int neighbor_count = neighbors_[neighbor_start];
382  if (neighbor_count == 0) {
383  if (all_connected) {
384  ss << "\n Not all vertices are connected.";
385  all_connected = false;
386  }
387  ss << "\n Vertex " << v << " is not included in any faces.";
388  }
389  for (int n_index = neighbor_start + 1;
390  n_index <= neighbor_start + neighbor_count; ++n_index) {
391  const int n = neighbors_[n_index];
392  per_edge_face_count[make_edge(v, n)] = 0;
393  }
394  }
395 
396  // To count adjacent faces, we must iterate through the faces; we can't infer
397  // it from how many times an edge appears in the neighbor list. In the
398  // degenerate case where three faces share an edge, that edge would only
399  // appear twice. So, we must explicitly examine each face.
400  const std::vector<int>& faces = *faces_;
401  int face_index = 0;
402  for (int f = 0; f < num_faces_; ++f) {
403  const int vertex_count = faces[face_index];
404  int prev_v = faces[face_index + vertex_count];
405  for (int i = face_index + 1; i <= face_index + vertex_count; ++i) {
406  const int v = faces[i];
407  ++per_edge_face_count[make_edge(v, prev_v)];
408  prev_v = v;
409  }
410  face_index += vertex_count + 1;
411  }
412 
413  // Now examine the results.
414  bool is_watertight = true;
415  for (const auto& key_value_pair : per_edge_face_count) {
416  const auto& edge = key_value_pair.first;
417  const int count = key_value_pair.second;
418  if (count != 2) {
419  if (is_watertight) {
420  is_watertight = false;
421  ss << "\n The mesh is not watertight.";
422  }
423  ss << "\n Edge between vertices " << edge.first << " and " << edge.second
424  << " is shared by " << count << " faces (should be 2).";
425  }
426  }
427  // We can't trust walking the edges on a mesh that isn't watertight.
428  const bool has_error = !(is_watertight && all_connected);
429  // Note: find_extreme_via_neighbors_ may already be false because the mesh
430  // is too small. Don't indirectly re-enable it just because the mesh doesn't
431  // have any errors.
432  find_extreme_via_neighbors_ = find_extreme_via_neighbors_ && !has_error;
433  if (has_error && throw_on_error) {
434  throw std::runtime_error(ss.str());
435  }
436 }
437 
438 //==============================================================================
439 template <typename S>
441  // We initially build it using sets. Two faces with a common edge will
442  // independently want to report that the edge's vertices are neighbors. So,
443  // we rely on the set to eliminate the redundant declaration and then dump
444  // the results to a more compact representation: the vector.
445  const int v_count = static_cast<int>(vertices_->size());
446  std::vector<std::set<int>> neighbors(v_count);
447  const std::vector<int>& faces = *faces_;
448  int face_index = 0;
449  for (int f = 0; f < num_faces_; ++f) {
450  const int vertex_count = faces[face_index];
451  int prev_v = faces[face_index + vertex_count];
452  for (int i = face_index + 1; i <= face_index + vertex_count; ++i) {
453  const int v = faces[i];
454  neighbors[v].insert(prev_v);
455  neighbors[prev_v].insert(v);
456  prev_v = v;
457  }
458  face_index += vertex_count + 1;
459  }
460 
461  // Now build the encoded adjacency graph as documented.
462  neighbors_.resize(v_count);
463  for (int v = 0; v < v_count; ++v) {
464  const std::set<int>& v_neighbors = neighbors[v];
465  neighbors_[v] = static_cast<int>(neighbors_.size());
466  neighbors_.push_back(static_cast<int>(v_neighbors.size()));
467  neighbors_.insert(neighbors_.end(), v_neighbors.begin(), v_neighbors.end());
468  }
469 }
470 
471 } // namespace fcl
472 
473 #endif
fcl::Convex< S >::S
S S
Definition: convex.h:88
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::Convex< double >
template class FCL_EXPORT Convex< double >
fcl::detail::ScalarRepr
Definition: representation.h:46
fcl::Convex
A convex polytope.
Definition: convex.h:84
swap
static void swap(T &x, T &y)
Definition: svm.cpp:54
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::ShapeBase
Base class for all basic geometric shapes.
Definition: shape_base.h:48
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:85
convex.h
representation.h
fcl::GEOM_CONVEX
@ GEOM_CONVEX
Definition: collision_geometry.h:54
fcl::NODE_TYPE
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_geometry.h:53
fcl::Convex::Convex
Convex(const std::shared_ptr< const std::vector< Vector3< S >>> &vertices, int num_faces, const std::shared_ptr< const std::vector< int >> &faces, bool throw_if_invalid=false)
Constructor.
Definition: convex-inl.h:60
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45


fcl
Author(s):
autogenerated on Tue Dec 5 2023 03:40:48