geometric_shapes_utility.cpp
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 #include <hpp/fcl/shape/geometric_shapes_utility.h>
39 #include <hpp/fcl/internal/BV_fitter.h>
40 #include <hpp/fcl/internal/tools.h>
41 
42 namespace hpp {
43 namespace fcl {
44 
45 namespace details {
46 
47 std::vector<Vec3f> getBoundVertices(const Box& box, const Transform3f& tf) {
48  std::vector<Vec3f> result(8);
49  FCL_REAL a = box.halfSide[0];
50  FCL_REAL b = box.halfSide[1];
51  FCL_REAL c = box.halfSide[2];
52  result[0] = tf.transform(Vec3f(a, b, c));
53  result[1] = tf.transform(Vec3f(a, b, -c));
54  result[2] = tf.transform(Vec3f(a, -b, c));
55  result[3] = tf.transform(Vec3f(a, -b, -c));
56  result[4] = tf.transform(Vec3f(-a, b, c));
57  result[5] = tf.transform(Vec3f(-a, b, -c));
58  result[6] = tf.transform(Vec3f(-a, -b, c));
59  result[7] = tf.transform(Vec3f(-a, -b, -c));
60 
61  return result;
62 }
63 
64 // we use icosahedron to bound the sphere
65 std::vector<Vec3f> getBoundVertices(const Sphere& sphere,
66  const Transform3f& tf) {
67  std::vector<Vec3f> result(12);
68  const FCL_REAL m = (1 + sqrt(5.0)) / 2.0;
69  FCL_REAL edge_size = sphere.radius * 6 / (sqrt(27.0) + sqrt(15.0));
70 
71  FCL_REAL a = edge_size;
72  FCL_REAL b = m * edge_size;
73  result[0] = tf.transform(Vec3f(0, a, b));
74  result[1] = tf.transform(Vec3f(0, -a, b));
75  result[2] = tf.transform(Vec3f(0, a, -b));
76  result[3] = tf.transform(Vec3f(0, -a, -b));
77  result[4] = tf.transform(Vec3f(a, b, 0));
78  result[5] = tf.transform(Vec3f(-a, b, 0));
79  result[6] = tf.transform(Vec3f(a, -b, 0));
80  result[7] = tf.transform(Vec3f(-a, -b, 0));
81  result[8] = tf.transform(Vec3f(b, 0, a));
82  result[9] = tf.transform(Vec3f(b, 0, -a));
83  result[10] = tf.transform(Vec3f(-b, 0, a));
84  result[11] = tf.transform(Vec3f(-b, 0, -a));
85 
86  return result;
87 }
88 
89 // we use scaled icosahedron to bound the ellipsoid
90 std::vector<Vec3f> getBoundVertices(const Ellipsoid& ellipsoid,
91  const Transform3f& tf) {
92  std::vector<Vec3f> result(12);
93  const FCL_REAL phi = (1 + sqrt(5.0)) / 2.0;
94 
95  const FCL_REAL a = sqrt(3.0) / (phi * phi);
96  const FCL_REAL b = phi * a;
97 
98  const FCL_REAL& A = ellipsoid.radii[0];
99  const FCL_REAL& B = ellipsoid.radii[1];
100  const FCL_REAL& C = ellipsoid.radii[2];
101 
102  FCL_REAL Aa = A * a;
103  FCL_REAL Ab = A * b;
104  FCL_REAL Ba = B * a;
105  FCL_REAL Bb = B * b;
106  FCL_REAL Ca = C * a;
107  FCL_REAL Cb = C * b;
108  result[0] = tf.transform(Vec3f(0, Ba, Cb));
109  result[1] = tf.transform(Vec3f(0, -Ba, Cb));
110  result[2] = tf.transform(Vec3f(0, Ba, -Cb));
111  result[3] = tf.transform(Vec3f(0, -Ba, -Cb));
112  result[4] = tf.transform(Vec3f(Aa, Bb, 0));
113  result[5] = tf.transform(Vec3f(-Aa, Bb, 0));
114  result[6] = tf.transform(Vec3f(Aa, -Bb, 0));
115  result[7] = tf.transform(Vec3f(-Aa, -Bb, 0));
116  result[8] = tf.transform(Vec3f(Ab, 0, Ca));
117  result[9] = tf.transform(Vec3f(Ab, 0, -Ca));
118  result[10] = tf.transform(Vec3f(-Ab, 0, Ca));
119  result[11] = tf.transform(Vec3f(-Ab, 0, -Ca));
120 
121  return result;
122 }
123 
124 std::vector<Vec3f> getBoundVertices(const Capsule& capsule,
125  const Transform3f& tf) {
126  std::vector<Vec3f> result(36);
127  const FCL_REAL m = (1 + sqrt(5.0)) / 2.0;
128 
129  FCL_REAL hl = capsule.halfLength;
130  FCL_REAL edge_size = capsule.radius * 6 / (sqrt(27.0) + sqrt(15.0));
131  FCL_REAL a = edge_size;
132  FCL_REAL b = m * edge_size;
133  FCL_REAL r2 = capsule.radius * 2 / sqrt(3.0);
134 
135  result[0] = tf.transform(Vec3f(0, a, b + hl));
136  result[1] = tf.transform(Vec3f(0, -a, b + hl));
137  result[2] = tf.transform(Vec3f(0, a, -b + hl));
138  result[3] = tf.transform(Vec3f(0, -a, -b + hl));
139  result[4] = tf.transform(Vec3f(a, b, hl));
140  result[5] = tf.transform(Vec3f(-a, b, hl));
141  result[6] = tf.transform(Vec3f(a, -b, hl));
142  result[7] = tf.transform(Vec3f(-a, -b, hl));
143  result[8] = tf.transform(Vec3f(b, 0, a + hl));
144  result[9] = tf.transform(Vec3f(b, 0, -a + hl));
145  result[10] = tf.transform(Vec3f(-b, 0, a + hl));
146  result[11] = tf.transform(Vec3f(-b, 0, -a + hl));
147 
148  result[12] = tf.transform(Vec3f(0, a, b - hl));
149  result[13] = tf.transform(Vec3f(0, -a, b - hl));
150  result[14] = tf.transform(Vec3f(0, a, -b - hl));
151  result[15] = tf.transform(Vec3f(0, -a, -b - hl));
152  result[16] = tf.transform(Vec3f(a, b, -hl));
153  result[17] = tf.transform(Vec3f(-a, b, -hl));
154  result[18] = tf.transform(Vec3f(a, -b, -hl));
155  result[19] = tf.transform(Vec3f(-a, -b, -hl));
156  result[20] = tf.transform(Vec3f(b, 0, a - hl));
157  result[21] = tf.transform(Vec3f(b, 0, -a - hl));
158  result[22] = tf.transform(Vec3f(-b, 0, a - hl));
159  result[23] = tf.transform(Vec3f(-b, 0, -a - hl));
160 
161  FCL_REAL c = 0.5 * r2;
162  FCL_REAL d = capsule.radius;
163  result[24] = tf.transform(Vec3f(r2, 0, hl));
164  result[25] = tf.transform(Vec3f(c, d, hl));
165  result[26] = tf.transform(Vec3f(-c, d, hl));
166  result[27] = tf.transform(Vec3f(-r2, 0, hl));
167  result[28] = tf.transform(Vec3f(-c, -d, hl));
168  result[29] = tf.transform(Vec3f(c, -d, hl));
169 
170  result[30] = tf.transform(Vec3f(r2, 0, -hl));
171  result[31] = tf.transform(Vec3f(c, d, -hl));
172  result[32] = tf.transform(Vec3f(-c, d, -hl));
173  result[33] = tf.transform(Vec3f(-r2, 0, -hl));
174  result[34] = tf.transform(Vec3f(-c, -d, -hl));
175  result[35] = tf.transform(Vec3f(c, -d, -hl));
176 
177  return result;
178 }
179 
180 std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf) {
181  std::vector<Vec3f> result(7);
182 
183  FCL_REAL hl = cone.halfLength;
184  FCL_REAL r2 = cone.radius * 2 / sqrt(3.0);
185  FCL_REAL a = 0.5 * r2;
186  FCL_REAL b = cone.radius;
187 
188  result[0] = tf.transform(Vec3f(r2, 0, -hl));
189  result[1] = tf.transform(Vec3f(a, b, -hl));
190  result[2] = tf.transform(Vec3f(-a, b, -hl));
191  result[3] = tf.transform(Vec3f(-r2, 0, -hl));
192  result[4] = tf.transform(Vec3f(-a, -b, -hl));
193  result[5] = tf.transform(Vec3f(a, -b, -hl));
194 
195  result[6] = tf.transform(Vec3f(0, 0, hl));
196 
197  return result;
198 }
199 
200 std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder,
201  const Transform3f& tf) {
202  std::vector<Vec3f> result(12);
203 
204  FCL_REAL hl = cylinder.halfLength;
205  FCL_REAL r2 = cylinder.radius * 2 / sqrt(3.0);
206  FCL_REAL a = 0.5 * r2;
207  FCL_REAL b = cylinder.radius;
208 
209  result[0] = tf.transform(Vec3f(r2, 0, -hl));
210  result[1] = tf.transform(Vec3f(a, b, -hl));
211  result[2] = tf.transform(Vec3f(-a, b, -hl));
212  result[3] = tf.transform(Vec3f(-r2, 0, -hl));
213  result[4] = tf.transform(Vec3f(-a, -b, -hl));
214  result[5] = tf.transform(Vec3f(a, -b, -hl));
215 
216  result[6] = tf.transform(Vec3f(r2, 0, hl));
217  result[7] = tf.transform(Vec3f(a, b, hl));
218  result[8] = tf.transform(Vec3f(-a, b, hl));
219  result[9] = tf.transform(Vec3f(-r2, 0, hl));
220  result[10] = tf.transform(Vec3f(-a, -b, hl));
221  result[11] = tf.transform(Vec3f(a, -b, hl));
222 
223  return result;
224 }
225 
226 std::vector<Vec3f> getBoundVertices(const ConvexBase& convex,
227  const Transform3f& tf) {
228  std::vector<Vec3f> result(convex.num_points);
229  for (std::size_t i = 0; i < convex.num_points; ++i) {
230  result[i] = tf.transform(convex.points[i]);
231  }
232 
233  return result;
234 }
235 
236 std::vector<Vec3f> getBoundVertices(const TriangleP& triangle,
237  const Transform3f& tf) {
238  std::vector<Vec3f> result(3);
239  result[0] = tf.transform(triangle.a);
240  result[1] = tf.transform(triangle.b);
241  result[2] = tf.transform(triangle.c);
242 
243  return result;
244 }
245 
246 } // namespace details
247 
248 Halfspace transform(const Halfspace& a, const Transform3f& tf) {
254 
255  Vec3f n = tf.getRotation() * a.n;
256  FCL_REAL d = a.d + n.dot(tf.getTranslation());
257 
258  return Halfspace(n, d);
259 }
260 
261 Plane transform(const Plane& a, const Transform3f& tf) {
267 
268  Vec3f n = tf.getRotation() * a.n;
269  FCL_REAL d = a.d + n.dot(tf.getTranslation());
270 
271  return Plane(n, d);
272 }
273 
274 template <>
275 void computeBV<AABB, Box>(const Box& s, const Transform3f& tf, AABB& bv) {
276  const Matrix3f& R = tf.getRotation();
277  const Vec3f& T = tf.getTranslation();
278 
279  Vec3f v_delta(R.cwiseAbs() * s.halfSide);
280  bv.max_ = T + v_delta;
281  bv.min_ = T - v_delta;
282 }
283 
284 template <>
285 void computeBV<AABB, Sphere>(const Sphere& s, const Transform3f& tf, AABB& bv) {
286  const Vec3f& T = tf.getTranslation();
287 
288  Vec3f v_delta(Vec3f::Constant(s.radius));
289  bv.max_ = T + v_delta;
290  bv.min_ = T - v_delta;
291 }
292 
293 template <>
294 void computeBV<AABB, Ellipsoid>(const Ellipsoid& e, const Transform3f& tf,
295  AABB& bv) {
296  const Matrix3f& R = tf.getRotation();
297  const Vec3f& T = tf.getTranslation();
298 
299  Vec3f v_delta = R * e.radii;
300  bv.max_ = T + v_delta;
301  bv.min_ = T - v_delta;
302 }
303 
304 template <>
305 void computeBV<AABB, Capsule>(const Capsule& s, const Transform3f& tf,
306  AABB& bv) {
307  const Matrix3f& R = tf.getRotation();
308  const Vec3f& T = tf.getTranslation();
309 
310  Vec3f v_delta(R.col(2).cwiseAbs() * s.halfLength + Vec3f::Constant(s.radius));
311  bv.max_ = T + v_delta;
312  bv.min_ = T - v_delta;
313 }
314 
315 template <>
316 void computeBV<AABB, Cone>(const Cone& s, const Transform3f& tf, AABB& bv) {
317  const Matrix3f& R = tf.getRotation();
318  const Vec3f& T = tf.getTranslation();
319 
320  FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) +
321  fabs(R(0, 2) * s.halfLength);
322  FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) +
323  fabs(R(1, 2) * s.halfLength);
324  FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) +
325  fabs(R(2, 2) * s.halfLength);
326 
327  Vec3f v_delta(x_range, y_range, z_range);
328  bv.max_ = T + v_delta;
329  bv.min_ = T - v_delta;
330 }
331 
332 template <>
333 void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3f& tf,
334  AABB& bv) {
335  const Matrix3f& R = tf.getRotation();
336  const Vec3f& T = tf.getTranslation();
337 
338  FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) +
339  fabs(R(0, 2) * s.halfLength);
340  FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) +
341  fabs(R(1, 2) * s.halfLength);
342  FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) +
343  fabs(R(2, 2) * s.halfLength);
344 
345  Vec3f v_delta(x_range, y_range, z_range);
346  bv.max_ = T + v_delta;
347  bv.min_ = T - v_delta;
348 }
349 
350 template <>
351 void computeBV<AABB, ConvexBase>(const ConvexBase& s, const Transform3f& tf,
352  AABB& bv) {
353  const Matrix3f& R = tf.getRotation();
354  const Vec3f& T = tf.getTranslation();
355 
356  AABB bv_;
357  for (std::size_t i = 0; i < s.num_points; ++i) {
358  Vec3f new_p = R * s.points[i] + T;
359  bv_ += new_p;
360  }
361 
362  bv = bv_;
363 }
364 
365 template <>
366 void computeBV<AABB, TriangleP>(const TriangleP& s, const Transform3f& tf,
367  AABB& bv) {
368  bv = AABB(tf.transform(s.a), tf.transform(s.b), tf.transform(s.c));
369 }
370 
371 template <>
372 void computeBV<AABB, Halfspace>(const Halfspace& s, const Transform3f& tf,
373  AABB& bv) {
374  Halfspace new_s = transform(s, tf);
375  const Vec3f& n = new_s.n;
376  const FCL_REAL& d = new_s.d;
377 
378  AABB bv_;
379  bv_.min_ = Vec3f::Constant(-(std::numeric_limits<FCL_REAL>::max)());
380  bv_.max_ = Vec3f::Constant((std::numeric_limits<FCL_REAL>::max)());
381  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
382  // normal aligned with x axis
383  if (n[0] < 0)
384  bv_.min_[0] = -d;
385  else if (n[0] > 0)
386  bv_.max_[0] = d;
387  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
388  // normal aligned with y axis
389  if (n[1] < 0)
390  bv_.min_[1] = -d;
391  else if (n[1] > 0)
392  bv_.max_[1] = d;
393  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
394  // normal aligned with z axis
395  if (n[2] < 0)
396  bv_.min_[2] = -d;
397  else if (n[2] > 0)
398  bv_.max_[2] = d;
399  }
400 
401  bv = bv_;
402 }
403 
404 template <>
405 void computeBV<AABB, Plane>(const Plane& s, const Transform3f& tf, AABB& bv) {
406  Plane new_s = transform(s, tf);
407  const Vec3f& n = new_s.n;
408  const FCL_REAL& d = new_s.d;
409 
410  AABB bv_;
411  bv_.min_ = Vec3f::Constant(-(std::numeric_limits<FCL_REAL>::max)());
412  bv_.max_ = Vec3f::Constant((std::numeric_limits<FCL_REAL>::max)());
413  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
414  // normal aligned with x axis
415  if (n[0] < 0) {
416  bv_.min_[0] = bv_.max_[0] = -d;
417  } else if (n[0] > 0) {
418  bv_.min_[0] = bv_.max_[0] = d;
419  }
420  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
421  // normal aligned with y axis
422  if (n[1] < 0) {
423  bv_.min_[1] = bv_.max_[1] = -d;
424  } else if (n[1] > 0) {
425  bv_.min_[1] = bv_.max_[1] = d;
426  }
427  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
428  // normal aligned with z axis
429  if (n[2] < 0) {
430  bv_.min_[2] = bv_.max_[2] = -d;
431  } else if (n[2] > 0) {
432  bv_.min_[2] = bv_.max_[2] = d;
433  }
434  }
435 
436  bv = bv_;
437 }
438 
439 template <>
440 void computeBV<OBB, Box>(const Box& s, const Transform3f& tf, OBB& bv) {
441  const Matrix3f& R = tf.getRotation();
442  const Vec3f& T = tf.getTranslation();
443 
444  bv.To = T;
445  bv.axes = R;
446  bv.extent = s.halfSide;
447 }
448 
449 template <>
450 void computeBV<OBB, Sphere>(const Sphere& s, const Transform3f& tf, OBB& bv) {
451  const Vec3f& T = tf.getTranslation();
452 
453  bv.To.noalias() = T;
454  bv.axes.setIdentity();
455  bv.extent.setConstant(s.radius);
456 }
457 
458 template <>
459 void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv) {
460  const Matrix3f& R = tf.getRotation();
461  const Vec3f& T = tf.getTranslation();
462 
463  bv.To.noalias() = T;
464  bv.axes.noalias() = R;
465  bv.extent << s.radius, s.radius, s.halfLength + s.radius;
466 }
467 
468 template <>
469 void computeBV<OBB, Cone>(const Cone& s, const Transform3f& tf, OBB& bv) {
470  const Matrix3f& R = tf.getRotation();
471  const Vec3f& T = tf.getTranslation();
472 
473  bv.To.noalias() = T;
474  bv.axes.noalias() = R;
475  bv.extent << s.radius, s.radius, s.halfLength;
476 }
477 
478 template <>
479 void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3f& tf,
480  OBB& bv) {
481  const Matrix3f& R = tf.getRotation();
482  const Vec3f& T = tf.getTranslation();
483 
484  bv.To.noalias() = T;
485  bv.axes.noalias() = R;
486  bv.extent << s.radius, s.radius, s.halfLength;
487 }
488 
489 template <>
490 void computeBV<OBB, ConvexBase>(const ConvexBase& s, const Transform3f& tf,
491  OBB& bv) {
492  const Matrix3f& R = tf.getRotation();
493  const Vec3f& T = tf.getTranslation();
494 
495  fit(s.points, s.num_points, bv);
496 
497  bv.axes.applyOnTheLeft(R);
498 
499  bv.To = R * bv.To + T;
500 }
501 
502 template <>
503 void computeBV<OBB, Halfspace>(const Halfspace&, const Transform3f&, OBB& bv) {
505  bv.axes.setIdentity();
506  bv.To.setZero();
507  bv.extent.setConstant(((std::numeric_limits<FCL_REAL>::max)()));
508 }
509 
510 template <>
511 void computeBV<RSS, Halfspace>(const Halfspace&, const Transform3f&, RSS& bv) {
513  bv.axes.setIdentity();
514  bv.Tr.setZero();
515  bv.length[0] = bv.length[1] = bv.radius =
516  (std::numeric_limits<FCL_REAL>::max)();
517 }
518 
519 template <>
520 void computeBV<OBBRSS, Halfspace>(const Halfspace& s, const Transform3f& tf,
521  OBBRSS& bv) {
522  computeBV<OBB, Halfspace>(s, tf, bv.obb);
523  computeBV<RSS, Halfspace>(s, tf, bv.rss);
524 }
525 
526 template <>
527 void computeBV<kIOS, Halfspace>(const Halfspace& s, const Transform3f& tf,
528  kIOS& bv) {
529  bv.num_spheres = 1;
530  computeBV<OBB, Halfspace>(s, tf, bv.obb);
531  bv.spheres[0].o = Vec3f();
532  bv.spheres[0].r = (std::numeric_limits<FCL_REAL>::max)();
533 }
534 
535 template <>
536 void computeBV<KDOP<16>, Halfspace>(const Halfspace& s, const Transform3f& tf,
537  KDOP<16>& bv) {
538  Halfspace new_s = transform(s, tf);
539  const Vec3f& n = new_s.n;
540  const FCL_REAL& d = new_s.d;
541 
542  const short D = 8;
543  for (short i = 0; i < D; ++i)
544  bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)();
545  for (short i = D; i < 2 * D; ++i)
546  bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
547 
548  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
549  if (n[0] > 0)
550  bv.dist(D) = d;
551  else
552  bv.dist(0) = -d;
553  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
554  if (n[1] > 0)
555  bv.dist(D + 1) = d;
556  else
557  bv.dist(1) = -d;
558  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
559  if (n[2] > 0)
560  bv.dist(D + 2) = d;
561  else
562  bv.dist(2) = -d;
563  } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) {
564  if (n[0] > 0)
565  bv.dist(D + 3) = n[0] * d * 2;
566  else
567  bv.dist(3) = n[0] * d * 2;
568  } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) {
569  if (n[1] > 0)
570  bv.dist(D + 4) = n[0] * d * 2;
571  else
572  bv.dist(4) = n[0] * d * 2;
573  } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) {
574  if (n[1] > 0)
575  bv.dist(D + 5) = n[1] * d * 2;
576  else
577  bv.dist(5) = n[1] * d * 2;
578  } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
579  if (n[0] > 0)
580  bv.dist(D + 6) = n[0] * d * 2;
581  else
582  bv.dist(6) = n[0] * d * 2;
583  } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
584  if (n[0] > 0)
585  bv.dist(D + 7) = n[0] * d * 2;
586  else
587  bv.dist(7) = n[0] * d * 2;
588  }
589 }
590 
591 template <>
592 void computeBV<KDOP<18>, Halfspace>(const Halfspace& s, const Transform3f& tf,
593  KDOP<18>& bv) {
594  Halfspace new_s = transform(s, tf);
595  const Vec3f& n = new_s.n;
596  const FCL_REAL& d = new_s.d;
597 
598  const short D = 9;
599 
600  for (short i = 0; i < D; ++i)
601  bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)();
602  for (short i = D; i < 2 * D; ++i)
603  bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
604 
605  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
606  if (n[0] > 0)
607  bv.dist(D) = d;
608  else
609  bv.dist(0) = -d;
610  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
611  if (n[1] > 0)
612  bv.dist(D + 1) = d;
613  else
614  bv.dist(1) = -d;
615  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
616  if (n[2] > 0)
617  bv.dist(D + 2) = d;
618  else
619  bv.dist(2) = -d;
620  } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) {
621  if (n[0] > 0)
622  bv.dist(D + 3) = n[0] * d * 2;
623  else
624  bv.dist(3) = n[0] * d * 2;
625  } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) {
626  if (n[1] > 0)
627  bv.dist(D + 4) = n[0] * d * 2;
628  else
629  bv.dist(4) = n[0] * d * 2;
630  } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) {
631  if (n[1] > 0)
632  bv.dist(D + 5) = n[1] * d * 2;
633  else
634  bv.dist(5) = n[1] * d * 2;
635  } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
636  if (n[0] > 0)
637  bv.dist(D + 6) = n[0] * d * 2;
638  else
639  bv.dist(6) = n[0] * d * 2;
640  } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
641  if (n[0] > 0)
642  bv.dist(D + 7) = n[0] * d * 2;
643  else
644  bv.dist(7) = n[0] * d * 2;
645  } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) {
646  if (n[1] > 0)
647  bv.dist(D + 8) = n[1] * d * 2;
648  else
649  bv.dist(8) = n[1] * d * 2;
650  }
651 }
652 
653 template <>
654 void computeBV<KDOP<24>, Halfspace>(const Halfspace& s, const Transform3f& tf,
655  KDOP<24>& bv) {
656  Halfspace new_s = transform(s, tf);
657  const Vec3f& n = new_s.n;
658  const FCL_REAL& d = new_s.d;
659 
660  const short D = 12;
661 
662  for (short i = 0; i < D; ++i)
663  bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)();
664  for (short i = D; i < 2 * D; ++i)
665  bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
666 
667  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
668  if (n[0] > 0)
669  bv.dist(D) = d;
670  else
671  bv.dist(0) = -d;
672  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
673  if (n[1] > 0)
674  bv.dist(D + 1) = d;
675  else
676  bv.dist(1) = -d;
677  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
678  if (n[2] > 0)
679  bv.dist(D + 2) = d;
680  else
681  bv.dist(2) = -d;
682  } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) {
683  if (n[0] > 0)
684  bv.dist(D + 3) = n[0] * d * 2;
685  else
686  bv.dist(3) = n[0] * d * 2;
687  } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) {
688  if (n[1] > 0)
689  bv.dist(D + 4) = n[0] * d * 2;
690  else
691  bv.dist(4) = n[0] * d * 2;
692  } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) {
693  if (n[1] > 0)
694  bv.dist(D + 5) = n[1] * d * 2;
695  else
696  bv.dist(5) = n[1] * d * 2;
697  } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
698  if (n[0] > 0)
699  bv.dist(D + 6) = n[0] * d * 2;
700  else
701  bv.dist(6) = n[0] * d * 2;
702  } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
703  if (n[0] > 0)
704  bv.dist(D + 7) = n[0] * d * 2;
705  else
706  bv.dist(7) = n[0] * d * 2;
707  } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) {
708  if (n[1] > 0)
709  bv.dist(D + 8) = n[1] * d * 2;
710  else
711  bv.dist(8) = n[1] * d * 2;
712  } else if (n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
713  if (n[0] > 0)
714  bv.dist(D + 9) = n[0] * d * 3;
715  else
716  bv.dist(9) = n[0] * d * 3;
717  } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) {
718  if (n[0] > 0)
719  bv.dist(D + 10) = n[0] * d * 3;
720  else
721  bv.dist(10) = n[0] * d * 3;
722  } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
723  if (n[1] > 0)
724  bv.dist(D + 11) = n[1] * d * 3;
725  else
726  bv.dist(11) = n[1] * d * 3;
727  }
728 }
729 
730 template <>
731 void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv) {
732  Vec3f n = tf.getRotation() * s.n;
733  generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2));
734  bv.axes.col(0).noalias() = n;
735 
736  bv.extent << 0, (std::numeric_limits<FCL_REAL>::max)(),
737  (std::numeric_limits<FCL_REAL>::max)();
738 
739  Vec3f p = s.n * s.d;
740  bv.To =
741  tf.transform(p);
742 }
743 
744 template <>
745 void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv) {
746  Vec3f n = tf.getRotation() * s.n;
747 
748  generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2));
749  bv.axes.col(0).noalias() = n;
750 
751  bv.length[0] = (std::numeric_limits<FCL_REAL>::max)();
752  bv.length[1] = (std::numeric_limits<FCL_REAL>::max)();
753 
754  bv.radius = 0;
755 
756  Vec3f p = s.n * s.d;
757  bv.Tr = tf.transform(p);
758 }
759 
760 template <>
761 void computeBV<OBBRSS, Plane>(const Plane& s, const Transform3f& tf,
762  OBBRSS& bv) {
763  computeBV<OBB, Plane>(s, tf, bv.obb);
764  computeBV<RSS, Plane>(s, tf, bv.rss);
765 }
766 
767 template <>
768 void computeBV<kIOS, Plane>(const Plane& s, const Transform3f& tf, kIOS& bv) {
769  bv.num_spheres = 1;
770  computeBV<OBB, Plane>(s, tf, bv.obb);
771  bv.spheres[0].o = Vec3f();
772  bv.spheres[0].r = (std::numeric_limits<FCL_REAL>::max)();
773 }
774 
775 template <>
776 void computeBV<KDOP<16>, Plane>(const Plane& s, const Transform3f& tf,
777  KDOP<16>& bv) {
778  Plane new_s = transform(s, tf);
779  const Vec3f& n = new_s.n;
780  const FCL_REAL& d = new_s.d;
781 
782  const short D = 8;
783 
784  for (short i = 0; i < D; ++i)
785  bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)();
786  for (short i = D; i < 2 * D; ++i)
787  bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
788 
789  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
790  if (n[0] > 0)
791  bv.dist(0) = bv.dist(D) = d;
792  else
793  bv.dist(0) = bv.dist(D) = -d;
794  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
795  if (n[1] > 0)
796  bv.dist(1) = bv.dist(D + 1) = d;
797  else
798  bv.dist(1) = bv.dist(D + 1) = -d;
799  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
800  if (n[2] > 0)
801  bv.dist(2) = bv.dist(D + 2) = d;
802  else
803  bv.dist(2) = bv.dist(D + 2) = -d;
804  } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) {
805  bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
806  } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) {
807  bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
808  } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) {
809  bv.dist(6) = bv.dist(D + 5) = n[1] * d * 2;
810  } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
811  bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
812  } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
813  bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
814  }
815 }
816 
817 template <>
818 void computeBV<KDOP<18>, Plane>(const Plane& s, const Transform3f& tf,
819  KDOP<18>& bv) {
820  Plane new_s = transform(s, tf);
821  const Vec3f& n = new_s.n;
822  const FCL_REAL& d = new_s.d;
823 
824  const short D = 9;
825 
826  for (short i = 0; i < D; ++i)
827  bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)();
828  for (short i = D; i < 2 * D; ++i)
829  bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
830 
831  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
832  if (n[0] > 0)
833  bv.dist(0) = bv.dist(D) = d;
834  else
835  bv.dist(0) = bv.dist(D) = -d;
836  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
837  if (n[1] > 0)
838  bv.dist(1) = bv.dist(D + 1) = d;
839  else
840  bv.dist(1) = bv.dist(D + 1) = -d;
841  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
842  if (n[2] > 0)
843  bv.dist(2) = bv.dist(D + 2) = d;
844  else
845  bv.dist(2) = bv.dist(D + 2) = -d;
846  } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) {
847  bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
848  } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) {
849  bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
850  } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) {
851  bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2;
852  } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
853  bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
854  } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
855  bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
856  } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) {
857  bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2;
858  }
859 }
860 
861 template <>
862 void computeBV<KDOP<24>, Plane>(const Plane& s, const Transform3f& tf,
863  KDOP<24>& bv) {
864  Plane new_s = transform(s, tf);
865  const Vec3f& n = new_s.n;
866  const FCL_REAL& d = new_s.d;
867 
868  const short D = 12;
869 
870  for (short i = 0; i < D; ++i)
871  bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)();
872  for (short i = D; i < 2 * D; ++i)
873  bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
874 
875  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
876  if (n[0] > 0)
877  bv.dist(0) = bv.dist(D) = d;
878  else
879  bv.dist(0) = bv.dist(D) = -d;
880  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
881  if (n[1] > 0)
882  bv.dist(1) = bv.dist(D + 1) = d;
883  else
884  bv.dist(1) = bv.dist(D + 1) = -d;
885  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
886  if (n[2] > 0)
887  bv.dist(2) = bv.dist(D + 2) = d;
888  else
889  bv.dist(2) = bv.dist(D + 2) = -d;
890  } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) {
891  bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
892  } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) {
893  bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
894  } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) {
895  bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2;
896  } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
897  bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
898  } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
899  bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
900  } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) {
901  bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2;
902  } else if (n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
903  bv.dist(9) = bv.dist(D + 9) = n[0] * d * 3;
904  } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) {
905  bv.dist(10) = bv.dist(D + 10) = n[0] * d * 3;
906  } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
907  bv.dist(11) = bv.dist(D + 11) = n[1] * d * 3;
908  }
909 }
910 
911 void constructBox(const AABB& bv, Box& box, Transform3f& tf) {
912  box = Box(bv.max_ - bv.min_);
913  tf = Transform3f(bv.center());
914 }
915 
916 void constructBox(const OBB& bv, Box& box, Transform3f& tf) {
917  box = Box(bv.extent * 2);
918  tf = Transform3f(bv.axes, bv.To);
919 }
920 
921 void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf) {
922  box = Box(bv.obb.extent * 2);
923  tf = Transform3f(bv.obb.axes, bv.obb.To);
924 }
925 
926 void constructBox(const kIOS& bv, Box& box, Transform3f& tf) {
927  box = Box(bv.obb.extent * 2);
928  tf = Transform3f(bv.obb.axes, bv.obb.To);
929 }
930 
931 void constructBox(const RSS& bv, Box& box, Transform3f& tf) {
932  box = Box(bv.width(), bv.height(), bv.depth());
933  tf = Transform3f(bv.axes, bv.Tr);
934 }
935 
936 void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf) {
937  box = Box(bv.width(), bv.height(), bv.depth());
938  tf = Transform3f(bv.center());
939 }
940 
941 void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf) {
942  box = Box(bv.width(), bv.height(), bv.depth());
943  tf = Transform3f(bv.center());
944 }
945 
946 void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf) {
947  box = Box(bv.width(), bv.height(), bv.depth());
948  tf = Transform3f(bv.center());
949 }
950 
951 void constructBox(const AABB& bv, const Transform3f& tf_bv, Box& box,
952  Transform3f& tf) {
953  box = Box(bv.max_ - bv.min_);
954  tf = tf_bv * Transform3f(bv.center());
955 }
956 
957 void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box,
958  Transform3f& tf) {
959  box = Box(bv.extent * 2);
960  tf = tf_bv * Transform3f(bv.axes, bv.To);
961 }
962 
963 void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, Box& box,
964  Transform3f& tf) {
965  box = Box(bv.obb.extent * 2);
966  tf = tf_bv * Transform3f(bv.obb.axes, bv.obb.To);
967 }
968 
969 void constructBox(const kIOS& bv, const Transform3f& tf_bv, Box& box,
970  Transform3f& tf) {
971  box = Box(bv.obb.extent * 2);
972  tf = tf_bv * Transform3f(bv.obb.axes, bv.obb.To);
973 }
974 
975 void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box,
976  Transform3f& tf) {
977  box = Box(bv.width(), bv.height(), bv.depth());
978  tf = tf_bv * Transform3f(bv.axes, bv.Tr);
979 }
980 
981 void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, Box& box,
982  Transform3f& tf) {
983  box = Box(bv.width(), bv.height(), bv.depth());
984  tf = tf_bv * Transform3f(bv.center());
985 }
986 
987 void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, Box& box,
988  Transform3f& tf) {
989  box = Box(bv.width(), bv.height(), bv.depth());
990  tf = tf_bv * Transform3f(bv.center());
991 }
992 
993 void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, Box& box,
994  Transform3f& tf) {
995  box = Box(bv.width(), bv.height(), bv.depth());
996  tf = tf_bv * Transform3f(bv.center());
997 }
998 
999 } // namespace fcl
1000 
1001 } // namespace hpp
hpp::fcl::RSS::depth
FCL_REAL depth() const
Depth of the RSS.
Definition: BV/RSS.h:132
hpp::fcl::Box::halfSide
Vec3f halfSide
box side half-length
Definition: shape/geometric_shapes.h:148
hpp::fcl::computeBV< OBB, Halfspace >
HPP_FCL_DLLAPI void computeBV< OBB, Halfspace >(const Halfspace &s, const Transform3f &tf, OBB &bv)
Definition: geometric_shapes_utility.cpp:503
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
hpp::fcl::computeBV< OBB, ConvexBase >
HPP_FCL_DLLAPI void computeBV< OBB, ConvexBase >(const ConvexBase &s, const Transform3f &tf, OBB &bv)
Definition: geometric_shapes_utility.cpp:490
hpp::fcl::OBBRSS::obb
OBB obb
OBB member, for rotation.
Definition: BV/OBBRSS.h:56
hpp::fcl::generateCoordinateSystem
void generateCoordinateSystem(const Eigen::MatrixBase< Derived1 > &_w, const Eigen::MatrixBase< Derived2 > &_u, const Eigen::MatrixBase< Derived3 > &_v)
Definition: tools.h:61
generate_distance_plot.b
float b
Definition: generate_distance_plot.py:8
hpp::fcl::computeBV< AABB, Capsule >
HPP_FCL_DLLAPI void computeBV< AABB, Capsule >(const Capsule &s, const Transform3f &tf, AABB &bv)
Definition: geometric_shapes_utility.cpp:305
hpp::fcl::AABB::min_
Vec3f min_
The min point in the AABB.
Definition: BV/AABB.h:57
hpp::fcl::Halfspace::n
Vec3f n
Plane normal.
Definition: shape/geometric_shapes.h:787
hpp::fcl::KDOP
KDOP class describes the KDOP collision structures. K is set as the template parameter, which should be 16, 18, or 24 The KDOP structure is defined by some pairs of parallel planes defined by some axes. For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off some space of the edges: (-1,0,0) and (1,0,0) -> indices 0 and 8 (0,-1,0) and (0,1,0) -> indices 1 and 9 (0,0,-1) and (0,0,1) -> indices 2 and 10 (-1,-1,0) and (1,1,0) -> indices 3 and 11 (-1,0,-1) and (1,0,1) -> indices 4 and 12 (0,-1,-1) and (0,1,1) -> indices 5 and 13 (-1,1,0) and (1,-1,0) -> indices 6 and 14 (-1,0,1) and (1,0,-1) -> indices 7 and 15 For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off some space of the edges: (-1,0,0) and (1,0,0) -> indices 0 and 9 (0,-1,0) and (0,1,0) -> indices 1 and 10 (0,0,-1) and (0,0,1) -> indices 2 and 11 (-1,-1,0) and (1,1,0) -> indices 3 and 12 (-1,0,-1) and (1,0,1) -> indices 4 and 13 (0,-1,-1) and (0,1,1) -> indices 5 and 14 (-1,1,0) and (1,-1,0) -> indices 6 and 15 (-1,0,1) and (1,0,-1) -> indices 7 and 16 (0,-1,1) and (0,1,-1) -> indices 8 and 17 For K = 18, the planes are 6 AABB planes and 18 diagonal planes that cut off some space of the edges: (-1,0,0) and (1,0,0) -> indices 0 and 12 (0,-1,0) and (0,1,0) -> indices 1 and 13 (0,0,-1) and (0,0,1) -> indices 2 and 14 (-1,-1,0) and (1,1,0) -> indices 3 and 15 (-1,0,-1) and (1,0,1) -> indices 4 and 16 (0,-1,-1) and (0,1,1) -> indices 5 and 17 (-1,1,0) and (1,-1,0) -> indices 6 and 18 (-1,0,1) and (1,0,-1) -> indices 7 and 19 (0,-1,1) and (0,1,-1) -> indices 8 and 20 (-1, -1, 1) and (1, 1, -1) –> indices 9 and 21 (-1, 1, -1) and (1, -1, 1) –> indices 10 and 22 (1, -1, -1) and (-1, 1, 1) –> indices 11 and 23.
Definition: kDOP.h:92
hpp::fcl::Ellipsoid
Ellipsoid centered at point zero.
Definition: shape/geometric_shapes.h:258
hpp::fcl::computeBV< AABB, Cone >
HPP_FCL_DLLAPI void computeBV< AABB, Cone >(const Cone &s, const Transform3f &tf, AABB &bv)
Definition: geometric_shapes_utility.cpp:316
hpp::fcl::AABB::center
Vec3f center() const
Center of the AABB.
Definition: BV/AABB.h:157
hpp::fcl::TriangleP::b
Vec3f b
Definition: shape/geometric_shapes.h:109
hpp::fcl::Cone::halfLength
FCL_REAL halfLength
Half Length along z axis.
Definition: shape/geometric_shapes.h:433
hpp::fcl::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: shape/geometric_shapes.h:501
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::computeBV< OBB, Capsule >
HPP_FCL_DLLAPI void computeBV< OBB, Capsule >(const Capsule &s, const Transform3f &tf, OBB &bv)
Definition: geometric_shapes_utility.cpp:459
hpp::fcl::computeBV< OBB, Cone >
HPP_FCL_DLLAPI void computeBV< OBB, Cone >(const Cone &s, const Transform3f &tf, OBB &bv)
Definition: geometric_shapes_utility.cpp:469
hpp::fcl::computeBV< OBB, Box >
HPP_FCL_DLLAPI void computeBV< OBB, Box >(const Box &s, const Transform3f &tf, OBB &bv)
Definition: geometric_shapes_utility.cpp:440
hpp::fcl::RSS::axes
Matrix3f axes
Orientation of RSS. axis[i] is the ith column of the orientation matrix for the RSS; it is also the i...
Definition: BV/RSS.h:59
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
hpp::fcl::computeBV< AABB, Sphere >
HPP_FCL_DLLAPI void computeBV< AABB, Sphere >(const Sphere &s, const Transform3f &tf, AABB &bv)
Definition: geometric_shapes_utility.cpp:285
hpp::fcl::Halfspace
Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; Poi...
Definition: shape/geometric_shapes.h:729
hpp::fcl::details::getBoundVertices
std::vector< Vec3f > getBoundVertices(const Box &box, const Transform3f &tf)
Definition: geometric_shapes_utility.cpp:47
hpp::fcl::computeBV< AABB, Plane >
HPP_FCL_DLLAPI void computeBV< AABB, Plane >(const Plane &s, const Transform3f &tf, AABB &bv)
Definition: geometric_shapes_utility.cpp:405
hpp::fcl::computeBV< OBB, Plane >
HPP_FCL_DLLAPI void computeBV< OBB, Plane >(const Plane &s, const Transform3f &tf, OBB &bv)
Definition: geometric_shapes_utility.cpp:731
hpp::fcl::TriangleP::c
Vec3f c
Definition: shape/geometric_shapes.h:109
hpp::fcl::KDOP::depth
FCL_REAL depth() const
The (AABB) depth.
Definition: kDOP.h:157
hpp::fcl::kIOS::obb
OBB obb
@ OBB related with kIOS
Definition: kIOS.h:117
hpp::fcl::computeBV< kIOS, Plane >
HPP_FCL_DLLAPI void computeBV< kIOS, Plane >(const Plane &s, const Transform3f &tf, kIOS &bv)
Definition: geometric_shapes_utility.cpp:768
c
c
hpp::fcl::RSS::height
FCL_REAL height() const
Height of the RSS.
Definition: BV/RSS.h:129
hpp::fcl::kIOS
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:53
D
D
hpp::fcl::computeBV< AABB, Cylinder >
HPP_FCL_DLLAPI void computeBV< AABB, Cylinder >(const Cylinder &s, const Transform3f &tf, AABB &bv)
Definition: geometric_shapes_utility.cpp:333
hpp::fcl::fit
void fit(Vec3f *ps, unsigned int n, BV &bv)
Compute a bounding volume that fits a set of n points.
Definition: BV_fitter.h:52
hpp::fcl::computeBV< AABB, Ellipsoid >
HPP_FCL_DLLAPI void computeBV< AABB, Ellipsoid >(const Ellipsoid &e, const Transform3f &tf, AABB &bv)
Definition: geometric_shapes_utility.cpp:294
hpp::fcl::ConvexBase::num_points
unsigned int num_points
Definition: shape/geometric_shapes.h:624
BV_fitter.h
hpp::fcl::Transform3f::transform
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:153
hpp::fcl::Cone::radius
FCL_REAL radius
Radius of the cone.
Definition: shape/geometric_shapes.h:427
hpp::fcl::computeBV< AABB, Box >
HPP_FCL_DLLAPI void computeBV< AABB, Box >(const Box &s, const Transform3f &tf, AABB &bv)
Definition: geometric_shapes_utility.cpp:275
hpp::fcl::computeBV< AABB, TriangleP >
HPP_FCL_DLLAPI void computeBV< AABB, TriangleP >(const TriangleP &s, const Transform3f &tf, AABB &bv)
Definition: geometric_shapes_utility.cpp:366
hpp::fcl::RSS::Tr
Vec3f Tr
Origin of the rectangle in RSS.
Definition: BV/RSS.h:62
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: shape/geometric_shapes.h:125
hpp::fcl::computeBV< AABB, Halfspace >
HPP_FCL_DLLAPI void computeBV< AABB, Halfspace >(const Halfspace &s, const Transform3f &tf, AABB &bv)
Definition: geometric_shapes_utility.cpp:372
collision_manager.sphere
sphere
Definition: collision_manager.py:4
hpp::fcl::KDOP::height
FCL_REAL height() const
The (AABB) height.
Definition: kDOP.h:154
hpp::fcl::RSS
A class for rectangle sphere-swept bounding volume.
Definition: BV/RSS.h:53
hpp::fcl::TriangleP
Triangle stores the points instead of only indices of points.
Definition: shape/geometric_shapes.h:71
hpp::fcl::computeBV< OBBRSS, Halfspace >
HPP_FCL_DLLAPI void computeBV< OBBRSS, Halfspace >(const Halfspace &s, const Transform3f &tf, OBBRSS &bv)
Definition: geometric_shapes_utility.cpp:520
hpp::fcl::computeBV< kIOS, Halfspace >
HPP_FCL_DLLAPI void computeBV< kIOS, Halfspace >(const Halfspace &s, const Transform3f &tf, kIOS &bv)
Definition: geometric_shapes_utility.cpp:527
geometric_shapes_utility.h
hpp::fcl::Transform3f::getTranslation
const Vec3f & getTranslation() const
get translation
Definition: transform.h:100
hpp::fcl::Cone
Cone The base of the cone is at and the top is at .
Definition: shape/geometric_shapes.h:414
hpp::fcl::AABB
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: BV/AABB.h:54
hpp::fcl::TriangleP::a
Vec3f a
Definition: shape/geometric_shapes.h:109
hpp::fcl::AABB::max_
Vec3f max_
The max point in the AABB.
Definition: BV/AABB.h:59
R
R
hpp::fcl::Halfspace::d
FCL_REAL d
Plane offset.
Definition: shape/geometric_shapes.h:790
hpp::fcl::Plane::d
FCL_REAL d
Plane offset.
Definition: shape/geometric_shapes.h:851
hpp::fcl::Sphere::radius
FCL_REAL radius
Radius of the sphere.
Definition: shape/geometric_shapes.h:206
generate_distance_plot.m
float m
Definition: generate_distance_plot.py:7
hpp::fcl::Cylinder::radius
FCL_REAL radius
Radius of the cylinder.
Definition: shape/geometric_shapes.h:522
hpp::fcl::Plane::n
Vec3f n
Plane normal.
Definition: shape/geometric_shapes.h:848
hpp::fcl::Sphere
Center at zero point sphere.
Definition: shape/geometric_shapes.h:196
hpp::fcl::Capsule
Capsule It is where is the distance between the point x and the capsule segment AB...
Definition: shape/geometric_shapes.h:333
hpp::fcl::ConvexBase
Base for convex polytope.
Definition: shape/geometric_shapes.h:581
B
B
r2
r2
hpp::fcl::KDOP::center
Vec3f center() const
The (AABB) center.
Definition: kDOP.h:146
hpp::fcl::RSS::width
FCL_REAL width() const
Width of the RSS.
Definition: BV/RSS.h:126
hpp::fcl::computeBV< RSS, Halfspace >
HPP_FCL_DLLAPI void computeBV< RSS, Halfspace >(const Halfspace &s, const Transform3f &tf, RSS &bv)
Definition: geometric_shapes_utility.cpp:511
hpp::fcl::computeBV< OBB, Cylinder >
HPP_FCL_DLLAPI void computeBV< OBB, Cylinder >(const Cylinder &s, const Transform3f &tf, OBB &bv)
Definition: geometric_shapes_utility.cpp:479
hpp::fcl::OBB::To
Vec3f To
Center of OBB.
Definition: include/hpp/fcl/BV/OBB.h:61
hpp::fcl::computeBV< OBB, Sphere >
HPP_FCL_DLLAPI void computeBV< OBB, Sphere >(const Sphere &s, const Transform3f &tf, OBB &bv)
Definition: geometric_shapes_utility.cpp:450
collision_manager.box
box
Definition: collision_manager.py:11
hpp::fcl::Cylinder::halfLength
FCL_REAL halfLength
Half Length along z axis.
Definition: shape/geometric_shapes.h:528
a
list a
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::Ellipsoid::radii
Vec3f radii
Radii of the Ellipsoid (such that on boundary: x^2/rx^2 + y^2/ry^2.
Definition: shape/geometric_shapes.h:271
A
A
hpp::fcl::constructBox
HPP_FCL_DLLAPI void constructBox(const AABB &bv, Box &box, Transform3f &tf)
construct a box shape (with a configuration) from a given bounding volume
Definition: geometric_shapes_utility.cpp:911
hpp::fcl::computeBV< OBBRSS, Plane >
HPP_FCL_DLLAPI void computeBV< OBBRSS, Plane >(const Plane &s, const Transform3f &tf, OBBRSS &bv)
Definition: geometric_shapes_utility.cpp:761
hpp::fcl::computeBV< AABB, ConvexBase >
HPP_FCL_DLLAPI void computeBV< AABB, ConvexBase >(const ConvexBase &s, const Transform3f &tf, AABB &bv)
Definition: geometric_shapes_utility.cpp:351
hpp::fcl::OBB::axes
Matrix3f axes
Orientation of OBB. axis[i] is the ith column of the orientation matrix for the box; it is also the i...
Definition: include/hpp/fcl/BV/OBB.h:58
hpp::fcl::transform
HPP_FCL_DLLAPI Halfspace transform(const Halfspace &a, const Transform3f &tf)
Definition: geometric_shapes_utility.cpp:248
hpp::fcl::OBBRSS
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: BV/OBBRSS.h:54
hpp::fcl::KDOP::width
FCL_REAL width() const
The (AABB) width.
Definition: kDOP.h:151
hpp::fcl::Transform3f::getRotation
const Matrix3f & getRotation() const
get rotation
Definition: transform.h:109
hpp::fcl::computeBV< RSS, Plane >
HPP_FCL_DLLAPI void computeBV< RSS, Plane >(const Plane &s, const Transform3f &tf, RSS &bv)
Definition: geometric_shapes_utility.cpp:745
hpp::fcl::Capsule::radius
FCL_REAL radius
Radius of capsule.
Definition: shape/geometric_shapes.h:346
hpp::fcl::Capsule::halfLength
FCL_REAL halfLength
Half Length along z axis.
Definition: shape/geometric_shapes.h:352
hpp::fcl::OBB::extent
Vec3f extent
Half dimensions of OBB.
Definition: include/hpp/fcl/BV/OBB.h:64
hpp::fcl::ConvexBase::points
Vec3f * points
An array of the points of the polygon.
Definition: shape/geometric_shapes.h:623
hpp::fcl::OBB
Oriented bounding box class.
Definition: include/hpp/fcl/BV/OBB.h:52

hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:01