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)
224 b = (j == tot - 1) ? 0 : (j + 1);
226 d = (j == tot - 1) ? tot : (j + 1 + 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)
316 b = (j == tot - 1) ? 0 : (j + 1);
318 d = (j == tot - 1) ? tot : (j + 1 + 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);
Vec3f halfSide
box side half-length
Definition: geometric_shapes.h:100
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:211
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: geometric_shapes.h:243
Main namespace.
Definition: AABB.h:43
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.
Definition: geometric_shapes.h:208
double FCL_REAL
Definition: data_types.h:68
Center at zero point, axis aligned box.
Definition: geometric_shapes.h:86
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:255
Cone The base of the cone is at and the top is at .
Definition: geometric_shapes.h:199
FCL_REAL radius
Radius of the sphere.
Definition: geometric_shapes.h:130
FCL_REAL radius
Radius of the cylinder.
Definition: geometric_shapes.h:252
Center at zero point sphere.
Definition: geometric_shapes.h:122
Triangle with 3 indices for points.
Definition: data_types.h:77
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:255
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:73
void computeLocalAABB()
Compute the AABB for the BVH, used for broad-phase collision.
int beginModel(int num_tris=0, int num_vertices=0)
Begin a new BVH model.
void generateBVHModel(BVHModel< BV > &model, const Box &shape, const Transform3f &pose)
Generate BVH model from box.
Definition: geometric_shape_to_BVH_model.h:53
int endModel()
End BVH model construction, will build the bounding volume hierarchy.