hpp-fcl 2.4.1
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
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
37#ifndef HPP_FCL_SHAPE_CONVEX_HXX
38#define HPP_FCL_SHAPE_CONVEX_HXX
39
40#include <set>
41#include <vector>
42
43namespace hpp {
44namespace fcl {
45
46template <typename PolygonT>
47Convex<PolygonT>::Convex(bool own_storage, Vec3f* points_,
48 unsigned int num_points_, PolygonT* polygons_,
49 unsigned int num_polygons_)
50 : ConvexBase(), polygons(polygons_), num_polygons(num_polygons_) {
51 initialize(own_storage, points_, num_points_);
53}
54
55template <typename PolygonT>
57 : ConvexBase(other),
58 polygons(other.polygons),
59 num_polygons(other.num_polygons) {
60 if (own_storage_) {
61 polygons = new PolygonT[num_polygons];
62 std::copy(other.polygons, other.polygons + num_polygons, polygons);
63 }
64}
65
66template <typename PolygonT>
68 if (own_storage_) delete[] polygons;
69}
70
71template <typename PolygonT>
72void Convex<PolygonT>::set(bool own_storage, Vec3f* points_,
73 unsigned int num_points_, PolygonT* polygons_,
74 unsigned int num_polygons_) {
75 if (own_storage_) delete[] polygons;
76 ConvexBase::set(own_storage, points_, num_points_);
77
78 num_polygons = num_polygons_;
79 polygons = polygons_;
80
81 fillNeighbors();
82}
83
84template <typename PolygonT>
86 Vec3f* cloned_points = new Vec3f[num_points];
87 std::copy(points, points + num_points, cloned_points);
88
89 PolygonT* cloned_polygons = new PolygonT[num_polygons];
90 std::copy(polygons, polygons + num_polygons, cloned_polygons);
91
92 Convex* copy_ptr = new Convex(true, cloned_points, num_points,
93 cloned_polygons, num_polygons);
94
95 copy_ptr->ShapeBase::operator=(*this);
96 return copy_ptr;
97}
98
99template <typename PolygonT>
101 typedef typename PolygonT::size_type size_type;
102 typedef typename PolygonT::index_type index_type;
103
104 Matrix3f C = Matrix3f::Zero();
105
106 Matrix3f C_canonical;
107 C_canonical << 1 / 60.0, 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0, 1 / 120.0,
108 1 / 120.0, 1 / 120.0, 1 / 60.0;
109
110 for (unsigned int i = 0; i < num_polygons; ++i) {
111 const PolygonT& polygon = polygons[i];
112
113 // compute the center of the polygon
114 Vec3f plane_center(0, 0, 0);
115 for (size_type j = 0; j < polygon.size(); ++j)
116 plane_center += points[polygon[(index_type)j]];
117 plane_center /= polygon.size();
118
119 // compute the volume of tetrahedron making by neighboring two points, the
120 // plane center and the reference point (zero) of the convex shape
121 const Vec3f& v3 = plane_center;
122 for (size_type j = 0; j < polygon.size(); ++j) {
123 index_type e_first = polygon[static_cast<index_type>(j)];
124 index_type e_second =
125 polygon[static_cast<index_type>((j + 1) % polygon.size())];
126 const Vec3f& v1 = points[e_first];
127 const Vec3f& v2 = points[e_second];
128 Matrix3f A;
129 A << v1.transpose(), v2.transpose(),
130 v3.transpose(); // this is A' in the original document
131 C += A.transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
132 }
133 }
134
135 return C.trace() * Matrix3f::Identity() - C;
136}
137
138template <typename PolygonT>
140 typedef typename PolygonT::size_type size_type;
141 typedef typename PolygonT::index_type index_type;
142
143 Vec3f com(0, 0, 0);
144 FCL_REAL vol = 0;
145 for (unsigned int i = 0; i < num_polygons; ++i) {
146 const PolygonT& polygon = polygons[i];
147 // compute the center of the polygon
148 Vec3f plane_center(0, 0, 0);
149 for (size_type j = 0; j < polygon.size(); ++j)
150 plane_center += points[polygon[(index_type)j]];
151 plane_center /= polygon.size();
152
153 // compute the volume of tetrahedron making by neighboring two points, the
154 // plane center and the reference point (zero) of the convex shape
155 const Vec3f& v3 = plane_center;
156 for (size_type j = 0; j < polygon.size(); ++j) {
157 index_type e_first = polygon[static_cast<index_type>(j)];
158 index_type e_second =
159 polygon[static_cast<index_type>((j + 1) % polygon.size())];
160 const Vec3f& v1 = points[e_first];
161 const Vec3f& v2 = points[e_second];
162 FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
163 vol += d_six_vol;
164 com += (points[e_first] + points[e_second] + plane_center) * d_six_vol;
165 }
166 }
167
168 return com / (vol * 4); // here we choose zero as the reference
169}
170
171template <typename PolygonT>
173 typedef typename PolygonT::size_type size_type;
174 typedef typename PolygonT::index_type index_type;
175
176 FCL_REAL vol = 0;
177 for (unsigned int i = 0; i < num_polygons; ++i) {
178 const PolygonT& polygon = polygons[i];
179
180 // compute the center of the polygon
181 Vec3f plane_center(0, 0, 0);
182 for (size_type j = 0; j < polygon.size(); ++j)
183 plane_center += points[polygon[(index_type)j]];
184 plane_center /= polygon.size();
185
186 // compute the volume of tetrahedron making by neighboring two points, the
187 // plane center and the reference point (zero point) of the convex shape
188 const Vec3f& v3 = plane_center;
189 for (size_type j = 0; j < polygon.size(); ++j) {
190 index_type e_first = polygon[static_cast<index_type>(j)];
191 index_type e_second =
192 polygon[static_cast<index_type>((j + 1) % polygon.size())];
193 const Vec3f& v1 = points[e_first];
194 const Vec3f& v2 = points[e_second];
195 FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
196 vol += d_six_vol;
197 }
198 }
199
200 return vol / 6;
201}
202
203template <typename PolygonT>
205 if (neighbors) delete[] neighbors;
206 neighbors = new Neighbors[num_points];
207
208 typedef typename PolygonT::size_type size_type;
209 typedef typename PolygonT::index_type index_type;
210 std::vector<std::set<index_type> > nneighbors(num_points);
211 unsigned int c_nneighbors = 0;
212
213 for (unsigned int l = 0; l < num_polygons; ++l) {
214 const PolygonT& polygon = polygons[l];
215 const size_type n = polygon.size();
216
217 for (size_type j = 0; j < polygon.size(); ++j) {
218 size_type i = (j == 0) ? n - 1 : j - 1;
219 size_type k = (j == n - 1) ? 0 : j + 1;
220 index_type pi = polygon[(index_type)i], pj = polygon[(index_type)j],
221 pk = polygon[(index_type)k];
222 // Update neighbors of pj;
223 if (nneighbors[pj].count(pi) == 0) {
224 c_nneighbors++;
225 nneighbors[pj].insert(pi);
226 }
227 if (nneighbors[pj].count(pk) == 0) {
228 c_nneighbors++;
229 nneighbors[pj].insert(pk);
230 }
231 }
232 }
233
234 if (nneighbors_) delete[] nneighbors_;
235 nneighbors_ = new unsigned int[c_nneighbors];
236
237 unsigned int* p_nneighbors = nneighbors_;
238 for (unsigned int i = 0; i < num_points; ++i) {
239 Neighbors& n = neighbors[i];
240 if (nneighbors[i].size() >= (std::numeric_limits<unsigned char>::max)())
241 HPP_FCL_THROW_PRETTY("Too many neighbors.", std::logic_error);
242 n.count_ = (unsigned char)nneighbors[i].size();
243 n.n_ = p_nneighbors;
244 p_nneighbors =
245 std::copy(nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors);
246 }
247 assert(p_nneighbors == nneighbors_ + c_nneighbors);
248}
249
250} // namespace fcl
251
252} // namespace hpp
253
254#endif
Base for convex polytope.
Definition: geometric_shapes.h:581
Convex polytope.
Definition: convex.h:50
virtual Convex< PolygonT > * clone() const
&#160;
Definition: convex.hxx:85
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:72
Convex()
Construct an uninitialized convex object.
Definition: convex.h:53
~Convex()
Definition: convex.hxx:67
unsigned int num_polygons
Definition: convex.h:77
Vec3f computeCOM() const
compute center of mass
Definition: convex.hxx:139
PolygonT * polygons
An array of PolygonT object. PolygonT should contains a list of vertices for each polygon,...
Definition: convex.h:76
FCL_REAL computeVolume() const
compute the volume
Definition: convex.hxx:172
void fillNeighbors()
Definition: convex.hxx:204
Matrix3f computeMomentofInertia() const
based on http://number-none.com/blow/inertia/bb_inertia.doc
Definition: convex.hxx:100
#define HPP_FCL_THROW_PRETTY(message, exception)
Definition: fwd.hh:57
void initialize(bool ownStorage, Vec3f *points_, unsigned int num_points_)
Initialize the points of the convex shape This also initializes the ConvexBase::center.
bool own_storage_
Definition: geometric_shapes.h:693
unsigned int * n_
Definition: geometric_shapes.h:628
void set(bool ownStorage, Vec3f *points_, unsigned int num_points_)
Set the points of the convex shape.
unsigned char count_
Definition: geometric_shapes.h:627
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
double FCL_REAL
Definition: data_types.h:65
Main namespace.
Definition: broadphase_bruteforce.h:44
Definition: geometric_shapes.h:626