coal 3.0.1
Coal, The Collision Detection Library. Previously known as HPP-FCL, fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
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
42#include "coal/BVH/BVH_model.h"
43#include <boost/math/constants/constants.hpp>
44
45namespace coal {
46
48template <typename BV>
49void generateBVHModel(BVHModel<BV>& model, const Box& shape,
50 const Transform3s& pose) {
51 Scalar a = shape.halfSide[0];
52 Scalar b = shape.halfSide[1];
53 Scalar c = shape.halfSide[2];
54 std::vector<Vec3s> points(8);
55 std::vector<Triangle32> 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
90template <typename BV>
91void 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<Triangle32> tri_indices;
96
97 Scalar r = shape.radius;
98 Scalar phi, phid;
99 const Scalar pi = boost::math::constants::pi<Scalar>();
100 phid = pi * 2 / Scalar(seg);
101 phi = 0;
102
103 Scalar theta, thetad;
104 thetad = pi / Scalar(ring + 1);
105 theta = 0;
106
107 for (unsigned int i = 0; i < ring; ++i) {
108 Scalar theta_ = theta + thetad * Scalar(i + 1);
109 for (unsigned int j = 0; j < seg; ++j) {
110 points.push_back(Vec3s(r * sin(theta_) * cos(phi + Scalar(j) * phid),
111 r * sin(theta_) * sin(phi + Scalar(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(Triangle32(a, c, b));
126 tri_indices.push_back(Triangle32(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(Triangle32(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(Triangle32(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
156template <typename BV>
157void generateBVHModel(BVHModel<BV>& model, const Sphere& shape,
158 const Transform3s& pose,
159 unsigned int n_faces_for_unit_sphere) {
160 Scalar r = shape.radius;
161 Scalar n_low_bound =
162 std::sqrt((Scalar)n_faces_for_unit_sphere / Scalar(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
171template <typename BV>
172void 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<Triangle32> tri_indices;
177
178 Scalar r = shape.radius;
179 Scalar h = shape.halfLength;
180 Scalar phi, phid;
181 const Scalar pi = boost::math::constants::pi<Scalar>();
182 phid = pi * 2 / Scalar(tot);
183 phi = 0;
184
185 Scalar hd = 2 * h / Scalar(h_num);
186
187 for (unsigned int i = 0; i < tot; ++i)
188 points.push_back(Vec3s(r * cos(phi + phid * Scalar(i)),
189 r * sin(phi + phid * Scalar(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 * Scalar(j)),
194 r * sin(phi + phid * Scalar(j)),
195 h - Scalar(i + 1) * hd));
196 }
197 }
198
199 for (unsigned int i = 0; i < tot; ++i)
200 points.push_back(Vec3s(r * cos(phi + phid * Scalar(i)),
201 r * sin(phi + phid * Scalar(i)), -h));
202
203 points.push_back(Vec3s(0, 0, h));
204 points.push_back(Vec3s(0, 0, -h));
205
206 for (unsigned int i = 0; i < tot; ++i) {
207 Triangle32 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 Triangle32 tmp((h_num + 1) * tot + 1,
213 h_num * tot + ((i == tot - 1) ? 0 : (i + 1)),
214 h_num * tot + i);
215 tri_indices.push_back(tmp);
216 }
217
218 for (unsigned int i = 0; i < h_num; ++i) {
219 for (unsigned int j = 0; j < tot; ++j) {
220 unsigned int a, b, c, d;
221 a = j;
222 b = (j == tot - 1) ? 0 : (j + 1);
223 c = j + tot;
224 d = (j == tot - 1) ? tot : (j + 1 + tot);
225
226 unsigned int start = i * tot;
227 tri_indices.push_back(Triangle32(start + b, start + a, start + c));
228 tri_indices.push_back(Triangle32(start + b, start + c, start + d));
229 }
230 }
231
232 for (unsigned int i = 0; i < points.size(); ++i) {
233 points[i] = pose.transform(points[i]);
234 }
235
236 model.beginModel();
237 model.addSubModel(points, tri_indices);
238 model.endModel();
239 model.computeLocalAABB();
240}
241
246template <typename BV>
247void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape,
248 const Transform3s& pose,
249 unsigned int tot_for_unit_cylinder) {
250 Scalar r = shape.radius;
251 Scalar h = 2 * shape.halfLength;
252
253 const Scalar pi = boost::math::constants::pi<Scalar>();
254 unsigned int tot = (unsigned int)(Scalar(tot_for_unit_cylinder) * r);
255 Scalar phid = pi * 2 / Scalar(tot);
256
257 Scalar circle_edge = phid * r;
258 unsigned int h_num = (unsigned int)ceil(h / circle_edge);
259
260 generateBVHModel(model, shape, pose, tot, h_num);
261}
262
265template <typename BV>
266void generateBVHModel(BVHModel<BV>& model, const Cone& shape,
267 const Transform3s& pose, unsigned int tot,
268 unsigned int h_num) {
269 std::vector<Vec3s> points;
270 std::vector<Triangle32> tri_indices;
271
272 Scalar r = shape.radius;
273 Scalar h = shape.halfLength;
274
275 Scalar phi, phid;
276 const Scalar pi = boost::math::constants::pi<Scalar>();
277 phid = pi * 2 / Scalar(tot);
278 phi = 0;
279
280 Scalar hd = 2 * h / Scalar(h_num);
281
282 for (unsigned int i = 0; i < h_num - 1; ++i) {
283 Scalar h_i = h - Scalar(i + 1) * hd;
284 Scalar rh = r * (Scalar(0.5) - h_i / h / 2);
285 for (unsigned int j = 0; j < tot; ++j) {
286 points.push_back(Vec3s(rh * cos(phi + phid * Scalar(j)),
287 rh * sin(phi + phid * Scalar(j)), h_i));
288 }
289 }
290
291 for (unsigned int i = 0; i < tot; ++i)
292 points.push_back(Vec3s(r * cos(phi + phid * Scalar(i)),
293 r * sin(phi + phid * Scalar(i)), -h));
294
295 points.push_back(Vec3s(0, 0, h));
296 points.push_back(Vec3s(0, 0, -h));
297
298 for (unsigned int i = 0; i < tot; ++i) {
299 Triangle32 tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1));
300 tri_indices.push_back(tmp);
301 }
302
303 for (unsigned int i = 0; i < tot; ++i) {
304 Triangle32 tmp(h_num * tot + 1,
305 (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)),
306 (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 for (unsigned int j = 0; j < tot; ++j) {
312 unsigned int a, b, c, d;
313 a = j;
314 b = (j == tot - 1) ? 0 : (j + 1);
315 c = j + tot;
316 d = (j == tot - 1) ? tot : (j + 1 + tot);
317
318 unsigned int start = i * tot;
319 tri_indices.push_back(Triangle32(start + b, start + a, start + c));
320 tri_indices.push_back(Triangle32(start + b, start + c, start + d));
321 }
322 }
323
324 for (unsigned int i = 0; i < points.size(); ++i) {
325 points[i] = pose.transform(points[i]);
326 }
327
328 model.beginModel();
329 model.addSubModel(points, tri_indices);
330 model.endModel();
331 model.computeLocalAABB();
332}
333
338template <typename BV>
339void generateBVHModel(BVHModel<BV>& model, const Cone& shape,
340 const Transform3s& pose, unsigned int tot_for_unit_cone) {
341 Scalar r = shape.radius;
342 Scalar h = 2 * shape.halfLength;
343
344 const Scalar pi = boost::math::constants::pi<Scalar>();
345 unsigned int tot = (unsigned int)(Scalar(tot_for_unit_cone) * r);
346 Scalar phid = pi * 2 / Scalar(tot);
347
348 Scalar circle_edge = phid * r;
349 unsigned int h_num = (unsigned int)ceil(h / circle_edge);
350
351 generateBVHModel(model, shape, pose, tot, h_num);
352}
353
354} // namespace coal
355
356#endif
void computeLocalAABB()
Compute the AABB for the BVH, used for broad-phase collision.
int addSubModel(const std::vector< Vec3s > &ps, const std::vector< Triangle32 > &ts)
Add a set of triangles in the new BVH model.
int beginModel(unsigned int num_tris=0, unsigned int num_vertices=0)
Begin a new BVH model.
int endModel()
End BVH model construction, will build the bounding volume hierarchy.
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition BVH_model.h:326
Center at zero point, axis aligned box.
Definition geometric_shapes.h:164
Vec3s halfSide
box side half-length
Definition geometric_shapes.h:187
Cone The base of the cone is at and the top is at .
Definition geometric_shapes.h:466
Scalar halfLength
Half Length along z axis.
Definition geometric_shapes.h:485
Scalar radius
Radius of the cone.
Definition geometric_shapes.h:482
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition geometric_shapes.h:556
Scalar radius
Radius of the cylinder.
Definition geometric_shapes.h:580
Scalar halfLength
Half Length along z axis.
Definition geometric_shapes.h:583
Center at zero point sphere.
Definition geometric_shapes.h:238
Scalar radius
Radius of the sphere.
Definition geometric_shapes.h:251
Simple transform class used locally by InterpMotion.
Definition transform.h:55
Vec3s transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition transform.h:152
Triangle with 3 indices for points.
Definition data_types.h:122
Main namespace.
Definition broadphase_bruteforce.h:44
void generateBVHModel(BVHModel< BV > &model, const Box &shape, const Transform3s &pose)
Generate BVH model from box.
Definition geometric_shape_to_BVH_model.h:49
Eigen::Matrix< Scalar, 3, 1 > Vec3s
Definition data_types.h:70
double Scalar
Definition data_types.h:68
TriangleTpl< std::uint32_t > Triangle32
Definition data_types.h:205