hpp-fcl 2.4.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
38#ifndef GEOMETRIC_SHAPE_TO_BVH_MODEL_H
39#define GEOMETRIC_SHAPE_TO_BVH_MODEL_H
40
43#include <boost/math/constants/constants.hpp>
44
45namespace hpp {
46namespace fcl {
47
49template <typename BV>
50void generateBVHModel(BVHModel<BV>& model, const Box& shape,
51 const Transform3f& pose) {
52 FCL_REAL a = shape.halfSide[0];
53 FCL_REAL b = shape.halfSide[1];
54 FCL_REAL c = shape.halfSide[2];
55 std::vector<Vec3f> points(8);
56 std::vector<Triangle> tri_indices(12);
57 points[0] = Vec3f(a, -b, c);
58 points[1] = Vec3f(a, b, c);
59 points[2] = Vec3f(-a, b, c);
60 points[3] = Vec3f(-a, -b, c);
61 points[4] = Vec3f(a, -b, -c);
62 points[5] = Vec3f(a, b, -c);
63 points[6] = Vec3f(-a, b, -c);
64 points[7] = Vec3f(-a, -b, -c);
65
66 tri_indices[0].set(0, 4, 1);
67 tri_indices[1].set(1, 4, 5);
68 tri_indices[2].set(2, 6, 3);
69 tri_indices[3].set(3, 6, 7);
70 tri_indices[4].set(3, 0, 2);
71 tri_indices[5].set(2, 0, 1);
72 tri_indices[6].set(6, 5, 7);
73 tri_indices[7].set(7, 5, 4);
74 tri_indices[8].set(1, 5, 2);
75 tri_indices[9].set(2, 5, 6);
76 tri_indices[10].set(3, 7, 0);
77 tri_indices[11].set(0, 7, 4);
78
79 for (unsigned int i = 0; i < points.size(); ++i) {
80 points[i] = pose.transform(points[i]).eval();
81 }
82
83 model.beginModel();
84 model.addSubModel(points, tri_indices);
85 model.endModel();
86 model.computeLocalAABB();
87}
88
91template <typename BV>
92void generateBVHModel(BVHModel<BV>& model, const Sphere& shape,
93 const Transform3f& pose, unsigned int seg,
94 unsigned int ring) {
95 std::vector<Vec3f> points;
96 std::vector<Triangle> tri_indices;
97
98 FCL_REAL r = shape.radius;
99 FCL_REAL phi, phid;
100 const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
101 phid = pi * 2 / seg;
102 phi = 0;
103
104 FCL_REAL theta, thetad;
105 thetad = pi / (ring + 1);
106 theta = 0;
107
108 for (unsigned int i = 0; i < ring; ++i) {
109 FCL_REAL theta_ = theta + thetad * (i + 1);
110 for (unsigned int j = 0; j < seg; ++j) {
111 points.push_back(Vec3f(r * sin(theta_) * cos(phi + j * phid),
112 r * sin(theta_) * sin(phi + j * phid),
113 r * cos(theta_)));
114 }
115 }
116 points.push_back(Vec3f(0, 0, r));
117 points.push_back(Vec3f(0, 0, -r));
118
119 for (unsigned int i = 0; i < ring - 1; ++i) {
120 for (unsigned int j = 0; j < seg; ++j) {
121 unsigned int a, b, c, d;
122 a = i * seg + j;
123 b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
124 c = (i + 1) * seg + j;
125 d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
126 tri_indices.push_back(Triangle(a, c, b));
127 tri_indices.push_back(Triangle(b, c, d));
128 }
129 }
130
131 for (unsigned int j = 0; j < seg; ++j) {
132 unsigned int a, b;
133 a = j;
134 b = (j == seg - 1) ? 0 : (j + 1);
135 tri_indices.push_back(Triangle(ring * seg, a, b));
136
137 a = (ring - 1) * seg + j;
138 b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
139 tri_indices.push_back(Triangle(a, ring * seg + 1, b));
140 }
141
142 for (unsigned int i = 0; i < points.size(); ++i) {
143 points[i] = pose.transform(points[i]);
144 }
145
146 model.beginModel();
147 model.addSubModel(points, tri_indices);
148 model.endModel();
149 model.computeLocalAABB();
150}
151
157template <typename BV>
158void generateBVHModel(BVHModel<BV>& model, const Sphere& shape,
159 const Transform3f& pose,
160 unsigned int n_faces_for_unit_sphere) {
161 FCL_REAL r = shape.radius;
162 FCL_REAL n_low_bound =
163 std::sqrt((FCL_REAL)n_faces_for_unit_sphere / FCL_REAL(2.)) * r * r;
164 unsigned int ring = (unsigned int)ceil(n_low_bound);
165 unsigned int seg = (unsigned int)ceil(n_low_bound);
166
167 generateBVHModel(model, shape, pose, seg, ring);
168}
169
172template <typename BV>
173void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape,
174 const Transform3f& pose, unsigned int tot,
175 unsigned int h_num) {
176 std::vector<Vec3f> points;
177 std::vector<Triangle> tri_indices;
178
179 FCL_REAL r = shape.radius;
180 FCL_REAL h = shape.halfLength;
181 FCL_REAL phi, phid;
182 const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
183 phid = pi * 2 / tot;
184 phi = 0;
185
186 FCL_REAL hd = 2 * h / h_num;
187
188 for (unsigned int i = 0; i < tot; ++i)
189 points.push_back(
190 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 for (unsigned int j = 0; j < tot; ++j) {
194 points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j),
195 h - (i + 1) * hd));
196 }
197 }
198
199 for (unsigned int i = 0; i < tot; ++i)
200 points.push_back(
201 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 Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1)));
208 tri_indices.push_back(tmp);
209 }
210
211 for (unsigned int i = 0; i < tot; ++i) {
212 Triangle tmp((h_num + 1) * tot + 1,
213 h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i);
214 tri_indices.push_back(tmp);
215 }
216
217 for (unsigned int i = 0; i < h_num; ++i) {
218 for (unsigned int j = 0; j < tot; ++j) {
219 unsigned int a, b, c, d;
220 a = j;
221 b = (j == tot - 1) ? 0 : (j + 1);
222 c = j + tot;
223 d = (j == tot - 1) ? tot : (j + 1 + tot);
224
225 unsigned int start = i * tot;
226 tri_indices.push_back(Triangle(start + b, start + a, start + c));
227 tri_indices.push_back(Triangle(start + b, start + c, start + d));
228 }
229 }
230
231 for (unsigned int i = 0; i < points.size(); ++i) {
232 points[i] = pose.transform(points[i]);
233 }
234
235 model.beginModel();
236 model.addSubModel(points, tri_indices);
237 model.endModel();
238 model.computeLocalAABB();
239}
240
245template <typename BV>
246void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape,
247 const Transform3f& pose,
248 unsigned int tot_for_unit_cylinder) {
249 FCL_REAL r = shape.radius;
250 FCL_REAL h = 2 * shape.halfLength;
251
252 const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
253 unsigned int tot = (unsigned int)(tot_for_unit_cylinder * r);
254 FCL_REAL phid = pi * 2 / tot;
255
256 FCL_REAL circle_edge = phid * r;
257 unsigned int h_num = (unsigned int)ceil(h / circle_edge);
258
259 generateBVHModel(model, shape, pose, tot, h_num);
260}
261
264template <typename BV>
265void generateBVHModel(BVHModel<BV>& model, const Cone& shape,
266 const Transform3f& pose, unsigned int tot,
267 unsigned int h_num) {
268 std::vector<Vec3f> points;
269 std::vector<Triangle> tri_indices;
270
271 FCL_REAL r = shape.radius;
272 FCL_REAL h = shape.halfLength;
273
274 FCL_REAL phi, phid;
275 const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
276 phid = pi * 2 / tot;
277 phi = 0;
278
279 FCL_REAL hd = 2 * h / h_num;
280
281 for (unsigned int i = 0; i < h_num - 1; ++i) {
282 FCL_REAL h_i = h - (i + 1) * hd;
283 FCL_REAL rh = r * (0.5 - h_i / h / 2);
284 for (unsigned int j = 0; j < tot; ++j) {
285 points.push_back(
286 Vec3f(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i));
287 }
288 }
289
290 for (unsigned int i = 0; i < tot; ++i)
291 points.push_back(
292 Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), -h));
293
294 points.push_back(Vec3f(0, 0, h));
295 points.push_back(Vec3f(0, 0, -h));
296
297 for (unsigned int i = 0; i < tot; ++i) {
298 Triangle tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1));
299 tri_indices.push_back(tmp);
300 }
301
302 for (unsigned int i = 0; i < tot; ++i) {
303 Triangle tmp(h_num * tot + 1,
304 (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)),
305 (h_num - 1) * tot + i);
306 tri_indices.push_back(tmp);
307 }
308
309 for (unsigned int i = 0; i < h_num - 1; ++i) {
310 for (unsigned int j = 0; j < tot; ++j) {
311 unsigned int a, b, c, d;
312 a = j;
313 b = (j == tot - 1) ? 0 : (j + 1);
314 c = j + tot;
315 d = (j == tot - 1) ? tot : (j + 1 + tot);
316
317 unsigned int start = i * tot;
318 tri_indices.push_back(Triangle(start + b, start + a, start + c));
319 tri_indices.push_back(Triangle(start + b, start + c, start + d));
320 }
321 }
322
323 for (unsigned int i = 0; i < points.size(); ++i) {
324 points[i] = pose.transform(points[i]);
325 }
326
327 model.beginModel();
328 model.addSubModel(points, tri_indices);
329 model.endModel();
330 model.computeLocalAABB();
331}
332
337template <typename BV>
338void generateBVHModel(BVHModel<BV>& model, const Cone& shape,
339 const Transform3f& pose, unsigned int tot_for_unit_cone) {
340 FCL_REAL r = shape.radius;
341 FCL_REAL h = 2 * shape.halfLength;
342
343 const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
344 unsigned int tot = (unsigned int)(tot_for_unit_cone * r);
345 FCL_REAL phid = pi * 2 / tot;
346
347 FCL_REAL circle_edge = phid * r;
348 unsigned int h_num = (unsigned int)ceil(h / circle_edge);
349
350 generateBVHModel(model, shape, pose, tot, h_num);
351}
352
353} // namespace fcl
354
355} // namespace hpp
356
357#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:273
Center at zero point, axis aligned box.
Definition: geometric_shapes.h:125
Cone The base of the cone is at and the top is at .
Definition: geometric_shapes.h:414
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: geometric_shapes.h:501
Center at zero point sphere.
Definition: geometric_shapes.h:196
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:153
Triangle with 3 indices for points.
Definition: data_types.h:96
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:528
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:433
FCL_REAL radius
Radius of the sphere.
Definition: geometric_shapes.h:209
Vec3f halfSide
box side half-length
Definition: geometric_shapes.h:148
FCL_REAL radius
Radius of the cone.
Definition: geometric_shapes.h:430
FCL_REAL radius
Radius of the cylinder.
Definition: geometric_shapes.h:525
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
void generateBVHModel(BVHModel< BV > &model, const Box &shape, const Transform3f &pose)
Generate BVH model from box.
Definition: geometric_shape_to_BVH_model.h:50
double FCL_REAL
Definition: data_types.h:65
Main namespace.
Definition: broadphase_bruteforce.h:44