hpp-fcl  1.4.5
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_, int num_points_,
51  PolygonT* polygons_, 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),
64 {
65  if (own_storage_) {
66  delete [] polygons;
67  polygons = new PolygonT[num_polygons];
68  memcpy(polygons, other.polygons, sizeof(PolygonT) * num_polygons);
69  }
70 }
71 
72 template <typename PolygonT>
74 {
75  if (own_storage_) delete [] polygons;
76 }
77 
78 template <typename PolygonT>
80 {
81  typedef typename PolygonT::size_type size_type;
82  typedef typename PolygonT::index_type index_type;
83 
84  Matrix3f C = Matrix3f::Zero();
85 
86  Matrix3f C_canonical;
87  C_canonical << 1/60.0, 1/120.0, 1/120.0,
88  1/120.0, 1/60.0, 1/120.0,
89  1/120.0, 1/120.0, 1/60.0;
90 
91  for(int i = 0; i < num_polygons; ++i)
92  {
93  const PolygonT& polygon (polygons[i]);
94 
95  // compute the center of the polygon
96  Vec3f plane_center(0,0,0);
97  for(size_type j = 0; j < polygon.size(); ++j)
98  plane_center += points[polygon[j]];
99  plane_center /= polygon.size();
100 
101  // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero) of the convex shape
102  const Vec3f& v3 = plane_center;
103  for(size_type j = 0; j < polygon.size(); ++j)
104  {
105  index_type e_first = polygon[j];
106  index_type e_second = polygon[(j+1)%polygon.size()];
107  const Vec3f& v1 = points[e_first];
108  const Vec3f& v2 = points[e_second];
109  Matrix3f A; A << v1.transpose(), v2.transpose(), v3.transpose(); // this is A' in the original document
110  C += A.transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
111  }
112  }
113 
114  return C.trace() * Matrix3f::Identity() - C;
115 }
116 
117 template <typename PolygonT>
119 {
120  typedef typename PolygonT::size_type size_type;
121  typedef typename PolygonT::index_type index_type;
122 
123  Vec3f com(0,0,0);
124  FCL_REAL vol = 0;
125  for(int i = 0; i < num_polygons; ++i)
126  {
127  const PolygonT& polygon (polygons[i]);
128  // compute the center of the polygon
129  Vec3f plane_center(0,0,0);
130  for(size_type j = 0; j < polygon.size(); ++j)
131  plane_center += points[polygon[j]];
132  plane_center /= polygon.size();
133 
134  // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero) of the convex shape
135  const Vec3f& v3 = plane_center;
136  for(size_type j = 0; j < polygon.size(); ++j)
137  {
138  index_type e_first = polygon[j];
139  index_type e_second = polygon[(j+1)%polygon.size()];
140  const Vec3f& v1 = points[e_first];
141  const Vec3f& v2 = points[e_second];
142  FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
143  vol += d_six_vol;
144  com += (points[e_first] + points[e_second] + plane_center) * d_six_vol;
145  }
146  }
147 
148  return com / (vol * 4); // here we choose zero as the reference
149 }
150 
151 template <typename PolygonT>
153 {
154  typedef typename PolygonT::size_type size_type;
155  typedef typename PolygonT::index_type index_type;
156 
157  FCL_REAL vol = 0;
158  for(int i = 0; i < num_polygons; ++i)
159  {
160  const PolygonT& polygon (polygons[i]);
161 
162  // compute the center of the polygon
163  Vec3f plane_center(0,0,0);
164  for(size_type j = 0; j < polygon.size(); ++j)
165  plane_center += points[polygon[j]];
166  plane_center /= polygon.size();
167 
168  // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero point) of the convex shape
169  const Vec3f& v3 = plane_center;
170  for(size_type j = 0; j < polygon.size(); ++j)
171  {
172  index_type e_first = polygon[j];
173  index_type e_second = polygon[(j+1)%polygon.size()];
174  const Vec3f& v1 = points[e_first];
175  const Vec3f& v2 = points[e_second];
176  FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
177  vol += d_six_vol;
178  }
179  }
180 
181  return vol / 6;
182 }
183 
184 template <typename PolygonT>
186 {
188 
189  typedef typename PolygonT::size_type size_type;
190  typedef typename PolygonT::index_type index_type;
191  std::vector<std::set<index_type> > nneighbors (num_points);
192  unsigned int c_nneighbors = 0;
193 
194  for (int l = 0; l < num_polygons; ++l)
195  {
196  const PolygonT& polygon (polygons[l]);
197  size_type n = polygon.size();
198 
199  for(size_type j = 0; j < polygon.size(); ++j)
200  {
201  size_type i = (j==0 ) ? n-1 : j-1;
202  size_type k = (j==n-1) ? 0 : j+1;
203  index_type pi = polygon[i],
204  pj = polygon[j],
205  pk = polygon[k];
206  // Update neighbors of pj;
207  if (nneighbors[pj].count(pi) == 0) {
208  c_nneighbors++;
209  nneighbors[pj].insert(pi);
210  }
211  if (nneighbors[pj].count(pk) == 0) {
212  c_nneighbors++;
213  nneighbors[pj].insert(pk);
214  }
215  }
216  }
217 
218  nneighbors_ = new unsigned int[c_nneighbors];
219  unsigned int* p_nneighbors = nneighbors_;
220  for (int i = 0; i < num_points; ++i) {
221  Neighbors& n = neighbors[i];
222  if (nneighbors[i].size() >= std::numeric_limits<unsigned char>::max())
223  throw std::logic_error ("Too many neighbors.");
224  n.count_ = (unsigned char)nneighbors[i].size();
225  n.n_ = p_nneighbors;
226  p_nneighbors = std::copy (nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors);
227  }
228  assert (p_nneighbors == nneighbors_ + c_nneighbors);
229 }
230 
231 }
232 
233 } // namespace hpp
234 
235 #endif
FCL_REAL computeVolume() const
compute the volume
Definition: convex.hxx:152
bool own_storage_
Definition: geometric_shapes.h:346
Main namespace.
Definition: AABB.h:43
Definition: geometric_shapes.h:311
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:75
void fillNeighbors()
Definition: convex.hxx:185
Vec3f computeCOM() const
compute center of mass
Definition: convex.hxx:118
void initialize(bool ownStorage, Vec3f *points_, int num_points_)
Initialize the points of the convex shape This also initializes the ConvexBase::center.
~Convex()
Definition: convex.hxx:73
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:69
unsigned int * n_
Definition: geometric_shapes.h:314
int num_polygons
Definition: convex.h:81
unsigned char count_
Definition: geometric_shapes.h:313
int num_points
Definition: geometric_shapes.h:309
Convex()
Construct an uninitialized convex object.
Definition: convex.h:57
Base for convex polytope.
Definition: geometric_shapes.h:281
Matrix3f computeMomentofInertia() const
based on http://number-none.com/blow/inertia/bb_inertia.doc
Definition: convex.hxx:79
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:74
Convex polytope.
Definition: convex.h:53
Neighbors * neighbors
Definition: geometric_shapes.h:323
unsigned int * nneighbors_
Definition: geometric_shapes.h:344
Vec3f * points
An array of the points of the polygon.
Definition: geometric_shapes.h:308