hpp-fcl  1.4.4
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  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
Vec3f halfSide
box side half-length
Definition: geometric_shapes.h:100
Simple transform class used locally by InterpMotion.
Definition: transform.h:59
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:211
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: geometric_shapes.h:243
Main namespace.
Definition: AABB.h:43
int addSubModel(const std::vector< Vec3f > &ps, const std::vector< Triangle > &ts)
Add a set of triangles in the new BVH model.
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:169
FCL_REAL radius
Radius of the cone.
Definition: geometric_shapes.h:208
double FCL_REAL
Definition: data_types.h:68
Center at zero point, axis aligned box.
Definition: geometric_shapes.h:86
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:255
Cone The base of the cone is at and the top is at .
Definition: geometric_shapes.h:199
FCL_REAL radius
Radius of the sphere.
Definition: geometric_shapes.h:130
FCL_REAL radius
Radius of the cylinder.
Definition: geometric_shapes.h:252
Center at zero point sphere.
Definition: geometric_shapes.h:122
Triangle with 3 indices for points.
Definition: data_types.h:77
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:255
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:73
void computeLocalAABB()
Compute the AABB for the BVH, used for broad-phase collision.
int beginModel(int num_tris=0, int num_vertices=0)
Begin a new BVH model.
void generateBVHModel(BVHModel< BV > &model, const Box &shape, const Transform3f &pose)
Generate BVH model from box.
Definition: geometric_shape_to_BVH_model.h:53
int endModel()
End BVH model construction, will build the bounding volume hierarchy.