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 
41 #include <hpp/fcl/shape/geometric_shapes.h>
42 #include <hpp/fcl/BVH/BVH_model.h>
43 #include <boost/math/constants/constants.hpp>
44 
45 namespace hpp {
46 namespace fcl {
47 
49 template <typename BV>
50 void 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 
91 template <typename BV>
92 void 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 
157 template <typename BV>
158 void 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 
172 template <typename BV>
173 void 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 
245 template <typename BV>
246 void 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 
264 template <typename BV>
265 void 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 
337 template <typename BV>
338 void 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
hpp::fcl::Box::halfSide
Vec3f halfSide
box side half-length
Definition: shape/geometric_shapes.h:148
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
generate_distance_plot.b
float b
Definition: generate_distance_plot.py:8
hpp::fcl::Cone::halfLength
FCL_REAL halfLength
Half Length along z axis.
Definition: shape/geometric_shapes.h:433
hpp::fcl::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: shape/geometric_shapes.h:501
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
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.
Definition: BVH_model.cpp:410
geometric_shapes.h
c
c
hpp::fcl::Transform3f::transform
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:153
hpp::fcl::Cone::radius
FCL_REAL radius
Radius of the cone.
Definition: shape/geometric_shapes.h:427
BVH_model.h
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::BVHModelBase::beginModel
int beginModel(unsigned int num_tris=0, unsigned int num_vertices=0)
Begin a new BVH model.
Definition: BVH_model.cpp:170
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: shape/geometric_shapes.h:125
hpp::fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH/BVH_model.h:273
hpp::fcl::Cone
Cone The base of the cone is at and the top is at .
Definition: shape/geometric_shapes.h:414
hpp::fcl::Sphere::radius
FCL_REAL radius
Radius of the sphere.
Definition: shape/geometric_shapes.h:206
hpp::fcl::Cylinder::radius
FCL_REAL radius
Radius of the cylinder.
Definition: shape/geometric_shapes.h:522
hpp::fcl::Sphere
Center at zero point sphere.
Definition: shape/geometric_shapes.h:196
hpp::fcl::Triangle
Triangle with 3 indices for points.
Definition: data_types.h:96
hpp::fcl::Cylinder::halfLength
FCL_REAL halfLength
Half Length along z axis.
Definition: shape/geometric_shapes.h:528
a
list a
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::BVHModelBase::computeLocalAABB
void computeLocalAABB()
Compute the AABB for the BVH, used for broad-phase collision.
Definition: BVH_model.cpp:736
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:50
hpp::fcl::BVHModelBase::endModel
int endModel()
End BVH model construction, will build the bounding volume hierarchy.
Definition: BVH_model.cpp:473

hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:01