FCL  0.6.0
Flexible Collision Library
geometric_shape_to_BVH_model-inl.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, 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 FCL_SHAPE_GEOMETRICSHAPETOBVHMODEL_INL_H
39 #define FCL_SHAPE_GEOMETRICSHAPETOBVHMODEL_INL_H
40 
41 #include "fcl/geometry/geometric_shape_to_BVH_model.h"
42 
43 namespace fcl
44 {
45 
46 //==============================================================================
47 template<typename BV>
48 void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose)
49 {
50  using S = typename BV::S;
51 
52  S a = shape.side[0];
53  S b = shape.side[1];
54  S c = shape.side[2];
55  std::vector<Vector3<S>> points(8);
56  std::vector<Triangle> tri_indices(12);
57  points[0] << 0.5 * a, -0.5 * b, 0.5 * c;
58  points[1] << 0.5 * a, 0.5 * b, 0.5 * c;
59  points[2] << -0.5 * a, 0.5 * b, 0.5 * c;
60  points[3] << -0.5 * a, -0.5 * b, 0.5 * c;
61  points[4] << 0.5 * a, -0.5 * b, -0.5 * c;
62  points[5] << 0.5 * a, 0.5 * b, -0.5 * c;
63  points[6] << -0.5 * a, 0.5 * b, -0.5 * c;
64  points[7] << -0.5 * a, -0.5 * b, -0.5 * 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  {
81  points[i] = pose * points[i];
82  }
83 
84  model.beginModel();
85  model.addSubModel(points, tri_indices);
86  model.endModel();
87  model.computeLocalAABB();
88 }
89 
90 //==============================================================================
91 template<typename BV>
92 void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring)
93 {
94  using S = typename BV::S;
95 
96  std::vector<Vector3<S>> points;
97  std::vector<Triangle> tri_indices;
98 
99  S r = shape.radius;
100  S phi, phid;
101  const S pi = constants<S>::pi();
102  phid = pi * 2 / seg;
103  phi = 0;
104 
105  S theta, thetad;
106  thetad = pi / (ring + 1);
107  theta = 0;
108 
109  for(unsigned int i = 0; i < ring; ++i)
110  {
111  S theta_ = theta + thetad * (i + 1);
112  for(unsigned int j = 0; j < seg; ++j)
113  {
114  points.emplace_back(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), r * cos(theta_));
115  }
116  }
117  points.emplace_back(0, 0, r);
118  points.emplace_back(0, 0, -r);
119 
120  for(unsigned int i = 0; i < ring - 1; ++i)
121  {
122  for(unsigned int j = 0; j < seg; ++j)
123  {
124  unsigned int a, b, c, d;
125  a = i * seg + j;
126  b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
127  c = (i + 1) * seg + j;
128  d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
129  tri_indices.emplace_back(a, c, b);
130  tri_indices.emplace_back(b, c, d);
131  }
132  }
133 
134  for(unsigned int j = 0; j < seg; ++j)
135  {
136  unsigned int a, b;
137  a = j;
138  b = (j == seg - 1) ? 0 : (j + 1);
139  tri_indices.emplace_back(ring * seg, a, b);
140 
141  a = (ring - 1) * seg + j;
142  b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
143  tri_indices.emplace_back(a, ring * seg + 1, b);
144  }
145 
146  for(unsigned int i = 0; i < points.size(); ++i)
147  {
148  points[i] = pose * points[i];
149  }
150 
151  model.beginModel();
152  model.addSubModel(points, tri_indices);
153  model.endModel();
154  model.computeLocalAABB();
155 }
156 
157 //==============================================================================
158 template<typename BV>
159 void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_sphere)
160 {
161  using S = typename BV::S;
162 
163  S r = shape.radius;
164  S n_low_bound = sqrtf(n_faces_for_unit_sphere / 2.0) * r * r;
165  unsigned int ring = ceil(n_low_bound);
166  unsigned int seg = ceil(n_low_bound);
167 
168  generateBVHModel(model, shape, pose, seg, ring);
169 }
170 
171 //==============================================================================
172 template<typename BV>
173 void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring)
174 {
175  using S = typename BV::S;
176 
177  std::vector<Vector3<S>> points;
178  std::vector<Triangle> tri_indices;
179 
180  const S& a = shape.radii[0];
181  const S& b = shape.radii[1];
182  const S& c = shape.radii[2];
183 
184  S phi, phid;
185  const S pi = constants<S>::pi();
186  phid = pi * 2 / seg;
187  phi = 0;
188 
189  S theta, thetad;
190  thetad = pi / (ring + 1);
191  theta = 0;
192 
193  for(unsigned int i = 0; i < ring; ++i)
194  {
195  S theta_ = theta + thetad * (i + 1);
196  for(unsigned int j = 0; j < seg; ++j)
197  {
198  points.emplace_back(a * sin(theta_) * cos(phi + j * phid), b * sin(theta_) * sin(phi + j * phid), c * cos(theta_));
199  }
200  }
201  points.emplace_back(0, 0, c);
202  points.emplace_back(0, 0, -c);
203 
204  for(unsigned int i = 0; i < ring - 1; ++i)
205  {
206  for(unsigned int j = 0; j < seg; ++j)
207  {
208  unsigned int a, b, c, d;
209  a = i * seg + j;
210  b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
211  c = (i + 1) * seg + j;
212  d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
213  tri_indices.emplace_back(a, c, b);
214  tri_indices.emplace_back(b, c, d);
215  }
216  }
217 
218  for(unsigned int j = 0; j < seg; ++j)
219  {
220  unsigned int a, b;
221  a = j;
222  b = (j == seg - 1) ? 0 : (j + 1);
223  tri_indices.emplace_back(ring * seg, a, b);
224 
225  a = (ring - 1) * seg + j;
226  b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
227  tri_indices.emplace_back(a, ring * seg + 1, b);
228  }
229 
230  for(unsigned int i = 0; i < points.size(); ++i)
231  {
232  points[i] = pose * points[i];
233  }
234 
235  model.beginModel();
236  model.addSubModel(points, tri_indices);
237  model.endModel();
238  model.computeLocalAABB();
239 }
240 
241 //==============================================================================
242 template<typename BV>
243 void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_ellipsoid)
244 {
245  using S = typename BV::S;
246 
247  const S p = 1.6075;
248 
249  const S& ap = std::pow(shape.radii[0], p);
250  const S& bp = std::pow(shape.radii[1], p);
251  const S& cp = std::pow(shape.radii[2], p);
252 
253  const S ratio = std::pow((ap * bp + bp * cp + cp * ap) / 3.0, 1.0 / p);
254  const S n_low_bound = std::sqrt(n_faces_for_unit_ellipsoid / 2.0) * ratio;
255 
256  const unsigned int ring = std::ceil(n_low_bound);
257  const unsigned int seg = std::ceil(n_low_bound);
258 
259  generateBVHModel(model, shape, pose, seg, ring);
260 }
261 
262 //==============================================================================
263 template<typename BV>
264 void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num)
265 {
266  using S = typename BV::S;
267 
268  std::vector<Vector3<S>> points;
269  std::vector<Triangle> tri_indices;
270 
271  S r = shape.radius;
272  S h = shape.lz;
273  S phi, phid;
274  const S pi = constants<S>::pi();
275  phid = pi * 2 / tot;
276  phi = 0;
277 
278  S hd = h / h_num;
279 
280  for(unsigned int i = 0; i < tot; ++i)
281  points.emplace_back(r * cos(phi + phid * i), r * sin(phi + phid * i), h / 2);
282 
283  for(unsigned int i = 0; i < h_num - 1; ++i)
284  {
285  for(unsigned int j = 0; j < tot; ++j)
286  {
287  points.emplace_back(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd);
288  }
289  }
290 
291  for(unsigned int i = 0; i < tot; ++i)
292  points.emplace_back(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2);
293 
294  points.emplace_back(0, 0, h / 2);
295  points.emplace_back(0, 0, -h / 2);
296 
297  for(unsigned int i = 0; i < tot; ++i)
298  tri_indices.emplace_back((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1)));
299 
300  for(unsigned int i = 0; i < tot; ++i)
301  tri_indices.emplace_back((h_num + 1) * tot + 1, h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i);
302 
303  for(unsigned int i = 0; i < h_num; ++i)
304  {
305  for(unsigned int j = 0; j < tot; ++j)
306  {
307  int a, b, c, d;
308  a = j;
309  b = (j == tot - 1) ? 0 : (j + 1);
310  c = j + tot;
311  d = (j == tot - 1) ? tot : (j + 1 + tot);
312 
313  int start = i * tot;
314  tri_indices.emplace_back(start + b, start + a, start + c);
315  tri_indices.emplace_back(start + b, start + c, start + d);
316  }
317  }
318 
319  for(unsigned int i = 0; i < points.size(); ++i)
320  {
321  points[i] = pose * points[i];
322  }
323 
324  model.beginModel();
325  model.addSubModel(points, tri_indices);
326  model.endModel();
327  model.computeLocalAABB();
328 }
329 
330 //==============================================================================
331 template<typename BV>
332 void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cylinder)
333 {
334  using S = typename BV::S;
335 
336  S r = shape.radius;
337  S h = shape.lz;
338 
339  const S pi = constants<S>::pi();
340  unsigned int tot = tot_for_unit_cylinder * r;
341  S phid = pi * 2 / tot;
342 
343  S circle_edge = phid * r;
344  unsigned int h_num = ceil(h / circle_edge);
345 
346  generateBVHModel(model, shape, pose, tot, h_num);
347 }
348 
349 
350 //==============================================================================
351 template<typename BV>
352 void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num)
353 {
354  using S = typename BV::S;
355 
356  std::vector<Vector3<S>> points;
357  std::vector<Triangle> tri_indices;
358 
359  S r = shape.radius;
360  S h = shape.lz;
361 
362  S phi, phid;
363  const S pi = constants<S>::pi();
364  phid = pi * 2 / tot;
365  phi = 0;
366 
367  S hd = h / h_num;
368 
369  for(unsigned int i = 0; i < h_num - 1; ++i)
370  {
371  S h_i = h / 2 - (i + 1) * hd;
372  S rh = r * (0.5 - h_i / h);
373  for(unsigned int j = 0; j < tot; ++j)
374  {
375  points.emplace_back(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i);
376  }
377  }
378 
379  for(unsigned int i = 0; i < tot; ++i)
380  points.emplace_back(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2);
381 
382  points.emplace_back(0, 0, h / 2);
383  points.emplace_back(0, 0, -h / 2);
384 
385  for(unsigned int i = 0; i < tot; ++i)
386  tri_indices.emplace_back(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1));
387 
388  for(unsigned int i = 0; i < tot; ++i)
389  tri_indices.emplace_back(h_num * tot + 1, (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)), (h_num - 1) * tot + i);
390 
391  for(unsigned int i = 0; i < h_num - 1; ++i)
392  {
393  for(unsigned int j = 0; j < tot; ++j)
394  {
395  int a, b, c, d;
396  a = j;
397  b = (j == tot - 1) ? 0 : (j + 1);
398  c = j + tot;
399  d = (j == tot - 1) ? tot : (j + 1 + tot);
400 
401  int start = i * tot;
402  tri_indices.emplace_back(start + b, start + a, start + c);
403  tri_indices.emplace_back(start + b, start + c, start + d);
404  }
405  }
406 
407  for(unsigned int i = 0; i < points.size(); ++i)
408  {
409  points[i] = pose * points[i];
410  }
411 
412  model.beginModel();
413  model.addSubModel(points, tri_indices);
414  model.endModel();
415  model.computeLocalAABB();
416 }
417 
418 //==============================================================================
419 template<typename BV>
420 void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cone)
421 {
422  using S = typename BV::S;
423 
424  S r = shape.radius;
425  S h = shape.lz;
426 
427  const S pi = constants<S>::pi();
428  unsigned int tot = tot_for_unit_cone * r;
429  S phid = pi * 2 / tot;
430 
431  S circle_edge = phid * r;
432  unsigned int h_num = ceil(h / circle_edge);
433 
434  generateBVHModel(model, shape, pose, tot, h_num);
435 }
436 
437 } // namespace fcl
438 
439 #endif
fcl::BVHModel::computeLocalAABB
void computeLocalAABB() override
Compute the AABB for the BVH, used for broad-phase collision.
Definition: BVH_model-inl.h:1063
fcl::BVHModel::beginModel
int beginModel(int num_tris=0, int num_vertices=0)
Begin a new BVH model.
Definition: BVH_model-inl.h:205
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::Cylinder::radius
S radius
Radius of the cylinder.
Definition: cylinder.h:58
fcl::generateBVHModel
void generateBVHModel(BVHModel< BV > &model, const Box< typename BV::S > &shape, const Transform3< typename BV::S > &pose)
Generate BVH model from box.
Definition: geometric_shape_to_BVH_model-inl.h:48
fcl::Ellipsoid
Center at zero point ellipsoid.
Definition: ellipsoid.h:48
fcl::Cylinder::lz
S lz
Length along z axis.
Definition: cylinder.h:61
fcl::BVHModel::addSubModel
int addSubModel(const std::vector< Vector3< S >> &ps, const std::vector< Triangle > &ts)
Add a set of triangles in the new BVH model.
Definition: BVH_model-inl.h:374
fcl::Cone::lz
S lz
Length along z axis.
Definition: cone.h:60
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:48
fcl::Cone::radius
S radius
Radius of the cone.
Definition: cone.h:57
fcl::constants::pi
static constexpr S pi()
The mathematical constant pi.
Definition: constants.h:49
fcl::Box::side
Vector3< S > side
box side length
Definition: box.h:64
fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:57
fcl::Cylinder
Center at zero cylinder.
Definition: cylinder.h:48
fcl::Ellipsoid::radii
Vector3< S > radii
Radii of the ellipsoid.
Definition: ellipsoid.h:61
fcl::Cone
Center at zero cone.
Definition: cone.h:48
fcl::BVHModel::endModel
int endModel()
End BVH model construction, will build the bounding volume hierarchy.
Definition: BVH_model-inl.h:437
fcl::Sphere::radius
S radius
Radius of the sphere.
Definition: sphere.h:57
fcl::Sphere
Center at zero point sphere.
Definition: sphere.h:48