hpp-fcl: include/hpp/fcl/shape/details/convex.hxx Source File

hpp-fcl 1.7.3 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>
60 Convex<PolygonT>::Convex(const Convex<PolygonT>& other) :
61 ConvexBase (other),
62 polygons (other.polygons),
63 num_polygons (other.num_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>
73 Convex<PolygonT>::~Convex()
74 {
75 if (own_storage_) delete [] polygons;
76 }
77
78 template <typename PolygonT>
79 Matrix3f Convex<PolygonT>::computeMomentofInertia() const
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>
118 Vec3f Convex<PolygonT>::computeCOM() const
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>
152 FCL_REAL Convex<PolygonT>::computeVolume() const
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>
185 void Convex<PolygonT>::fillNeighbors()
186 {
187 neighbors = new Neighbors[num_points];
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
hpp::fcl::ConvexBase
Base for convex polytope.
Definition: geometric_shapes.h:356
hpp::fcl::Convex
Convex polytope.
Definition: convex.h:54
hpp::fcl::Convex::Convex
Convex()
Construct an uninitialized convex object.
Definition: convex.h:57
hpp::fcl::Convex::num_polygons
int num_polygons
Definition: convex.h:81
hpp::fcl::Convex::~Convex
~Convex()
Definition: convex.hxx:73
hpp::fcl::Convex::computeCOM
Vec3f computeCOM() const
compute center of mass
Definition: convex.hxx:118
hpp::fcl::Convex::polygons
PolygonT * polygons
An array of PolygonT object. PolygonT should contains a list of vertices for each polygon,...
Definition: convex.h:80
hpp::fcl::Convex::computeVolume
FCL_REAL computeVolume() const
compute the volume
Definition: convex.hxx:152
hpp::fcl::Convex::fillNeighbors
void fillNeighbors()
Definition: convex.hxx:185
hpp::fcl::Convex::computeMomentofInertia
Matrix3f computeMomentofInertia() const
based on http://number-none.com/blow/inertia/bb_inertia.doc
Definition: convex.hxx:79
hpp::fcl::ConvexBase::own_storage_
bool own_storage_
Definition: geometric_shapes.h:438
hpp::fcl::ConvexBase::initialize
void initialize(bool ownStorage, Vec3f *points_, int num_points_)
Initialize the points of the convex shape This also initializes the ConvexBase::center.
hpp::fcl::ConvexBase::Neighbors::n_
unsigned int * n_
Definition: geometric_shapes.h:404
hpp::fcl::ConvexBase::Neighbors::count_
unsigned char count_
Definition: geometric_shapes.h:403
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:66
hpp
Main namespace.
Definition: AABB.h:44
hpp::fcl::ConvexBase::Neighbors
Definition: geometric_shapes.h:402