38 #ifndef GEOMETRIC_SHAPE_TO_BVH_MODEL_H 39 #define GEOMETRIC_SHAPE_TO_BVH_MODEL_H 43 #include <boost/math/constants/constants.hpp> 49 template <
typename BV>
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);
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);
79 for (
unsigned int i = 0; i < points.size(); ++i) {
80 points[i] = pose.
transform(points[i]).eval();
91 template <
typename BV>
95 std::vector<Vec3f> points;
96 std::vector<Triangle> tri_indices;
100 const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
105 thetad = pi / (ring + 1);
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),
116 points.push_back(
Vec3f(0, 0, r));
117 points.push_back(
Vec3f(0, 0, -r));
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;
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));
131 for (
unsigned int j = 0; j < seg; ++j) {
134 b = (j == seg - 1) ? 0 : (j + 1);
135 tri_indices.push_back(
Triangle(ring * seg, a, b));
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));
142 for (
unsigned int i = 0; i < points.size(); ++i) {
157 template <
typename BV>
160 unsigned int n_faces_for_unit_sphere) {
164 unsigned int ring = (
unsigned int)ceil(n_low_bound);
165 unsigned int seg = (
unsigned int)ceil(n_low_bound);
172 template <
typename BV>
175 unsigned int h_num) {
176 std::vector<Vec3f> points;
177 std::vector<Triangle> tri_indices;
182 const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
188 for (
unsigned int i = 0; i < tot; ++i)
190 Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h));
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),
199 for (
unsigned int i = 0; i < tot; ++i)
201 Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), -h));
203 points.push_back(
Vec3f(0, 0, h));
204 points.push_back(
Vec3f(0, 0, -h));
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);
211 for (
unsigned int i = 0; i < tot; ++i) {
213 h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i);
214 tri_indices.push_back(tmp);
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;
221 b = (j == tot - 1) ? 0 : (j + 1);
223 d = (j == tot - 1) ? tot : (j + 1 + tot);
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));
231 for (
unsigned int i = 0; i < points.size(); ++i) {
245 template <
typename BV>
248 unsigned int tot_for_unit_cylinder) {
252 const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
253 unsigned int tot = (
unsigned int)(tot_for_unit_cylinder * r);
257 unsigned int h_num = (
unsigned int)ceil(h / circle_edge);
264 template <
typename BV>
267 unsigned int h_num) {
268 std::vector<Vec3f> points;
269 std::vector<Triangle> tri_indices;
275 const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
281 for (
unsigned int i = 0; i < h_num - 1; ++i) {
283 FCL_REAL rh = r * (0.5 - h_i / h / 2);
284 for (
unsigned int j = 0; j < tot; ++j) {
286 Vec3f(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i));
290 for (
unsigned int i = 0; i < tot; ++i)
292 Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), -h));
294 points.push_back(
Vec3f(0, 0, h));
295 points.push_back(
Vec3f(0, 0, -h));
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);
302 for (
unsigned int i = 0; i < tot; ++i) {
304 (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)),
305 (h_num - 1) * tot + i);
306 tri_indices.push_back(tmp);
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;
313 b = (j == tot - 1) ? 0 : (j + 1);
315 d = (j == tot - 1) ? tot : (j + 1 + tot);
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));
323 for (
unsigned int i = 0; i < points.size(); ++i) {
337 template <
typename BV>
339 const Transform3f& pose,
unsigned int tot_for_unit_cone) {
343 const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
344 unsigned int tot = (
unsigned int)(tot_for_unit_cone * r);
348 unsigned int h_num = (
unsigned int)ceil(h / circle_edge);
Vec3f halfSide
box side half-length
FCL_REAL halfLength
Half Length along z axis.
Cylinder along Z axis. The cylinder is defined at its centroid.
int addSubModel(const std::vector< Vec3f > &ps, const std::vector< Triangle > &ts)
Add a set of triangles in the new BVH model.
FCL_REAL radius
Radius of the cone.
int beginModel(unsigned int num_tris=0, unsigned int num_vertices=0)
Begin a new BVH model.
Center at zero point, axis aligned box.
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Cone The base of the cone is at and the top is at .
FCL_REAL radius
Radius of the sphere.
FCL_REAL radius
Radius of the cylinder.
Center at zero point sphere.
Triangle with 3 indices for points.
FCL_REAL halfLength
Half Length along z axis.
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
void computeLocalAABB()
Compute the AABB for the BVH, used for broad-phase collision.
void generateBVHModel(BVHModel< BV > &model, const Box &shape, const Transform3f &pose)
Generate BVH model from box.
int endModel()
End BVH model construction, will build the bounding volume hierarchy.