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

fcl
Author(s):
autogenerated on Tue Dec 5 2023 03:40:48