hpp-fcl  1.8.0
HPP fork of FCL -- The Flexible Collision Library
convex.hxx
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, CNRS - LAAS
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Open Source Robotics Foundation nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
38 #ifndef HPP_FCL_SHAPE_CONVEX_HXX
39 #define HPP_FCL_SHAPE_CONVEX_HXX
40 
41 #include <set>
42 #include <vector>
43 
44 namespace hpp
45 {
46 namespace fcl
47 {
48 
49 template <typename PolygonT>
50 Convex<PolygonT>::Convex(bool own_storage, Vec3f* points_, unsigned int num_points_,
51  PolygonT* polygons_, unsigned int num_polygons_) : ConvexBase(),
52  polygons (polygons_),
53  num_polygons (num_polygons_)
54 {
55  initialize(own_storage, points_, num_points_);
56  fillNeighbors();
57 }
58 
59 template <typename PolygonT>
61  ConvexBase (other),
62  polygons (other.polygons),
63  num_polygons (other.num_polygons)
64 {
65  if (own_storage_) {
66  polygons = new PolygonT[num_polygons];
67  memcpy(polygons, other.polygons, sizeof(PolygonT) * num_polygons);
68  }
69 }
70 
71 template <typename PolygonT>
73 {
74  if (own_storage_) delete [] polygons;
75 }
76 
77 template <typename PolygonT>
78 void Convex<PolygonT>::set(bool own_storage, Vec3f* points_, unsigned int num_points_,
79  PolygonT* polygons_, unsigned int num_polygons_)
80 {
81  if (own_storage_)
82  delete [] polygons;
83  ConvexBase::set(own_storage,points_,num_points_);
84 
85  num_polygons = num_polygons_;
86  polygons = polygons_;
87 
88  fillNeighbors();
89 }
90 
91 template <typename PolygonT>
93 {
94  typedef typename PolygonT::size_type size_type;
95  typedef typename PolygonT::index_type index_type;
96 
97  Matrix3f C = Matrix3f::Zero();
98 
99  Matrix3f C_canonical;
100  C_canonical << 1/60.0, 1/120.0, 1/120.0,
101  1/120.0, 1/60.0, 1/120.0,
102  1/120.0, 1/120.0, 1/60.0;
103 
104  for(unsigned int i = 0; i < num_polygons; ++i)
105  {
106  const PolygonT & polygon = polygons[i];
107 
108  // compute the center of the polygon
109  Vec3f plane_center(0,0,0);
110  for(size_type j = 0; j < polygon.size(); ++j)
111  plane_center += points[polygon[j]];
112  plane_center /= polygon.size();
113 
114  // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero) of the convex shape
115  const Vec3f& v3 = plane_center;
116  for(size_type j = 0; j < polygon.size(); ++j)
117  {
118  index_type e_first = polygon[j];
119  index_type e_second = polygon[(j+1)%polygon.size()];
120  const Vec3f& v1 = points[e_first];
121  const Vec3f& v2 = points[e_second];
122  Matrix3f A; A << v1.transpose(), v2.transpose(), v3.transpose(); // this is A' in the original document
123  C += A.transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
124  }
125  }
126 
127  return C.trace() * Matrix3f::Identity() - C;
128 }
129 
130 template <typename PolygonT>
132 {
133  typedef typename PolygonT::size_type size_type;
134  typedef typename PolygonT::index_type index_type;
135 
136  Vec3f com(0,0,0);
137  FCL_REAL vol = 0;
138  for(unsigned int i = 0; i < num_polygons; ++i)
139  {
140  const PolygonT & polygon = polygons[i];
141  // compute the center of the polygon
142  Vec3f plane_center(0,0,0);
143  for(size_type j = 0; j < polygon.size(); ++j)
144  plane_center += points[polygon[j]];
145  plane_center /= polygon.size();
146 
147  // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero) of the convex shape
148  const Vec3f& v3 = plane_center;
149  for(size_type j = 0; j < polygon.size(); ++j)
150  {
151  index_type e_first = polygon[j];
152  index_type e_second = polygon[(j+1)%polygon.size()];
153  const Vec3f& v1 = points[e_first];
154  const Vec3f& v2 = points[e_second];
155  FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
156  vol += d_six_vol;
157  com += (points[e_first] + points[e_second] + plane_center) * d_six_vol;
158  }
159  }
160 
161  return com / (vol * 4); // here we choose zero as the reference
162 }
163 
164 template <typename PolygonT>
166 {
167  typedef typename PolygonT::size_type size_type;
168  typedef typename PolygonT::index_type index_type;
169 
170  FCL_REAL vol = 0;
171  for(unsigned int i = 0; i < num_polygons; ++i)
172  {
173  const PolygonT & polygon = polygons[i];
174 
175  // compute the center of the polygon
176  Vec3f plane_center(0,0,0);
177  for(size_type j = 0; j < polygon.size(); ++j)
178  plane_center += points[polygon[j]];
179  plane_center /= polygon.size();
180 
181  // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero point) of the convex shape
182  const Vec3f& v3 = plane_center;
183  for(size_type j = 0; j < polygon.size(); ++j)
184  {
185  index_type e_first = polygon[j];
186  index_type e_second = polygon[(j+1)%polygon.size()];
187  const Vec3f& v1 = points[e_first];
188  const Vec3f& v2 = points[e_second];
189  FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
190  vol += d_six_vol;
191  }
192  }
193 
194  return vol / 6;
195 }
196 
197 template <typename PolygonT>
199 {
200  neighbors = new Neighbors[num_points];
201 
202  typedef typename PolygonT::size_type size_type;
203  typedef typename PolygonT::index_type index_type;
204  std::vector<std::set<index_type> > nneighbors (num_points);
205  unsigned int c_nneighbors = 0;
206 
207  for (unsigned int l = 0; l < num_polygons; ++l)
208  {
209  const PolygonT & polygon = polygons[l];
210  const size_type n = polygon.size();
211 
212  for(size_type j = 0; j < polygon.size(); ++j)
213  {
214  size_type i = (j==0 ) ? n-1 : j-1;
215  size_type k = (j==n-1) ? 0 : j+1;
216  index_type pi = polygon[i],
217  pj = polygon[j],
218  pk = polygon[k];
219  // Update neighbors of pj;
220  if (nneighbors[pj].count(pi) == 0) {
221  c_nneighbors++;
222  nneighbors[pj].insert(pi);
223  }
224  if (nneighbors[pj].count(pk) == 0) {
225  c_nneighbors++;
226  nneighbors[pj].insert(pk);
227  }
228  }
229  }
230 
231  nneighbors_ = new unsigned int[c_nneighbors];
232  unsigned int* p_nneighbors = nneighbors_;
233  for (unsigned int i = 0; i < num_points; ++i) {
234  Neighbors& n = neighbors[i];
235  if (nneighbors[i].size() >= (std::numeric_limits<unsigned char>::max)())
236  throw std::logic_error ("Too many neighbors.");
237  n.count_ = (unsigned char)nneighbors[i].size();
238  n.n_ = p_nneighbors;
239  p_nneighbors = std::copy (nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors);
240  }
241  assert (p_nneighbors == nneighbors_ + c_nneighbors);
242 }
243 
244 }
245 
246 } // namespace hpp
247 
248 #endif
FCL_REAL computeVolume() const
compute the volume
Definition: convex.hxx:165
bool own_storage_
Definition: geometric_shapes.h:445
void set(bool ownStorage, Vec3f *points_, unsigned int num_points_)
Set the points of the convex shape.
Main namespace.
Definition: AABB.h:43
Definition: geometric_shapes.h:401
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:69
void fillNeighbors()
Definition: convex.hxx:198
Vec3f computeCOM() const
compute center of mass
Definition: convex.hxx:131
unsigned int num_polygons
Definition: convex.h:81
~Convex()
Definition: convex.hxx:72
PolygonT * polygons
An array of PolygonT object. PolygonT should contains a list of vertices for each polygon...
Definition: convex.h:80
double FCL_REAL
Definition: data_types.h:66
unsigned int * n_
Definition: geometric_shapes.h:404
unsigned char count_
Definition: geometric_shapes.h:403
void initialize(bool ownStorage, Vec3f *points_, unsigned int num_points_)
Initialize the points of the convex shape This also initializes the ConvexBase::center.
Convex()
Construct an uninitialized convex object.
Definition: convex.h:57
Base for convex polytope.
Definition: geometric_shapes.h:355
void set(bool ownStorage, Vec3f *points_, unsigned int num_points_, PolygonT *polygons_, unsigned int num_polygons_)
Set the current Convex from a list of points and polygons.
Definition: convex.hxx:78
Matrix3f computeMomentofInertia() const
based on http://number-none.com/blow/inertia/bb_inertia.doc
Definition: convex.hxx:92
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
Convex polytope.
Definition: convex.h:53