hpp-fcl 1.8.1
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
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
44#include <boost/math/constants/constants.hpp>
45
46namespace hpp
47{
48namespace fcl
49{
50
52template<typename BV>
53void 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
94template<typename BV>
95void 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
161template<typename BV>
162void 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
174template<typename BV>
175void 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 unsigned 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 unsigned 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
248template<typename BV>
249void 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
266template<typename BV>
267void 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 unsigned 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 unsigned 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
340template<typename BV>
341void 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
int addSubModel(const std::vector< Vec3f > &ps, const std::vector< Triangle > &ts)
Add a set of triangles in the new BVH model.
int endModel()
End BVH model construction, will build the bounding volume hierarchy.
void computeLocalAABB()
Compute the AABB for the BVH, used for broad-phase collision.
int beginModel(unsigned int num_tris=0, unsigned int num_vertices=0)
Begin a new BVH model.
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:278
Center at zero point, axis aligned box.
Definition: geometric_shapes.h:103
Cone The base of the cone is at and the top is at .
Definition: geometric_shapes.h:250
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: geometric_shapes.h:306
Center at zero point sphere.
Definition: geometric_shapes.h:150
Simple transform class used locally by InterpMotion.
Definition: transform.h:59
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:173
Triangle with 3 indices for points.
Definition: data_types.h:77
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:327
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:271
FCL_REAL radius
Radius of the sphere.
Definition: geometric_shapes.h:166
Vec3f halfSide
box side half-length
Definition: geometric_shapes.h:125
FCL_REAL radius
Radius of the cone.
Definition: geometric_shapes.h:268
FCL_REAL radius
Radius of the cylinder.
Definition: geometric_shapes.h:324
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
void generateBVHModel(BVHModel< BV > &model, const Box &shape, const Transform3f &pose)
Generate BVH model from box.
Definition: geometric_shape_to_BVH_model.h:53
double FCL_REAL
Definition: data_types.h:66
Main namespace.
Definition: AABB.h:44