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

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
44namespace hpp
45{
46namespace fcl
47{
48
49template <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
59template <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 polygons = new PolygonT[num_polygons];
67 memcpy(polygons, other.polygons, sizeof(PolygonT) * num_polygons);
68 }
69}
70
71template <typename PolygonT>
72Convex<PolygonT>::~Convex()
73{
74 if (own_storage_) delete [] polygons;
75}
76
77template <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
91template <typename PolygonT>
92 Matrix3f Convex<PolygonT>::computeMomentofInertia() const
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
130template <typename PolygonT>
131 Vec3f Convex<PolygonT>::computeCOM() const
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
164template <typename PolygonT>
165 FCL_REAL Convex<PolygonT>::computeVolume() const
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
197template <typename PolygonT>
198 void Convex<PolygonT>::fillNeighbors()
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
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::set
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
hpp::fcl::Convex::~Convex
~Convex()
Definition: convex.hxx:72
hpp::fcl::Convex::num_polygons
unsigned int num_polygons
Definition: convex.h:81
hpp::fcl::Convex::computeCOM
Vec3f computeCOM() const
compute center of mass
Definition: convex.hxx:131
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:165
hpp::fcl::Convex::fillNeighbors
void fillNeighbors()
Definition: convex.hxx:198
hpp::fcl::Convex::computeMomentofInertia
Matrix3f computeMomentofInertia() const
based on http://number-none.com/blow/inertia/bb_inertia.doc
Definition: convex.hxx:92
hpp::fcl::ConvexBase::initialize
void initialize(bool ownStorage, Vec3f *points_, unsigned int num_points_)
Initialize the points of the convex shape This also initializes the ConvexBase::center.
hpp::fcl::ConvexBase::own_storage_
bool own_storage_
Definition: geometric_shapes.h:445
hpp::fcl::ConvexBase::Neighbors::n_
unsigned int * n_
Definition: geometric_shapes.h:404
hpp::fcl::ConvexBase::set
void set(bool ownStorage, Vec3f *points_, unsigned int num_points_)
Set the points of the convex shape.
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:69
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