hpp-fcl  1.8.1
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 
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  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 
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  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 
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
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:163
Vec3f halfSide
box side half-length
Definition: geometric_shapes.h:125
FCL_REAL radius
Radius of the cone.
Definition: geometric_shapes.h:265
FCL_REAL radius
Radius of the cylinder.
Definition: geometric_shapes.h:321
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