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 |
|
|
|
35 |
|
|
/** \author Joseph Mirabel */ |
36 |
|
|
|
37 |
|
|
#ifndef HPP_FCL_SHAPE_CONVEX_HXX |
38 |
|
|
#define HPP_FCL_SHAPE_CONVEX_HXX |
39 |
|
|
|
40 |
|
|
#include <set> |
41 |
|
|
#include <vector> |
42 |
|
|
|
43 |
|
|
namespace hpp { |
44 |
|
|
namespace fcl { |
45 |
|
|
|
46 |
|
|
template <typename PolygonT> |
47 |
|
16 |
Convex<PolygonT>::Convex(bool own_storage, Vec3f* points_, |
48 |
|
|
unsigned int num_points_, PolygonT* polygons_, |
49 |
|
|
unsigned int num_polygons_) |
50 |
|
16 |
: ConvexBase(), polygons(polygons_), num_polygons(num_polygons_) { |
51 |
✓✗ |
16 |
initialize(own_storage, points_, num_points_); |
52 |
✓✗ |
16 |
fillNeighbors(); |
53 |
|
16 |
} |
54 |
|
|
|
55 |
|
|
template <typename PolygonT> |
56 |
|
|
Convex<PolygonT>::Convex(const Convex<PolygonT>& other) |
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 |
|
|
|
66 |
|
|
template <typename PolygonT> |
67 |
|
111362 |
Convex<PolygonT>::~Convex() { |
68 |
✓✗ |
111346 |
if (own_storage_) delete[] polygons; |
69 |
✓✓ |
222708 |
} |
70 |
|
|
|
71 |
|
|
template <typename PolygonT> |
72 |
|
55656 |
void Convex<PolygonT>::set(bool own_storage, Vec3f* points_, |
73 |
|
|
unsigned int num_points_, PolygonT* polygons_, |
74 |
|
|
unsigned int num_polygons_) { |
75 |
✗✓✗✗
|
55656 |
if (own_storage_) delete[] polygons; |
76 |
|
55656 |
ConvexBase::set(own_storage, points_, num_points_); |
77 |
|
|
|
78 |
|
55656 |
num_polygons = num_polygons_; |
79 |
|
55656 |
polygons = polygons_; |
80 |
|
|
|
81 |
|
55656 |
fillNeighbors(); |
82 |
|
55656 |
} |
83 |
|
|
|
84 |
|
|
template <typename PolygonT> |
85 |
|
1 |
Convex<PolygonT>* Convex<PolygonT>::clone() const { |
86 |
✓✓✓✗
|
9 |
Vec3f* cloned_points = new Vec3f[num_points]; |
87 |
|
1 |
std::copy(points, points + num_points, cloned_points); |
88 |
|
|
|
89 |
✓✓ |
13 |
PolygonT* cloned_polygons = new PolygonT[num_polygons]; |
90 |
|
1 |
std::copy(polygons, polygons + num_polygons, cloned_polygons); |
91 |
|
|
|
92 |
|
1 |
Convex* copy_ptr = new Convex(true, cloned_points, num_points, |
93 |
✓✗ |
1 |
cloned_polygons, num_polygons); |
94 |
|
|
|
95 |
|
1 |
copy_ptr->ShapeBase::operator=(*this); |
96 |
|
1 |
return copy_ptr; |
97 |
|
|
} |
98 |
|
|
|
99 |
|
|
template <typename PolygonT> |
100 |
|
|
Matrix3f Convex<PolygonT>::computeMomentofInertia() const { |
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 |
|
|
|
138 |
|
|
template <typename PolygonT> |
139 |
|
|
Vec3f Convex<PolygonT>::computeCOM() const { |
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 |
|
|
|
171 |
|
|
template <typename PolygonT> |
172 |
|
|
FCL_REAL Convex<PolygonT>::computeVolume() const { |
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 |
|
|
|
203 |
|
|
template <typename PolygonT> |
204 |
|
55673 |
void Convex<PolygonT>::fillNeighbors() { |
205 |
✗✓✗✗
|
55673 |
if (neighbors) delete[] neighbors; |
206 |
✓✗✓✗
|
55673 |
neighbors = new Neighbors[num_points]; |
207 |
|
|
|
208 |
|
|
typedef typename PolygonT::size_type size_type; |
209 |
|
|
typedef typename PolygonT::index_type index_type; |
210 |
✓✗ |
111346 |
std::vector<std::set<index_type> > nneighbors(num_points); |
211 |
|
55673 |
unsigned int c_nneighbors = 0; |
212 |
|
|
|
213 |
✓✓ |
501142 |
for (unsigned int l = 0; l < num_polygons; ++l) { |
214 |
|
445469 |
const PolygonT& polygon = polygons[l]; |
215 |
|
445469 |
const size_type n = polygon.size(); |
216 |
|
|
|
217 |
✓✓ |
1781888 |
for (size_type j = 0; j < polygon.size(); ++j) { |
218 |
✓✓ |
1336419 |
size_type i = (j == 0) ? n - 1 : j - 1; |
219 |
✓✓ |
1336419 |
size_type k = (j == n - 1) ? 0 : j + 1; |
220 |
|
1336419 |
index_type pi = polygon[(index_type)i], pj = polygon[(index_type)j], |
221 |
|
1336419 |
pk = polygon[(index_type)k]; |
222 |
|
|
// Update neighbors of pj; |
223 |
✓✗✓✓
|
1336419 |
if (nneighbors[pj].count(pi) == 0) { |
224 |
|
696039 |
c_nneighbors++; |
225 |
✓✗ |
696039 |
nneighbors[pj].insert(pi); |
226 |
|
|
} |
227 |
✓✗✓✓
|
1336419 |
if (nneighbors[pj].count(pk) == 0) { |
228 |
|
696039 |
c_nneighbors++; |
229 |
✓✗ |
696039 |
nneighbors[pj].insert(pk); |
230 |
|
|
} |
231 |
|
|
} |
232 |
|
|
} |
233 |
|
|
|
234 |
✗✓✗✗
|
55673 |
if (nneighbors_) delete[] nneighbors_; |
235 |
✓✗✓✗
|
55673 |
nneighbors_ = new unsigned int[c_nneighbors]; |
236 |
|
|
|
237 |
|
55673 |
unsigned int* p_nneighbors = nneighbors_; |
238 |
✓✓ |
501072 |
for (unsigned int i = 0; i < num_points; ++i) { |
239 |
|
445399 |
Neighbors& n = neighbors[i]; |
240 |
✗✓ |
445399 |
if (nneighbors[i].size() >= (std::numeric_limits<unsigned char>::max)()) |
241 |
|
|
HPP_FCL_THROW_PRETTY("Too many neighbors.", std::logic_error); |
242 |
|
445399 |
n.count_ = (unsigned char)nneighbors[i].size(); |
243 |
|
445399 |
n.n_ = p_nneighbors; |
244 |
|
|
p_nneighbors = |
245 |
✓✗ |
445399 |
std::copy(nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors); |
246 |
|
|
} |
247 |
✗✓ |
55673 |
assert(p_nneighbors == nneighbors_ + c_nneighbors); |
248 |
|
55673 |
} |
249 |
|
|
|
250 |
|
|
} // namespace fcl |
251 |
|
|
|
252 |
|
|
} // namespace hpp |
253 |
|
|
|
254 |
|
|
#endif |