hpp-fcl: include/hpp/fcl/shape/geometric_shape_to_BVH_model.h Source File

hpp-fcl 1.7.5 HPP fork of FCL -- The Flexible Collision Library

geometric_shape_to_BVH_model.h
Go to the documentation of this file.
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2015, Open Source Robotics Foundation
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Open Source Robotics Foundation nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 */
35
39 #ifndef GEOMETRIC_SHAPE_TO_BVH_MODEL_H
40 #define GEOMETRIC_SHAPE_TO_BVH_MODEL_H
41
42 #include <hpp/fcl/shape/geometric_shapes.h>
43 #include <hpp/fcl/BVH/BVH_model.h>
44 #include <boost/math/constants/constants.hpp>
45
46 namespace hpp
47 {
48 namespace fcl
49 {
50
52 template<typename BV>
53 void generateBVHModel(BVHModel<BV>& model, const Box& shape, const Transform3f& pose)
54 {
55 FCL_REAL a = shape.halfSide[0];
56 FCL_REAL b = shape.halfSide[1];
57 FCL_REAL c = shape.halfSide[2];
58 std::vector<Vec3f> points(8);
59 std::vector<Triangle> tri_indices(12);
60 points[0] = Vec3f ( a, -b, c);
61 points[1] = Vec3f ( a, b, c);
62 points[2] = Vec3f (-a, b, c);
63 points[3] = Vec3f (-a, -b, c);
64 points[4] = Vec3f ( a, -b, -c);
65 points[5] = Vec3f ( a, b, -c);
66 points[6] = Vec3f (-a, b, -c);
67 points[7] = Vec3f (-a, -b, -c);
68
69 tri_indices[ 0].set(0, 4, 1);
70 tri_indices[ 1].set(1, 4, 5);
71 tri_indices[ 2].set(2, 6, 3);
72 tri_indices[ 3].set(3, 6, 7);
73 tri_indices[ 4].set(3, 0, 2);
74 tri_indices[ 5].set(2, 0, 1);
75 tri_indices[ 6].set(6, 5, 7);
76 tri_indices[ 7].set(7, 5, 4);
77 tri_indices[ 8].set(1, 5, 2);
78 tri_indices[ 9].set(2, 5, 6);
79 tri_indices[10].set(3, 7, 0);
80 tri_indices[11].set(0, 7, 4);
81
82 for(unsigned int i = 0; i < points.size(); ++i)
83 {
84 points[i] = pose.transform(points[i]).eval();
85 }
86
87 model.beginModel();
88 model.addSubModel(points, tri_indices);
89 model.endModel();
90 model.computeLocalAABB();
91 }
92
94 template<typename BV>
95 void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3f& pose, unsigned int seg, unsigned int ring)
96 {
97 std::vector<Vec3f> points;
98 std::vector<Triangle> tri_indices;
99
100 FCL_REAL r = shape.radius;
101 FCL_REAL phi, phid;
102 const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
103 phid = pi * 2 / seg;
104 phi = 0;
105
106 FCL_REAL theta, thetad;
107 thetad = pi / (ring + 1);
108 theta = 0;
109
110 for(unsigned int i = 0; i < ring; ++i)
111 {
112 FCL_REAL theta_ = theta + thetad * (i + 1);
113 for(unsigned int j = 0; j < seg; ++j)
114 {
115 points.push_back(Vec3f(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), r * cos(theta_)));
116 }
117 }
118 points.push_back(Vec3f(0, 0, r));
119 points.push_back(Vec3f(0, 0, -r));
120
121 for(unsigned int i = 0; i < ring - 1; ++i)
122 {
123 for(unsigned int j = 0; j < seg; ++j)
124 {
125 unsigned int a, b, c, d;
126 a = i * seg + j;
127 b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
128 c = (i + 1) * seg + j;
129 d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
130 tri_indices.push_back(Triangle(a, c, b));
131 tri_indices.push_back(Triangle(b, c, d));
132 }
133 }
134
135 for(unsigned int j = 0; j < seg; ++j)
136 {
137 unsigned int a, b;
138 a = j;
139 b = (j == seg - 1) ? 0 : (j + 1);
140 tri_indices.push_back(Triangle(ring * seg, a, b));
141
142 a = (ring - 1) * seg + j;
143 b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
144 tri_indices.push_back(Triangle(a, ring * seg + 1, b));
145 }
146
147 for(unsigned int i = 0; i < points.size(); ++i)
148 {
149 points[i] = pose.transform(points[i]);
150 }
151
152 model.beginModel();
153 model.addSubModel(points, tri_indices);
154 model.endModel();
155 model.computeLocalAABB();
156 }
157
161 template<typename BV>
162 void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3f& pose, unsigned int n_faces_for_unit_sphere)
163 {
164 FCL_REAL r = shape.radius;
165 FCL_REAL n_low_bound = std::sqrt((FCL_REAL)n_faces_for_unit_sphere / FCL_REAL(2.)) * r * r;
166 unsigned int ring = (unsigned int) ceil(n_low_bound);
167 unsigned int seg = (unsigned int) ceil(n_low_bound);
168
169 generateBVHModel(model, shape, pose, seg, ring);
170 }
171
172
174 template<typename BV>
175 void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transform3f& pose, unsigned int tot, unsigned int h_num)
176 {
177 std::vector<Vec3f> points;
178 std::vector<Triangle> tri_indices;
179
180 FCL_REAL r = shape.radius;
181 FCL_REAL h = shape.halfLength;
182 FCL_REAL phi, phid;
183 const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
184 phid = pi * 2 / tot;
185 phi = 0;
186
187 FCL_REAL hd = 2 * h / h_num;
188
189 for(unsigned int i = 0; i < tot; ++i)
190 points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h));
191
192 for(unsigned int i = 0; i < h_num - 1; ++i)
193 {
194 for(unsigned int j = 0; j < tot; ++j)
195 {
196 points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j), h - (i + 1) * hd));
197 }
198 }
199
200 for(unsigned int i = 0; i < tot; ++i)
201 points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h));
202
203 points.push_back(Vec3f(0, 0, h));
204 points.push_back(Vec3f(0, 0, -h));
205
206 for(unsigned int i = 0; i < tot; ++i)
207 {
208 Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1)));
209 tri_indices.push_back(tmp);
210 }
211
212 for(unsigned int i = 0; i < tot; ++i)
213 {
214 Triangle tmp((h_num + 1) * tot + 1, h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i);
215 tri_indices.push_back(tmp);
216 }
217
218 for(unsigned int i = 0; i < h_num; ++i)
219 {
220 for(unsigned int j = 0; j < tot; ++j)
221 {
222 int a, b, c, d;
223 a = j;
224 b = (j == tot - 1) ? 0 : (j + 1);
225 c = j + tot;
226 d = (j == tot - 1) ? tot : (j + 1 + tot);
227
228 int start = i * tot;
229 tri_indices.push_back(Triangle(start + b, start + a, start + c));
230 tri_indices.push_back(Triangle(start + b, start + c, start + d));
231 }
232 }
233
234 for(unsigned int i = 0; i < points.size(); ++i)
235 {
236 points[i] = pose.transform(points[i]);
237 }
238
239 model.beginModel();
240 model.addSubModel(points, tri_indices);
241 model.endModel();
242 model.computeLocalAABB();
243 }
244
248 template<typename BV>
249 void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transform3f& pose, unsigned int tot_for_unit_cylinder)
250 {
251 FCL_REAL r = shape.radius;
252 FCL_REAL h = 2 * shape.halfLength;
253
254 const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
255 unsigned int tot = (unsigned int)(tot_for_unit_cylinder * r);
256 FCL_REAL phid = pi * 2 / tot;
257
258 FCL_REAL circle_edge = phid * r;
259 unsigned int h_num = (unsigned int)ceil(h / circle_edge);
260
261 generateBVHModel(model, shape, pose, tot, h_num);
262 }
263
264
266 template<typename BV>
267 void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f& pose, unsigned int tot, unsigned int h_num)
268 {
269 std::vector<Vec3f> points;
270 std::vector<Triangle> tri_indices;
271
272 FCL_REAL r = shape.radius;
273 FCL_REAL h = shape.halfLength;
274
275 FCL_REAL phi, phid;
276 const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
277 phid = pi * 2 / tot;
278 phi = 0;
279
280 FCL_REAL hd = 2 * h / h_num;
281
282 for(unsigned int i = 0; i < h_num - 1; ++i)
283 {
284 FCL_REAL h_i = h - (i + 1) * hd;
285 FCL_REAL rh = r * (0.5 - h_i / h / 2);
286 for(unsigned int j = 0; j < tot; ++j)
287 {
288 points.push_back(Vec3f(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i));
289 }
290 }
291
292 for(unsigned int i = 0; i < tot; ++i)
293 points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h));
294
295 points.push_back(Vec3f(0, 0, h));
296 points.push_back(Vec3f(0, 0, -h));
297
298 for(unsigned int i = 0; i < tot; ++i)
299 {
300 Triangle tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1));
301 tri_indices.push_back(tmp);
302 }
303
304 for(unsigned int i = 0; i < tot; ++i)
305 {
306 Triangle tmp(h_num * tot + 1, (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)), (h_num - 1) * tot + i);
307 tri_indices.push_back(tmp);
308 }
309
310 for(unsigned int i = 0; i < h_num - 1; ++i)
311 {
312 for(unsigned int j = 0; j < tot; ++j)
313 {
314 int a, b, c, d;
315 a = j;
316 b = (j == tot - 1) ? 0 : (j + 1);
317 c = j + tot;
318 d = (j == tot - 1) ? tot : (j + 1 + tot);
319
320 int start = i * tot;
321 tri_indices.push_back(Triangle(start + b, start + a, start + c));
322 tri_indices.push_back(Triangle(start + b, start + c, start + d));
323 }
324 }
325
326 for(unsigned int i = 0; i < points.size(); ++i)
327 {
328 points[i] = pose.transform(points[i]);
329 }
330
331 model.beginModel();
332 model.addSubModel(points, tri_indices);
333 model.endModel();
334 model.computeLocalAABB();
335 }
336
340 template<typename BV>
341 void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f& pose, unsigned int tot_for_unit_cone)
342 {
343 FCL_REAL r = shape.radius;
344 FCL_REAL h = 2 * shape.halfLength;
345
346 const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
347 unsigned int tot = (unsigned int)(tot_for_unit_cone * r);
348 FCL_REAL phid = pi * 2 / tot;
349
350 FCL_REAL circle_edge = phid * r;
351 unsigned int h_num = (unsigned int)ceil(h / circle_edge);
352
353 generateBVHModel(model, shape, pose, tot, h_num);
354 }
355
356 }
357
358 } // namespace hpp
359
360 #endif
BVH_model.h
hpp::fcl::BVHModelBase::addSubModel
int addSubModel(const std::vector< Vec3f > &ps, const std::vector< Triangle > &ts)
Add a set of triangles in the new BVH model.
hpp::fcl::BVHModelBase::endModel
int endModel()
End BVH model construction, will build the bounding volume hierarchy.
hpp::fcl::BVHModelBase::computeLocalAABB
void computeLocalAABB()
Compute the AABB for the BVH, used for broad-phase collision.
hpp::fcl::BVHModelBase::beginModel
int beginModel(int num_tris=0, int num_vertices=0)
Begin a new BVH model.
hpp::fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:278
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: geometric_shapes.h:103
hpp::fcl::Cone
Cone The base of the cone is at and the top is at .
Definition: geometric_shapes.h:250
hpp::fcl::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: geometric_shapes.h:306
hpp::fcl::Sphere
Center at zero point sphere.
Definition: geometric_shapes.h:150
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:59
hpp::fcl::Transform3f::transform
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:173
hpp::fcl::Triangle
Triangle with 3 indices for points.
Definition: data_types.h:75
hpp::fcl::Cylinder::halfLength
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:327
hpp::fcl::Cone::halfLength
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:271
hpp::fcl::Sphere::radius
FCL_REAL radius
Radius of the sphere.
Definition: geometric_shapes.h:163
hpp::fcl::Box::halfSide
Vec3f halfSide
box side half-length
Definition: geometric_shapes.h:125
hpp::fcl::Cone::radius
FCL_REAL radius
Radius of the cone.
Definition: geometric_shapes.h:265
hpp::fcl::Cylinder::radius
FCL_REAL radius
Radius of the cylinder.
Definition: geometric_shapes.h:321
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
hpp::fcl::generateBVHModel
void generateBVHModel(BVHModel< BV > &model, const Box &shape, const Transform3f &pose)
Generate BVH model from box.
Definition: geometric_shape_to_BVH_model.h:53
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:66
hpp
Main namespace.
Definition: AABB.h:44
geometric_shapes.h