coal/shape/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 COAL_GEOMETRIC_SHAPE_TO_BVH_MODEL_H
39 #define COAL_GEOMETRIC_SHAPE_TO_BVH_MODEL_H
40 
41 #include "coal/shape/geometric_shapes.h"
42 #include "coal/BVH/BVH_model.h"
43 #include <boost/math/constants/constants.hpp>
44 
45 namespace coal {
46 
48 template <typename BV>
49 void generateBVHModel(BVHModel<BV>& model, const Box& shape,
50  const Transform3s& pose) {
51  CoalScalar a = shape.halfSide[0];
52  CoalScalar b = shape.halfSide[1];
53  CoalScalar c = shape.halfSide[2];
54  std::vector<Vec3s> points(8);
55  std::vector<Triangle> tri_indices(12);
56  points[0] = Vec3s(a, -b, c);
57  points[1] = Vec3s(a, b, c);
58  points[2] = Vec3s(-a, b, c);
59  points[3] = Vec3s(-a, -b, c);
60  points[4] = Vec3s(a, -b, -c);
61  points[5] = Vec3s(a, b, -c);
62  points[6] = Vec3s(-a, b, -c);
63  points[7] = Vec3s(-a, -b, -c);
64 
65  tri_indices[0].set(0, 4, 1);
66  tri_indices[1].set(1, 4, 5);
67  tri_indices[2].set(2, 6, 3);
68  tri_indices[3].set(3, 6, 7);
69  tri_indices[4].set(3, 0, 2);
70  tri_indices[5].set(2, 0, 1);
71  tri_indices[6].set(6, 5, 7);
72  tri_indices[7].set(7, 5, 4);
73  tri_indices[8].set(1, 5, 2);
74  tri_indices[9].set(2, 5, 6);
75  tri_indices[10].set(3, 7, 0);
76  tri_indices[11].set(0, 7, 4);
77 
78  for (unsigned int i = 0; i < points.size(); ++i) {
79  points[i] = pose.transform(points[i]).eval();
80  }
81 
82  model.beginModel();
83  model.addSubModel(points, tri_indices);
84  model.endModel();
85  model.computeLocalAABB();
86 }
87 
90 template <typename BV>
91 void generateBVHModel(BVHModel<BV>& model, const Sphere& shape,
92  const Transform3s& pose, unsigned int seg,
93  unsigned int ring) {
94  std::vector<Vec3s> points;
95  std::vector<Triangle> tri_indices;
96 
97  CoalScalar r = shape.radius;
98  CoalScalar phi, phid;
99  const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
100  phid = pi * 2 / seg;
101  phi = 0;
102 
103  CoalScalar theta, thetad;
104  thetad = pi / (ring + 1);
105  theta = 0;
106 
107  for (unsigned int i = 0; i < ring; ++i) {
108  CoalScalar theta_ = theta + thetad * (i + 1);
109  for (unsigned int j = 0; j < seg; ++j) {
110  points.push_back(Vec3s(r * sin(theta_) * cos(phi + j * phid),
111  r * sin(theta_) * sin(phi + j * phid),
112  r * cos(theta_)));
113  }
114  }
115  points.push_back(Vec3s(0, 0, r));
116  points.push_back(Vec3s(0, 0, -r));
117 
118  for (unsigned int i = 0; i < ring - 1; ++i) {
119  for (unsigned int j = 0; j < seg; ++j) {
120  unsigned int a, b, c, d;
121  a = i * seg + j;
122  b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
123  c = (i + 1) * seg + j;
124  d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
125  tri_indices.push_back(Triangle(a, c, b));
126  tri_indices.push_back(Triangle(b, c, d));
127  }
128  }
129 
130  for (unsigned int j = 0; j < seg; ++j) {
131  unsigned int a, b;
132  a = j;
133  b = (j == seg - 1) ? 0 : (j + 1);
134  tri_indices.push_back(Triangle(ring * seg, a, b));
135 
136  a = (ring - 1) * seg + j;
137  b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
138  tri_indices.push_back(Triangle(a, ring * seg + 1, b));
139  }
140 
141  for (unsigned int i = 0; i < points.size(); ++i) {
142  points[i] = pose.transform(points[i]);
143  }
144 
145  model.beginModel();
146  model.addSubModel(points, tri_indices);
147  model.endModel();
148  model.computeLocalAABB();
149 }
150 
156 template <typename BV>
157 void generateBVHModel(BVHModel<BV>& model, const Sphere& shape,
158  const Transform3s& pose,
159  unsigned int n_faces_for_unit_sphere) {
160  CoalScalar r = shape.radius;
161  CoalScalar n_low_bound =
162  std::sqrt((CoalScalar)n_faces_for_unit_sphere / CoalScalar(2.)) * r * r;
163  unsigned int ring = (unsigned int)ceil(n_low_bound);
164  unsigned int seg = (unsigned int)ceil(n_low_bound);
165 
166  generateBVHModel(model, shape, pose, seg, ring);
167 }
168 
171 template <typename BV>
172 void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape,
173  const Transform3s& pose, unsigned int tot,
174  unsigned int h_num) {
175  std::vector<Vec3s> points;
176  std::vector<Triangle> tri_indices;
177 
178  CoalScalar r = shape.radius;
179  CoalScalar h = shape.halfLength;
180  CoalScalar phi, phid;
181  const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
182  phid = pi * 2 / tot;
183  phi = 0;
184 
185  CoalScalar hd = 2 * h / h_num;
186 
187  for (unsigned int i = 0; i < tot; ++i)
188  points.push_back(
189  Vec3s(r * cos(phi + phid * i), r * sin(phi + phid * i), h));
190 
191  for (unsigned int i = 0; i < h_num - 1; ++i) {
192  for (unsigned int j = 0; j < tot; ++j) {
193  points.push_back(Vec3s(r * cos(phi + phid * j), r * sin(phi + phid * j),
194  h - (i + 1) * hd));
195  }
196  }
197 
198  for (unsigned int i = 0; i < tot; ++i)
199  points.push_back(
200  Vec3s(r * cos(phi + phid * i), r * sin(phi + phid * i), -h));
201 
202  points.push_back(Vec3s(0, 0, h));
203  points.push_back(Vec3s(0, 0, -h));
204 
205  for (unsigned int i = 0; i < tot; ++i) {
206  Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1)));
207  tri_indices.push_back(tmp);
208  }
209 
210  for (unsigned int i = 0; i < tot; ++i) {
211  Triangle tmp((h_num + 1) * tot + 1,
212  h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i);
213  tri_indices.push_back(tmp);
214  }
215 
216  for (unsigned int i = 0; i < h_num; ++i) {
217  for (unsigned int j = 0; j < tot; ++j) {
218  unsigned int a, b, c, d;
219  a = j;
220  b = (j == tot - 1) ? 0 : (j + 1);
221  c = j + tot;
222  d = (j == tot - 1) ? tot : (j + 1 + tot);
223 
224  unsigned int start = i * tot;
225  tri_indices.push_back(Triangle(start + b, start + a, start + c));
226  tri_indices.push_back(Triangle(start + b, start + c, start + d));
227  }
228  }
229 
230  for (unsigned int i = 0; i < points.size(); ++i) {
231  points[i] = pose.transform(points[i]);
232  }
233 
234  model.beginModel();
235  model.addSubModel(points, tri_indices);
236  model.endModel();
237  model.computeLocalAABB();
238 }
239 
244 template <typename BV>
245 void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape,
246  const Transform3s& pose,
247  unsigned int tot_for_unit_cylinder) {
248  CoalScalar r = shape.radius;
249  CoalScalar h = 2 * shape.halfLength;
250 
251  const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
252  unsigned int tot = (unsigned int)(tot_for_unit_cylinder * r);
253  CoalScalar phid = pi * 2 / tot;
254 
255  CoalScalar circle_edge = phid * r;
256  unsigned int h_num = (unsigned int)ceil(h / circle_edge);
257 
258  generateBVHModel(model, shape, pose, tot, h_num);
259 }
260 
263 template <typename BV>
264 void generateBVHModel(BVHModel<BV>& model, const Cone& shape,
265  const Transform3s& pose, unsigned int tot,
266  unsigned int h_num) {
267  std::vector<Vec3s> points;
268  std::vector<Triangle> tri_indices;
269 
270  CoalScalar r = shape.radius;
271  CoalScalar h = shape.halfLength;
272 
273  CoalScalar phi, phid;
274  const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
275  phid = pi * 2 / tot;
276  phi = 0;
277 
278  CoalScalar hd = 2 * h / h_num;
279 
280  for (unsigned int i = 0; i < h_num - 1; ++i) {
281  CoalScalar h_i = h - (i + 1) * hd;
282  CoalScalar rh = r * (0.5 - h_i / h / 2);
283  for (unsigned int j = 0; j < tot; ++j) {
284  points.push_back(
285  Vec3s(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i));
286  }
287  }
288 
289  for (unsigned int i = 0; i < tot; ++i)
290  points.push_back(
291  Vec3s(r * cos(phi + phid * i), r * sin(phi + phid * i), -h));
292 
293  points.push_back(Vec3s(0, 0, h));
294  points.push_back(Vec3s(0, 0, -h));
295 
296  for (unsigned int i = 0; i < tot; ++i) {
297  Triangle tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1));
298  tri_indices.push_back(tmp);
299  }
300 
301  for (unsigned int i = 0; i < tot; ++i) {
302  Triangle tmp(h_num * tot + 1,
303  (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)),
304  (h_num - 1) * tot + i);
305  tri_indices.push_back(tmp);
306  }
307 
308  for (unsigned int i = 0; i < h_num - 1; ++i) {
309  for (unsigned int j = 0; j < tot; ++j) {
310  unsigned int a, b, c, d;
311  a = j;
312  b = (j == tot - 1) ? 0 : (j + 1);
313  c = j + tot;
314  d = (j == tot - 1) ? tot : (j + 1 + tot);
315 
316  unsigned int start = i * tot;
317  tri_indices.push_back(Triangle(start + b, start + a, start + c));
318  tri_indices.push_back(Triangle(start + b, start + c, start + d));
319  }
320  }
321 
322  for (unsigned int i = 0; i < points.size(); ++i) {
323  points[i] = pose.transform(points[i]);
324  }
325 
326  model.beginModel();
327  model.addSubModel(points, tri_indices);
328  model.endModel();
329  model.computeLocalAABB();
330 }
331 
336 template <typename BV>
337 void generateBVHModel(BVHModel<BV>& model, const Cone& shape,
338  const Transform3s& pose, unsigned int tot_for_unit_cone) {
339  CoalScalar r = shape.radius;
340  CoalScalar h = 2 * shape.halfLength;
341 
342  const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
343  unsigned int tot = (unsigned int)(tot_for_unit_cone * r);
344  CoalScalar phid = pi * 2 / tot;
345 
346  CoalScalar circle_edge = phid * r;
347  unsigned int h_num = (unsigned int)ceil(h / circle_edge);
348 
349  generateBVHModel(model, shape, pose, tot, h_num);
350 }
351 
352 } // namespace coal
353 
354 #endif
coal::BVHModelBase::beginModel
int beginModel(unsigned int num_tris=0, unsigned int num_vertices=0)
Begin a new BVH model.
Definition: BVH_model.cpp:179
coal::Cylinder::halfLength
CoalScalar halfLength
Half Length along z axis.
Definition: coal/shape/geometric_shapes.h:587
coal::Vec3s
Eigen::Matrix< CoalScalar, 3, 1 > Vec3s
Definition: coal/data_types.h:77
coal::BVHModelBase::endModel
int endModel()
End BVH model construction, will build the bounding volume hierarchy.
Definition: BVH_model.cpp:507
coal::Transform3s::transform
Vec3s transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: coal/math/transform.h:152
coal::Cylinder::radius
CoalScalar radius
Radius of the cylinder.
Definition: coal/shape/geometric_shapes.h:581
coal
Main namespace.
Definition: coal/broadphase/broadphase_bruteforce.h:44
octree.r
r
Definition: octree.py:9
coal::BVHModelBase::addSubModel
int addSubModel(const std::vector< Vec3s > &ps, const std::vector< Triangle > &ts)
Add a set of triangles in the new BVH model.
Definition: BVH_model.cpp:439
a
list a
coal::Box
Center at zero point, axis aligned box.
Definition: coal/shape/geometric_shapes.h:166
coal::Sphere
Center at zero point sphere.
Definition: coal/shape/geometric_shapes.h:240
coal::Transform3s
Simple transform class used locally by InterpMotion.
Definition: coal/math/transform.h:55
BVH_model.h
d
d
c
c
coal::Cone::halfLength
CoalScalar halfLength
Half Length along z axis.
Definition: coal/shape/geometric_shapes.h:486
coal::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: coal/shape/geometric_shapes.h:560
coal::BVHModelBase::computeLocalAABB
void computeLocalAABB()
Compute the AABB for the BVH, used for broad-phase collision.
Definition: BVH_model.cpp:781
generate_distance_plot.b
float b
Definition: generate_distance_plot.py:7
coal::Cone
Cone The base of the cone is at and the top is at .
Definition: coal/shape/geometric_shapes.h:467
coal::Box::halfSide
Vec3s halfSide
box side half-length
Definition: coal/shape/geometric_shapes.h:189
coal::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: coal/BVH/BVH_model.h:314
coal::Sphere::radius
CoalScalar radius
Radius of the sphere.
Definition: coal/shape/geometric_shapes.h:250
coal::Cone::radius
CoalScalar radius
Radius of the cone.
Definition: coal/shape/geometric_shapes.h:480
geometric_shapes.h
pi
constexpr CoalScalar pi
Definition: collision_node_asserts.cpp:10
coal::Triangle
Triangle with 3 indices for points.
Definition: coal/data_types.h:111
coal::CoalScalar
double CoalScalar
Definition: coal/data_types.h:76
coal::generateBVHModel
void generateBVHModel(BVHModel< BV > &model, const Box &shape, const Transform3s &pose)
Generate BVH model from box.
Definition: coal/shape/geometric_shape_to_BVH_model.h:49

hpp-fcl
Author(s):
autogenerated on Sat Nov 23 2024 03:44:58