39 #ifndef GEOMETRIC_SHAPE_TO_BVH_MODEL_H
40 #define GEOMETRIC_SHAPE_TO_BVH_MODEL_H
44 #include <boost/math/constants/constants.hpp>
58 std::vector<Vec3f> points(8);
59 std::vector<Triangle> tri_indices(12);
60 points[0] =
Vec3f ( a, -b, c);
61 points[1] =
Vec3f ( a, b, c);
62 points[2] =
Vec3f (-a, b, c);
63 points[3] =
Vec3f (-a, -b, c);
64 points[4] =
Vec3f ( a, -b, -c);
65 points[5] =
Vec3f ( a, b, -c);
66 points[6] =
Vec3f (-a, b, -c);
67 points[7] =
Vec3f (-a, -b, -c);
69 tri_indices[ 0].set(0, 4, 1);
70 tri_indices[ 1].set(1, 4, 5);
71 tri_indices[ 2].set(2, 6, 3);
72 tri_indices[ 3].set(3, 6, 7);
73 tri_indices[ 4].set(3, 0, 2);
74 tri_indices[ 5].set(2, 0, 1);
75 tri_indices[ 6].set(6, 5, 7);
76 tri_indices[ 7].set(7, 5, 4);
77 tri_indices[ 8].set(1, 5, 2);
78 tri_indices[ 9].set(2, 5, 6);
79 tri_indices[10].set(3, 7, 0);
80 tri_indices[11].set(0, 7, 4);
82 for(
unsigned int i = 0; i < points.size(); ++i)
84 points[i] = pose.
transform(points[i]).eval();
97 std::vector<Vec3f> points;
98 std::vector<Triangle> tri_indices;
102 const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
107 thetad = pi / (ring + 1);
110 for(
unsigned int i = 0; i < ring; ++i)
112 FCL_REAL theta_ = theta + thetad * (i + 1);
113 for(
unsigned int j = 0; j < seg; ++j)
115 points.push_back(
Vec3f(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), r * cos(theta_)));
118 points.push_back(
Vec3f(0, 0, r));
119 points.push_back(
Vec3f(0, 0, -r));
121 for(
unsigned int i = 0; i < ring - 1; ++i)
123 for(
unsigned int j = 0; j < seg; ++j)
125 unsigned int a, b, c, d;
127 b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
128 c = (i + 1) * seg + j;
129 d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
130 tri_indices.push_back(
Triangle(a, c, b));
131 tri_indices.push_back(
Triangle(b, c, d));
135 for(
unsigned int j = 0; j < seg; ++j)
139 b = (j == seg - 1) ? 0 : (j + 1);
140 tri_indices.push_back(
Triangle(ring * seg, a, b));
142 a = (ring - 1) * seg + j;
143 b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
144 tri_indices.push_back(
Triangle(a, ring * seg + 1, b));
147 for(
unsigned int i = 0; i < points.size(); ++i)
161 template<
typename BV>
166 unsigned int ring = (
unsigned int) ceil(n_low_bound);
167 unsigned int seg = (
unsigned int) ceil(n_low_bound);
174 template<
typename BV>
177 std::vector<Vec3f> points;
178 std::vector<Triangle> tri_indices;
183 const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
189 for(
unsigned int i = 0; i < tot; ++i)
190 points.push_back(
Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h));
192 for(
unsigned int i = 0; i < h_num - 1; ++i)
194 for(
unsigned int j = 0; j < tot; ++j)
196 points.push_back(
Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j), h - (i + 1) * hd));
200 for(
unsigned int i = 0; i < tot; ++i)
201 points.push_back(
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)
208 Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1)));
209 tri_indices.push_back(tmp);
212 for(
unsigned int i = 0; i < tot; ++i)
214 Triangle tmp((h_num + 1) * tot + 1, h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i);
215 tri_indices.push_back(tmp);
218 for(
unsigned int i = 0; i < h_num; ++i)
220 for(
unsigned int j = 0; j < tot; ++j)
222 unsigned int a, b, c, d;
224 b = (j == tot - 1) ? 0 : (j + 1);
226 d = (j == tot - 1) ? tot : (j + 1 + tot);
228 unsigned int start = i * tot;
229 tri_indices.push_back(
Triangle(start + b, start + a, start + c));
230 tri_indices.push_back(
Triangle(start + b, start + c, start + d));
234 for(
unsigned int i = 0; i < points.size(); ++i)
248 template<
typename BV>
254 const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
255 unsigned int tot = (
unsigned int)(tot_for_unit_cylinder * r);
259 unsigned int h_num = (
unsigned int)ceil(h / circle_edge);
266 template<
typename BV>
269 std::vector<Vec3f> points;
270 std::vector<Triangle> tri_indices;
276 const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
282 for(
unsigned int i = 0; i < h_num - 1; ++i)
285 FCL_REAL rh = r * (0.5 - h_i / h / 2);
286 for(
unsigned int j = 0; j < tot; ++j)
288 points.push_back(
Vec3f(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i));
292 for(
unsigned int i = 0; i < tot; ++i)
293 points.push_back(
Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h));
295 points.push_back(
Vec3f(0, 0, h));
296 points.push_back(
Vec3f(0, 0, -h));
298 for(
unsigned int i = 0; i < tot; ++i)
300 Triangle tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1));
301 tri_indices.push_back(tmp);
304 for(
unsigned int i = 0; i < tot; ++i)
306 Triangle tmp(h_num * tot + 1, (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)), (h_num - 1) * tot + i);
307 tri_indices.push_back(tmp);
310 for(
unsigned int i = 0; i < h_num - 1; ++i)
312 for(
unsigned int j = 0; j < tot; ++j)
314 unsigned int a, b, c, d;
316 b = (j == tot - 1) ? 0 : (j + 1);
318 d = (j == tot - 1) ? tot : (j + 1 + tot);
320 unsigned int start = i * tot;
321 tri_indices.push_back(
Triangle(start + b, start + a, start + c));
322 tri_indices.push_back(
Triangle(start + b, start + c, start + d));
326 for(
unsigned int i = 0; i < points.size(); ++i)
340 template<
typename BV>
346 const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
347 unsigned int tot = (
unsigned int)(tot_for_unit_cone * r);
351 unsigned int h_num = (
unsigned int)ceil(h / circle_edge);
int addSubModel(const std::vector< Vec3f > &ps, const std::vector< Triangle > &ts)
Add a set of triangles in the new BVH model.
int endModel()
End BVH model construction, will build the bounding volume hierarchy.
void computeLocalAABB()
Compute the AABB for the BVH, used for broad-phase collision.
int beginModel(unsigned int num_tris=0, unsigned int num_vertices=0)
Begin a new BVH model.
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:278
Center at zero point, axis aligned box.
Definition: geometric_shapes.h:103
Cone The base of the cone is at and the top is at .
Definition: geometric_shapes.h:250
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: geometric_shapes.h:306
Center at zero point sphere.
Definition: geometric_shapes.h:150
Triangle with 3 indices for points.
Definition: data_types.h:77
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:327
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:271
FCL_REAL radius
Radius of the sphere.
Definition: geometric_shapes.h:163
Vec3f halfSide
box side half-length
Definition: geometric_shapes.h:125
FCL_REAL radius
Radius of the cone.
Definition: geometric_shapes.h:265
FCL_REAL radius
Radius of the cylinder.
Definition: geometric_shapes.h:321
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
void generateBVHModel(BVHModel< BV > &model, const Box &shape, const Transform3f &pose)
Generate BVH model from box.
Definition: geometric_shape_to_BVH_model.h:53
double FCL_REAL
Definition: data_types.h:66
Main namespace.
Definition: AABB.h:44