convex.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_H
40 #define FCL_SHAPE_CONVEX_H
41 
42 #include <iostream>
43 #include <memory>
44 #include <vector>
45 
47 
48 namespace fcl
49 {
50 
82 template <typename S_>
83 class FCL_EXPORT Convex : public ShapeBase<S_>
84 {
85 public:
86 
87  using S = S_;
88 
89  // TODO(SeanCurtis-TRI): A huge shopping list of issues with this class are
90  // enumerated in https://github.com/flexible-collision-library/fcl/issues/326.
91 
109  Convex(const std::shared_ptr<const std::vector<Vector3<S>>>& vertices,
110  int num_faces, const std::shared_ptr<const std::vector<int>>& faces,
111  bool throw_if_invalid = false);
112 
114  Convex(const Convex& other);
115 
116  ~Convex() = default;
117 
119  void computeLocalAABB() override;
120 
122  NODE_TYPE getNodeType() const override;
123 
125  const std::vector<Vector3<S>>& getVertices() const { return *vertices_; }
126 
128  int getFaceCount() const { return num_faces_; }
129 
151  const std::vector<int>& getFaces() const { return *faces_; }
152 
155  const Vector3<S>& getInteriorPoint() const { return interior_point_; }
156 
157  // Documentation inherited.
158  Matrix3<S> computeMomentofInertia() const override;
159 
160  // Documentation inherited.
161  Vector3<S> computeCOM() const override;
162 
163  // Documentation inherited.
164  S computeVolume() const override;
165 
168  std::vector<Vector3<S>> getBoundVertices(const Transform3<S>& tf) const;
169 
176  const Vector3<S>& findExtremeVertex(const Vector3<S>& v_C) const;
177 
178  friend
179  std::ostream& operator<<(std::ostream& out, const Convex& convex) {
180  out << "Convex(v count: " << convex.vertices_->size() << ", f count: "
181  << convex.getFaceCount() << ")";
182  return out;
183  }
184 
185  private:
186  // Test utility to examine Convex internal state.
187  friend class ConvexTester;
188 
189  // TODO(SeanCurtis-TRI): Complete the validation.
190  // *Partially* validate the mesh. The following properties are validated:
191  // - Confirms mesh is water tight (see IsWaterTight).
192  // - Confirms that all vertices are included in a face.
193  // The following properties still need to be validated:
194  // - There are at least four vertices (the minimum to enclose a *volume*.)
195  // - the vertices associated with each face are planar.
196  // - For each face, all vertices *not* forming the face lie "on" or "behind"
197  // the face.
198  //
199  // Invoking this *can* have side effects. Internal configuration can change
200  // based on failed validation tests. For example, the performance of
201  // findExtremeVertex() depends on the mesh being "valid" -- an invalid mesh
202  // can be used, but will use a slower algorithm as a result of being found
203  // invalid.
204  //
205  // @param throw_on_error If `true` and the convex is shown to be invalid, an
206  // exception is thrown.
207  void ValidateMesh(bool throw_on_error);
208 
209  // Reports if the mesh is watertight and that every vertex is part of a face.
210  // The mesh is watertight if every edge is shared by two different faces.
211  //
212  // As a side effect, if this fails, find_extreme_via_neighbors_ is set to
213  // false because a water tight mesh is a prerequisite to being able to find
214  // extremal points by following edges.
215  //
216  // @param throw_on_error If `true` and the convex is shown to be invalid, an
217  // exception is thrown.
218  // @pre FindVertexNeighbors() must have been called already.
219  void ValidateTopology(bool throw_on_error);
220 
221  // Analyzes the convex specification and builds the map of vertex ->
222  // neighboring vertices.
223  void FindVertexNeighbors();
224 
225  const std::shared_ptr<const std::vector<Vector3<S>>> vertices_;
226  const int num_faces_;
227  const std::shared_ptr<const std::vector<int>> faces_;
229 
230  /* The encoding of vertex adjacency in the mesh. The encoding is as follows:
231  Entries [0, V-1] encode the location in `neighbors_` where the adjacency
232  data for the ith vertex lies in the array.
233  Following those offsets is the compact representation of each vertex's
234  neighbors as: number of neighbors, n0, n1, ....
235 
236  The figure below shows that for vertex j, entry j tells us to jump into
237  the vector at neighbors_[j]. The value mⱼ -- the number of vertices adjacent
238  to j -- is stored at that location. The next mⱼ entries starting at
239  neighbors_[j] + 1 are the *indices* of those vertices adjacent to vertex j.
240 
241  Index where
242  vertex j's Vertex j's
243  data lies neighbor count
244  ↓ ↓
245  |_|...|_|_|_|......┃mⱼ┃n₁┃n₂┃...┃nₘ┃mⱼ₊₁|...|
246  0 ... j ↑ ↑ ... ↑
247  Indices of vertex j's
248  neighboring vertices.
249 
250  A modicum testing indicated that this compact representation led to
251  measurably improved performance for findExtremeVertex() -- initial
252  hypothesis attributes it to improved cache hits. */
253  std::vector<int> neighbors_;
254 
255  // If true, findExtremeVertex() can reliably use neighbor_vertices_ to walk
256  // along the surface of the mesh. If false, it must linearly search.
257  bool find_extreme_via_neighbors_{false};
258 
259  // Empirical evidence suggests that finding the extreme vertex by walking the
260  // edges of the mesh is only more efficient if there are more than 32
261  // vertices.
262  static constexpr int kMinVertCountForEdgeWalking = 32;
263 };
264 
265 // Workaround for https://gcc.gnu.org/bugzilla/show_bug.cgi?id=57728 which
266 // should be moved back into the class definition once we no longer need to
267 // support GCC versions prior to 6.3.
268 template <typename S>
269 Convex<S>::Convex(const Convex<S>& other) = default;
270 
273 
274 } // namespace fcl
275 
277 
278 #endif
fcl::Convex< S >::S
S S
Definition: convex.h:87
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::Convex::operator<<
friend std::ostream & operator<<(std::ostream &out, const Convex &convex)
Definition: convex.h:179
fcl::Convex
A convex polytope.
Definition: convex.h:83
fcl::ShapeBase
Base class for all basic geometric shapes.
Definition: shape_base.h:48
shape_base.h
fcl::Convex::getFaceCount
int getFaceCount() const
Gets the total number of faces in the convex mesh.
Definition: convex.h:128
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::ConvexTester
Definition: test_convex.cpp:51
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:85
fcl::Convex::interior_point_
Vector3< S > interior_point_
Definition: convex.h:228
fcl::Convex::getFaces
const std::vector< int > & getFaces() const
Gets the representation of the faces of the convex hull.
Definition: convex.h:151
fcl::Convex::getVertices
const std::vector< Vector3< S > > & getVertices() const
Gets the vertex positions in the geometry's frame G.
Definition: convex.h:125
fcl::Convex::vertices_
const std::shared_ptr< const std::vector< Vector3< S > > > vertices_
Definition: convex.h:225
fcl::Convex::faces_
const std::shared_ptr< const std::vector< int > > faces_
Definition: convex.h:227
fcl::Convex::neighbors_
std::vector< int > neighbors_
Definition: convex.h:253
convex-inl.h
fcl::Convex::num_faces_
const int num_faces_
Definition: convex.h:226
fcl::Convex::getInteriorPoint
const Vector3< S > & getInteriorPoint() const
A point guaranteed to be on the interior of the convex polytope, used for collision.
Definition: convex.h:155
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:57
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45


fcl
Author(s):
autogenerated on Tue Jun 22 2021 07:27:50