38 #ifndef FCL_SHAPE_GEOMETRICSHAPETOBVHMODEL_INL_H
39 #define FCL_SHAPE_GEOMETRICSHAPETOBVHMODEL_INL_H
71 using S =
typename BV::S;
76 std::vector<Vector3<S>> points(8);
77 std::vector<Triangle> tri_indices(12);
78 points[0] << 0.5 * a, -0.5 * b, 0.5 * c;
79 points[1] << 0.5 * a, 0.5 * b, 0.5 * c;
80 points[2] << -0.5 * a, 0.5 * b, 0.5 * c;
81 points[3] << -0.5 * a, -0.5 * b, 0.5 * c;
82 points[4] << 0.5 * a, -0.5 * b, -0.5 * c;
83 points[5] << 0.5 * a, 0.5 * b, -0.5 * c;
84 points[6] << -0.5 * a, 0.5 * b, -0.5 * c;
85 points[7] << -0.5 * a, -0.5 * b, -0.5 * c;
87 tri_indices[0].set(0, 4, 1);
88 tri_indices[1].set(1, 4, 5);
89 tri_indices[2].set(2, 6, 3);
90 tri_indices[3].set(3, 6, 7);
91 tri_indices[4].set(3, 0, 2);
92 tri_indices[5].set(2, 0, 1);
93 tri_indices[6].set(6, 5, 7);
94 tri_indices[7].set(7, 5, 4);
95 tri_indices[8].set(1, 5, 2);
96 tri_indices[9].set(2, 5, 6);
97 tri_indices[10].set(3, 7, 0);
98 tri_indices[11].set(0, 7, 4);
100 for(
unsigned int i = 0; i < points.size(); ++i)
102 points[i] = pose * points[i];
105 return addTriangles(model, points, tri_indices, finalize_model);
110 template<
typename BV>
113 using S =
typename BV::S;
115 std::vector<Vector3<S>> points;
116 std::vector<Triangle> tri_indices;
125 thetad = pi / (ring + 1);
128 for(
unsigned int i = 0; i < ring; ++i)
130 S theta_ = theta + thetad * (i + 1);
131 for(
unsigned int j = 0; j < seg; ++j)
133 points.emplace_back(
r * sin(theta_) * cos(phi + j * phid),
r * sin(theta_) * sin(phi + j * phid),
r * cos(theta_));
136 points.emplace_back(0, 0,
r);
137 points.emplace_back(0, 0, -
r);
139 for(
unsigned int i = 0; i < ring - 1; ++i)
141 for(
unsigned int j = 0; j < seg; ++j)
143 unsigned int a, b, c, d;
145 b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
146 c = (i + 1) * seg + j;
147 d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
148 tri_indices.emplace_back(a, c, b);
149 tri_indices.emplace_back(b, c, d);
153 for(
unsigned int j = 0; j < seg; ++j)
157 b = (j == seg - 1) ? 0 : (j + 1);
158 tri_indices.emplace_back(ring * seg, a, b);
160 a = (ring - 1) * seg + j;
161 b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
162 tri_indices.emplace_back(a, ring * seg + 1, b);
165 for(
unsigned int i = 0; i < points.size(); ++i)
167 points[i] = pose * points[i];
170 return addTriangles(model, points, tri_indices, finalize_model);
174 template<
typename BV>
177 using S =
typename BV::S;
180 S n_low_bound = sqrtf(n_faces_for_unit_sphere / 2.0) *
r *
r;
181 unsigned int ring = ceil(n_low_bound);
182 unsigned int seg = ceil(n_low_bound);
188 template<
typename BV>
191 using S =
typename BV::S;
193 std::vector<Vector3<S>> points;
194 std::vector<Triangle> tri_indices;
196 const S& a = shape.
radii[0];
197 const S& b = shape.
radii[1];
198 const S& c = shape.
radii[2];
206 thetad = pi / (ring + 1);
209 for(
unsigned int i = 0; i < ring; ++i)
211 S theta_ = theta + thetad * (i + 1);
212 for(
unsigned int j = 0; j < seg; ++j)
214 points.emplace_back(a * sin(theta_) * cos(phi + j * phid), b * sin(theta_) * sin(phi + j * phid), c * cos(theta_));
217 points.emplace_back(0, 0, c);
218 points.emplace_back(0, 0, -c);
220 for(
unsigned int i = 0; i < ring - 1; ++i)
222 for(
unsigned int j = 0; j < seg; ++j)
224 unsigned int a, b, c, d;
226 b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
227 c = (i + 1) * seg + j;
228 d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
229 tri_indices.emplace_back(a, c, b);
230 tri_indices.emplace_back(b, c, d);
234 for(
unsigned int j = 0; j < seg; ++j)
238 b = (j == seg - 1) ? 0 : (j + 1);
239 tri_indices.emplace_back(ring * seg, a, b);
241 a = (ring - 1) * seg + j;
242 b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
243 tri_indices.emplace_back(a, ring * seg + 1, b);
246 for(
unsigned int i = 0; i < points.size(); ++i)
248 points[i] = pose * points[i];
251 return addTriangles(model, points, tri_indices, finalize_model);
255 template<
typename BV>
258 using S =
typename BV::S;
262 const S& ap = std::pow(shape.
radii[0], p);
263 const S& bp = std::pow(shape.
radii[1], p);
264 const S& cp = std::pow(shape.
radii[2], p);
266 const S ratio = std::pow((ap * bp + bp * cp + cp * ap) / 3.0, 1.0 / p);
267 const S n_low_bound = std::sqrt(n_faces_for_unit_ellipsoid / 2.0) * ratio;
269 const unsigned int ring = std::ceil(n_low_bound);
270 const unsigned int seg = std::ceil(n_low_bound);
276 template<
typename BV>
279 using S =
typename BV::S;
281 std::vector<Vector3<S>> points;
282 std::vector<Triangle> tri_indices;
288 phid = pi * 2 / circle_split_tot;
293 for(
unsigned int i = 0; i < circle_split_tot; ++i)
294 points.emplace_back(
r * cos(phi + phid * i),
r * sin(phi + phid * i), h / 2);
296 for(
unsigned int i = 0; i < h_num - 1; ++i)
298 for(
unsigned int j = 0; j < circle_split_tot; ++j)
300 points.emplace_back(
r * cos(phi + phid * j),
r * sin(phi + phid * j), h / 2 - (i + 1) * hd);
304 for(
unsigned int i = 0; i < circle_split_tot; ++i)
305 points.emplace_back(
r * cos(phi + phid * i),
r * sin(phi + phid * i), - h / 2);
307 points.emplace_back(0, 0, h / 2);
308 points.emplace_back(0, 0, -h / 2);
310 for(
unsigned int i = 0; i < circle_split_tot; ++i)
311 tri_indices.emplace_back((h_num + 1) * circle_split_tot, i, ((i == circle_split_tot - 1) ? 0 : (i + 1)));
313 for(
unsigned int i = 0; i < circle_split_tot; ++i)
314 tri_indices.emplace_back((h_num + 1) * circle_split_tot + 1, h_num * circle_split_tot + ((i == circle_split_tot - 1) ? 0 : (i + 1)), h_num * circle_split_tot + i);
316 for(
unsigned int i = 0; i < h_num; ++i)
318 for(
unsigned int j = 0; j < circle_split_tot; ++j)
322 b = (j == circle_split_tot - 1) ? 0 : (j + 1);
323 c = j + circle_split_tot;
324 d = (j == circle_split_tot - 1) ? circle_split_tot : (j + 1 + circle_split_tot);
326 int start = i * circle_split_tot;
327 tri_indices.emplace_back(start + b, start + a, start + c);
328 tri_indices.emplace_back(start + b, start + c, start + d);
332 for(
unsigned int i = 0; i < points.size(); ++i)
334 points[i] = pose * points[i];
337 return addTriangles(model, points, tri_indices, finalize_model);
341 template<
typename BV>
344 using S =
typename BV::S;
350 unsigned int circle_split_tot = circle_split_tot_for_unit_cylinder *
r;
351 S phid = pi * 2 / circle_split_tot;
353 S circle_edge = phid *
r;
354 unsigned int h_num = ceil(h / circle_edge);
356 return generateBVHModel(model, shape, pose, circle_split_tot, h_num, finalize_model);
360 template<
typename BV>
363 using S =
typename BV::S;
365 std::vector<Vector3<S>> points;
366 std::vector<Triangle> tri_indices;
373 phid = pi * 2 / circle_split_tot;
378 for(
unsigned int i = 0; i < h_num - 1; ++i)
380 S h_i = h / 2 - (i + 1) * hd;
381 S rh =
r * (0.5 - h_i / h);
382 for(
unsigned int j = 0; j < circle_split_tot; ++j)
384 points.emplace_back(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i);
388 for(
unsigned int i = 0; i < circle_split_tot; ++i)
389 points.emplace_back(
r * cos(phi + phid * i),
r * sin(phi + phid * i), - h / 2);
391 points.emplace_back(0, 0, h / 2);
392 points.emplace_back(0, 0, -h / 2);
394 for(
unsigned int i = 0; i < circle_split_tot; ++i)
395 tri_indices.emplace_back(h_num * circle_split_tot, i, (i == circle_split_tot - 1) ? 0 : (i + 1));
397 for(
unsigned int i = 0; i < circle_split_tot; ++i)
398 tri_indices.emplace_back(h_num * circle_split_tot + 1, (h_num - 1) * circle_split_tot + ((i == circle_split_tot - 1) ? 0 : (i + 1)), (h_num - 1) * circle_split_tot + i);
400 for(
unsigned int i = 0; i < h_num - 1; ++i)
402 for(
unsigned int j = 0; j < circle_split_tot; ++j)
406 b = (j == circle_split_tot - 1) ? 0 : (j + 1);
407 c = j + circle_split_tot;
408 d = (j == circle_split_tot - 1) ? circle_split_tot : (j + 1 + circle_split_tot);
410 int start = i * circle_split_tot;
411 tri_indices.emplace_back(start + b, start + a, start + c);
412 tri_indices.emplace_back(start + b, start + c, start + d);
416 for(
unsigned int i = 0; i < points.size(); ++i)
418 points[i] = pose * points[i];
421 return addTriangles(model, points, tri_indices, finalize_model);
426 template<
typename BV>
429 using S =
typename BV::S;
436 unsigned int circle_split_tot = circle_split_tot_for_unit_cone *
r;
437 S phid = pi * 2 / circle_split_tot;
439 S circle_edge = phid *
r;
440 unsigned int h_num = ceil(h / circle_edge);
442 return generateBVHModel(model, shape, pose, circle_split_tot, h_num, finalize_model);