geometry/shape/utility-inl.h
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-2016, 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 #ifndef FCL_GEOMETRY_SHAPE_UTILITY_INL_H
39 #define FCL_GEOMETRY_SHAPE_UTILITY_INL_H
40 
41 #include "fcl/geometry/shape/utility.h"
42 
43 #include "fcl/common/unused.h"
44 
45 #include "fcl/math/bv/utility.h"
46 
47 #include "fcl/geometry/shape/capsule.h"
48 #include "fcl/geometry/shape/cone.h"
49 #include "fcl/geometry/shape/convex.h"
50 #include "fcl/geometry/shape/cylinder.h"
51 #include "fcl/geometry/shape/ellipsoid.h"
52 #include "fcl/geometry/shape/halfspace.h"
53 #include "fcl/geometry/shape/plane.h"
54 #include "fcl/geometry/shape/sphere.h"
55 #include "fcl/geometry/shape/triangle_p.h"
56 
57 namespace fcl {
58 
59 //==============================================================================
60 extern template
61 void constructBox(const OBB<double>& bv, Box<double>& box, Transform3<double>& tf);
62 
63 //==============================================================================
64 extern template
65 void constructBox(const OBBRSS<double>& bv, Box<double>& box, Transform3<double>& tf);
66 
67 //==============================================================================
68 extern template
69 void constructBox(const kIOS<double>& bv, Box<double>& box, Transform3<double>& tf);
70 
71 //==============================================================================
72 extern template
73 void constructBox(const RSS<double>& bv, Box<double>& box, Transform3<double>& tf);
74 
75 //==============================================================================
76 extern template
77 void constructBox(const KDOP<double, 16>& bv, Box<double>& box, Transform3<double>& tf);
78 
79 //==============================================================================
80 extern template
81 void constructBox(const KDOP<double, 18>& bv, Box<double>& box, Transform3<double>& tf);
82 
83 //==============================================================================
84 extern template
85 void constructBox(const KDOP<double, 24>& bv, Box<double>& box, Transform3<double>& tf);
86 
87 //==============================================================================
88 extern template
89 void constructBox(const AABB<double>& bv, const Transform3<double>& tf_bv, Box<double>& box, Transform3<double>& tf);
90 
91 //==============================================================================
92 extern template
93 void constructBox(const OBB<double>& bv, const Transform3<double>& tf_bv, Box<double>& box, Transform3<double>& tf);
94 
95 //==============================================================================
96 extern template
97 void constructBox(const OBBRSS<double>& bv, const Transform3<double>& tf_bv, Box<double>& box, Transform3<double>& tf);
98 
99 //==============================================================================
100 extern template
101 void constructBox(const kIOS<double>& bv, const Transform3<double>& tf_bv, Box<double>& box, Transform3<double>& tf);
102 
103 //==============================================================================
104 extern template
105 void constructBox(const RSS<double>& bv, const Transform3<double>& tf_bv, Box<double>& box, Transform3<double>& tf);
106 
107 //==============================================================================
108 extern template
109 void constructBox(const KDOP<double, 16>& bv, const Transform3<double>& tf_bv, Box<double>& box, Transform3<double>& tf);
110 
111 //==============================================================================
112 extern template
113 void constructBox(const KDOP<double, 18>& bv, const Transform3<double>& tf_bv, Box<double>& box, Transform3<double>& tf);
114 
115 //==============================================================================
116 extern template
117 void constructBox(const KDOP<double, 24>& bv, const Transform3<double>& tf_bv, Box<double>& box, Transform3<double>& tf);
118 
119 //==============================================================================
120 namespace detail {
121 //==============================================================================
122 
123 //==============================================================================
124 template <typename S, typename BV, typename Shape>
125 struct FCL_EXPORT ComputeBVImpl
126 {
127  static void run(const Shape& s, const Transform3<S>& tf, BV& bv)
128  {
129  std::vector<Vector3<S>> convex_bound_vertices = s.getBoundVertices(tf);
130  fit(convex_bound_vertices.data(),
131  static_cast<int>(convex_bound_vertices.size()), bv);
132  }
133 };
134 
135 //==============================================================================
136 template <typename S>
137 struct FCL_EXPORT ComputeBVImpl<S, AABB<S>, Box<S>>
138 {
139  static void run(const Box<S>& s, const Transform3<S>& tf, AABB<S>& bv)
140  {
141  const Matrix3<S>& R = tf.linear();
142  const Vector3<S>& T = tf.translation();
143 
144  S x_range = 0.5 * (fabs(R(0, 0) * s.side[0]) + fabs(R(0, 1) * s.side[1]) + fabs(R(0, 2) * s.side[2]));
145  S y_range = 0.5 * (fabs(R(1, 0) * s.side[0]) + fabs(R(1, 1) * s.side[1]) + fabs(R(1, 2) * s.side[2]));
146  S z_range = 0.5 * (fabs(R(2, 0) * s.side[0]) + fabs(R(2, 1) * s.side[1]) + fabs(R(2, 2) * s.side[2]));
147 
148  Vector3<S> v_delta(x_range, y_range, z_range);
149  bv.max_ = T + v_delta;
150  bv.min_ = T - v_delta;
151  }
152 };
153 
154 //==============================================================================
155 template <typename S>
156 struct FCL_EXPORT ComputeBVImpl<S, OBB<S>, Box<S>>
157 {
158  static void run(const Box<S>& s, const Transform3<S>& tf, OBB<S>& bv)
159  {
160  bv.axis = tf.linear();
161  bv.To = tf.translation();
162  bv.extent = s.side * (S)0.5;
163  }
164 };
165 
166 //==============================================================================
167 template <typename S>
168 struct FCL_EXPORT ComputeBVImpl<S, AABB<S>, Capsule<S>>
169 {
170  static void run(const Capsule<S>& s, const Transform3<S>& tf, AABB<S>& bv)
171  {
172  const Matrix3<S>& R = tf.linear();
173  const Vector3<S>& T = tf.translation();
174 
175  S x_range = 0.5 * fabs(R(0, 2) * s.lz) + s.radius;
176  S y_range = 0.5 * fabs(R(1, 2) * s.lz) + s.radius;
177  S z_range = 0.5 * fabs(R(2, 2) * s.lz) + s.radius;
178 
179  Vector3<S> v_delta(x_range, y_range, z_range);
180  bv.max_ = T + v_delta;
181  bv.min_ = T - v_delta;
182  }
183 };
184 
185 //==============================================================================
186 template <typename S>
187 struct FCL_EXPORT ComputeBVImpl<S, OBB<S>, Capsule<S>>
188 {
189  static void run(const Capsule<S>& s, const Transform3<S>& tf, OBB<S>& bv)
190  {
191  bv.axis = tf.linear();
192  bv.To = tf.translation();
193  bv.extent << s.radius, s.radius, s.lz / 2 + s.radius;
194  }
195 };
196 
197 //==============================================================================
198 template <typename S>
199 struct FCL_EXPORT ComputeBVImpl<S, AABB<S>, Cone<S>>
200 {
201  static void run(const Cone<S>& s, const Transform3<S>& tf, AABB<S>& bv)
202  {
203  const Matrix3<S>& R = tf.linear();
204  const Vector3<S>& T = tf.translation();
205 
206  S x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz);
207  S y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz);
208  S z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz);
209 
210  Vector3<S> v_delta(x_range, y_range, z_range);
211  bv.max_ = T + v_delta;
212  bv.min_ = T - v_delta;
213  }
214 };
215 
216 //==============================================================================
217 template <typename S>
218 struct FCL_EXPORT ComputeBVImpl<S, OBB<S>, Cone<S>>
219 {
220  static void run(const Cone<S>& s, const Transform3<S>& tf, OBB<S>& bv)
221  {
222  bv.axis = tf.linear();
223  bv.To = tf.translation();
224  bv.extent << s.radius, s.radius, s.lz / 2;
225  }
226 };
227 
228 //==============================================================================
229 template <typename S>
230 struct FCL_EXPORT ComputeBVImpl<S, AABB<S>, Convex<S>>
231 {
232  static void run(const Convex<S>& s, const Transform3<S>& tf, AABB<S>& bv)
233  {
234  const Matrix3<S>& R = tf.linear();
235  const Vector3<S>& T = tf.translation();
236 
237  AABB<S> bv_;
238  for (const auto& vertex : s.getVertices())
239  {
240  Vector3<S> new_p = R * vertex + T;
241  bv_ += new_p;
242  }
243 
244  bv = bv_;
245  }
246 };
247 
248 //==============================================================================
249 template <typename S>
250 struct FCL_EXPORT ComputeBVImpl<S, OBB<S>, Convex<S>>
251 {
252  static void run(const Convex<S>& s, const Transform3<S>& tf, OBB<S>& bv)
253  {
254  fit(s.getVertices().data(), static_cast<int>(s.getVertices().size()), bv);
255 
256  bv.axis = tf.linear();
257  bv.To = tf * bv.To;
258  }
259 };
260 
261 //==============================================================================
262 template <typename S>
263 struct FCL_EXPORT ComputeBVImpl<S, AABB<S>, Cylinder<S>>
264 {
265  static void run(const Cylinder<S>& s, const Transform3<S>& tf, AABB<S>& bv)
266  {
267  const Matrix3<S>& R = tf.linear();
268  const Vector3<S>& T = tf.translation();
269 
270  S x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz);
271  S y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz);
272  S z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz);
273 
274  Vector3<S> v_delta(x_range, y_range, z_range);
275  bv.max_ = T + v_delta;
276  bv.min_ = T - v_delta;
277  }
278 };
279 
280 //==============================================================================
281 template <typename S>
282 struct FCL_EXPORT ComputeBVImpl<S, OBB<S>, Cylinder<S>>
283 {
284  static void run(const Cylinder<S>& s, const Transform3<S>& tf, OBB<S>& bv)
285  {
286  bv.axis = tf.linear();
287  bv.To = tf.translation();
288  bv.extent << s.radius, s.radius, s.lz / 2;
289  }
290 };
291 
292 //==============================================================================
293 template <typename S>
294 struct FCL_EXPORT ComputeBVImpl<S, AABB<S>, Ellipsoid<S>>
295 {
296  static void run(const Ellipsoid<S>& s, const Transform3<S>& tf, AABB<S>& bv)
297  {
298  const Matrix3<S>& R = tf.linear();
299  const Vector3<S>& T = tf.translation();
300 
301  S x_range = (fabs(R(0, 0) * s.radii[0]) + fabs(R(0, 1) * s.radii[1]) + fabs(R(0, 2) * s.radii[2]));
302  S y_range = (fabs(R(1, 0) * s.radii[0]) + fabs(R(1, 1) * s.radii[1]) + fabs(R(1, 2) * s.radii[2]));
303  S z_range = (fabs(R(2, 0) * s.radii[0]) + fabs(R(2, 1) * s.radii[1]) + fabs(R(2, 2) * s.radii[2]));
304 
305  Vector3<S> v_delta(x_range, y_range, z_range);
306  bv.max_ = T + v_delta;
307  bv.min_ = T - v_delta;
308  }
309 };
310 
311 //==============================================================================
312 template <typename S>
313 struct FCL_EXPORT ComputeBVImpl<S, OBB<S>, Ellipsoid<S>>
314 {
315  static void run(const Ellipsoid<S>& s, const Transform3<S>& tf, OBB<S>& bv)
316  {
317  bv.axis = tf.linear();
318  bv.To = tf.translation();
319  bv.extent = s.radii;
320  }
321 };
322 
323 //==============================================================================
324 template <typename S>
325 struct FCL_EXPORT ComputeBVImpl<S, AABB<S>, Halfspace<S>>
326 {
327  static void run(const Halfspace<S>& s, const Transform3<S>& tf, AABB<S>& bv)
328  {
329  Halfspace<S> new_s = transform(s, tf);
330  const Vector3<S>& n = new_s.n;
331  const S& d = new_s.d;
332 
333  AABB<S> bv_;
334  bv_.min_ = Vector3<S>::Constant(-std::numeric_limits<S>::max());
335  bv_.max_ = Vector3<S>::Constant(std::numeric_limits<S>::max());
336  if(n[1] == (S)0.0 && n[2] == (S)0.0)
337  {
338  // normal aligned with x axis
339  if(n[0] < 0) bv_.min_[0] = -d;
340  else if(n[0] > 0) bv_.max_[0] = d;
341  }
342  else if(n[0] == (S)0.0 && n[2] == (S)0.0)
343  {
344  // normal aligned with y axis
345  if(n[1] < 0) bv_.min_[1] = -d;
346  else if(n[1] > 0) bv_.max_[1] = d;
347  }
348  else if(n[0] == (S)0.0 && n[1] == (S)0.0)
349  {
350  // normal aligned with z axis
351  if(n[2] < 0) bv_.min_[2] = -d;
352  else if(n[2] > 0) bv_.max_[2] = d;
353  }
354 
355  bv = bv_;
356  }
357 };
358 
359 //==============================================================================
360 template <typename S>
361 struct FCL_EXPORT ComputeBVImpl<S, OBB<S>, Halfspace<S>>
362 {
363  static void run(const Halfspace<S>& s, const Transform3<S>& tf, OBB<S>& bv)
364  {
365  FCL_UNUSED(s);
366  FCL_UNUSED(tf);
367 
369  bv.axis.setIdentity();
370  bv.To.setZero();
371  bv.extent.setConstant(std::numeric_limits<S>::max());
372  }
373 };
374 
375 //==============================================================================
376 template <typename S>
377 struct FCL_EXPORT ComputeBVImpl<S, RSS<S>, Halfspace<S>>
378 {
379  static void run(const Halfspace<S>& s, const Transform3<S>& tf, RSS<S>& bv)
380  {
381  FCL_UNUSED(s);
382  FCL_UNUSED(tf);
383 
385  bv.axis.setIdentity();
386  bv.To.setZero();
387  bv.l[0] = bv.l[1] = bv.r = std::numeric_limits<S>::max();
388  }
389 };
390 
391 //==============================================================================
392 template <typename S>
393 struct FCL_EXPORT ComputeBVImpl<S, OBBRSS<S>, Halfspace<S>>
394 {
395  static void run(const Halfspace<S>& s, const Transform3<S>& tf, OBBRSS<S>& bv)
396  {
397  computeBV(s, tf, bv.obb);
398  computeBV(s, tf, bv.rss);
399  }
400 };
401 
402 //==============================================================================
403 template <typename S>
404 struct FCL_EXPORT ComputeBVImpl<S, kIOS<S>, Halfspace<S>>
405 {
406  static void run(const Halfspace<S>& s, const Transform3<S>& tf, kIOS<S>& bv)
407  {
408  bv.num_spheres = 1;
409  computeBV(s, tf, bv.obb);
410  bv.spheres[0].o.setZero();
411  bv.spheres[0].r = std::numeric_limits<S>::max();
412  }
413 };
414 
415 //==============================================================================
416 template <typename S>
417 struct FCL_EXPORT ComputeBVImpl<S, KDOP<S, 16>, Halfspace<S>>
418 {
419  static void run(const Halfspace<S>& s, const Transform3<S>& tf, KDOP<S, 16>& bv)
420  {
421  Halfspace<S> new_s = transform(s, tf);
422  const Vector3<S>& n = new_s.n;
423  const S& d = new_s.d;
424 
425  const std::size_t D = 8;
426  for(std::size_t i = 0; i < D; ++i)
427  bv.dist(i) = -std::numeric_limits<S>::max();
428  for(std::size_t i = D; i < 2 * D; ++i)
429  bv.dist(i) = std::numeric_limits<S>::max();
430 
431  if(n[1] == (S)0.0 && n[2] == (S)0.0)
432  {
433  if(n[0] > 0) bv.dist(D) = d;
434  else bv.dist(0) = -d;
435  }
436  else if(n[0] == (S)0.0 && n[2] == (S)0.0)
437  {
438  if(n[1] > 0) bv.dist(D + 1) = d;
439  else bv.dist(1) = -d;
440  }
441  else if(n[0] == (S)0.0 && n[1] == (S)0.0)
442  {
443  if(n[2] > 0) bv.dist(D + 2) = d;
444  else bv.dist(2) = -d;
445  }
446  else if(n[2] == (S)0.0 && n[0] == n[1])
447  {
448  if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2;
449  else bv.dist(3) = n[0] * d * 2;
450  }
451  else if(n[1] == (S)0.0 && n[0] == n[2])
452  {
453  if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2;
454  else bv.dist(4) = n[0] * d * 2;
455  }
456  else if(n[0] == (S)0.0 && n[1] == n[2])
457  {
458  if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2;
459  else bv.dist(5) = n[1] * d * 2;
460  }
461  else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0)
462  {
463  if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2;
464  else bv.dist(6) = n[0] * d * 2;
465  }
466  else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0)
467  {
468  if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2;
469  else bv.dist(7) = n[0] * d * 2;
470  }
471  }
472 };
473 
474 //==============================================================================
475 template <typename S>
476 struct FCL_EXPORT ComputeBVImpl<S, KDOP<S, 18>, Halfspace<S>>
477 {
478  static void run(const Halfspace<S>& s, const Transform3<S>& tf, KDOP<S, 18>& bv)
479  {
480  Halfspace<S> new_s = transform(s, tf);
481  const Vector3<S>& n = new_s.n;
482  const S& d = new_s.d;
483 
484  const std::size_t D = 9;
485 
486  for(std::size_t i = 0; i < D; ++i)
487  bv.dist(i) = -std::numeric_limits<S>::max();
488  for(std::size_t i = D; i < 2 * D; ++i)
489  bv.dist(i) = std::numeric_limits<S>::max();
490 
491  if(n[1] == (S)0.0 && n[2] == (S)0.0)
492  {
493  if(n[0] > 0) bv.dist(D) = d;
494  else bv.dist(0) = -d;
495  }
496  else if(n[0] == (S)0.0 && n[2] == (S)0.0)
497  {
498  if(n[1] > 0) bv.dist(D + 1) = d;
499  else bv.dist(1) = -d;
500  }
501  else if(n[0] == (S)0.0 && n[1] == (S)0.0)
502  {
503  if(n[2] > 0) bv.dist(D + 2) = d;
504  else bv.dist(2) = -d;
505  }
506  else if(n[2] == (S)0.0 && n[0] == n[1])
507  {
508  if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2;
509  else bv.dist(3) = n[0] * d * 2;
510  }
511  else if(n[1] == (S)0.0 && n[0] == n[2])
512  {
513  if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2;
514  else bv.dist(4) = n[0] * d * 2;
515  }
516  else if(n[0] == (S)0.0 && n[1] == n[2])
517  {
518  if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2;
519  else bv.dist(5) = n[1] * d * 2;
520  }
521  else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0)
522  {
523  if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2;
524  else bv.dist(6) = n[0] * d * 2;
525  }
526  else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0)
527  {
528  if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2;
529  else bv.dist(7) = n[0] * d * 2;
530  }
531  else if(n[0] == (S)0.0 && n[1] + n[2] == (S)0.0)
532  {
533  if(n[1] > 0) bv.dist(D + 8) = n[1] * d * 2;
534  else bv.dist(8) = n[1] * d * 2;
535  }
536  }
537 };
538 
539 //==============================================================================
540 template <typename S>
541 struct FCL_EXPORT ComputeBVImpl<S, KDOP<S, 24>, Halfspace<S>>
542 {
543  static void run(const Halfspace<S>& s, const Transform3<S>& tf, KDOP<S, 24>& bv)
544  {
545  Halfspace<S> new_s = transform(s, tf);
546  const Vector3<S>& n = new_s.n;
547  const S& d = new_s.d;
548 
549  const std::size_t D = 12;
550 
551  for(std::size_t i = 0; i < D; ++i)
552  bv.dist(i) = -std::numeric_limits<S>::max();
553  for(std::size_t i = D; i < 2 * D; ++i)
554  bv.dist(i) = std::numeric_limits<S>::max();
555 
556  if(n[1] == (S)0.0 && n[2] == (S)0.0)
557  {
558  if(n[0] > 0) bv.dist(D) = d;
559  else bv.dist(0) = -d;
560  }
561  else if(n[0] == (S)0.0 && n[2] == (S)0.0)
562  {
563  if(n[1] > 0) bv.dist(D + 1) = d;
564  else bv.dist(1) = -d;
565  }
566  else if(n[0] == (S)0.0 && n[1] == (S)0.0)
567  {
568  if(n[2] > 0) bv.dist(D + 2) = d;
569  else bv.dist(2) = -d;
570  }
571  else if(n[2] == (S)0.0 && n[0] == n[1])
572  {
573  if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2;
574  else bv.dist(3) = n[0] * d * 2;
575  }
576  else if(n[1] == (S)0.0 && n[0] == n[2])
577  {
578  if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2;
579  else bv.dist(4) = n[0] * d * 2;
580  }
581  else if(n[0] == (S)0.0 && n[1] == n[2])
582  {
583  if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2;
584  else bv.dist(5) = n[1] * d * 2;
585  }
586  else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0)
587  {
588  if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2;
589  else bv.dist(6) = n[0] * d * 2;
590  }
591  else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0)
592  {
593  if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2;
594  else bv.dist(7) = n[0] * d * 2;
595  }
596  else if(n[0] == (S)0.0 && n[1] + n[2] == (S)0.0)
597  {
598  if(n[1] > 0) bv.dist(D + 8) = n[1] * d * 2;
599  else bv.dist(8) = n[1] * d * 2;
600  }
601  else if(n[0] + n[2] == (S)0.0 && n[0] + n[1] == (S)0.0)
602  {
603  if(n[0] > 0) bv.dist(D + 9) = n[0] * d * 3;
604  else bv.dist(9) = n[0] * d * 3;
605  }
606  else if(n[0] + n[1] == (S)0.0 && n[1] + n[2] == (S)0.0)
607  {
608  if(n[0] > 0) bv.dist(D + 10) = n[0] * d * 3;
609  else bv.dist(10) = n[0] * d * 3;
610  }
611  else if(n[0] + n[1] == (S)0.0 && n[0] + n[2] == (S)0.0)
612  {
613  if(n[1] > 0) bv.dist(D + 11) = n[1] * d * 3;
614  else bv.dist(11) = n[1] * d * 3;
615  }
616  }
617 };
618 
619 //==============================================================================
620 template <typename S>
621 struct FCL_EXPORT ComputeBVImpl<S, AABB<S>, Plane<S>>
622 {
623  static void run(const Plane<S>& s, const Transform3<S>& tf, AABB<S>& bv)
624  {
625  Plane<S> new_s = transform(s, tf);
626  const Vector3<S>& n = new_s.n;
627  const S& d = new_s.d;
628 
629  AABB<S> bv_;
630  bv_.min_ = Vector3<S>::Constant(-std::numeric_limits<S>::max());
631  bv_.max_ = Vector3<S>::Constant(std::numeric_limits<S>::max());
632  if(n[1] == (S)0.0 && n[2] == (S)0.0)
633  {
634  // normal aligned with x axis
635  if(n[0] < 0) { bv_.min_[0] = bv_.max_[0] = -d; }
636  else if(n[0] > 0) { bv_.min_[0] = bv_.max_[0] = d; }
637  }
638  else if(n[0] == (S)0.0 && n[2] == (S)0.0)
639  {
640  // normal aligned with y axis
641  if(n[1] < 0) { bv_.min_[1] = bv_.max_[1] = -d; }
642  else if(n[1] > 0) { bv_.min_[1] = bv_.max_[1] = d; }
643  }
644  else if(n[0] == (S)0.0 && n[1] == (S)0.0)
645  {
646  // normal aligned with z axis
647  if(n[2] < 0) { bv_.min_[2] = bv_.max_[2] = -d; }
648  else if(n[2] > 0) { bv_.min_[2] = bv_.max_[2] = d; }
649  }
650 
651  bv = bv_;
652  }
653 };
654 
655 //==============================================================================
656 template <typename S>
657 struct FCL_EXPORT ComputeBVImpl<S, OBB<S>, Plane<S>>
658 {
659  static void run(const Plane<S>& s, const Transform3<S>& tf, OBB<S>& bv)
660  {
661  const Vector3<S> n = tf.linear() * s.n;
662  bv.axis = generateCoordinateSystem(n);
663 
664  bv.extent << 0, std::numeric_limits<S>::max(), std::numeric_limits<S>::max();
665 
666  Vector3<S> p = s.n * s.d;
667  bv.To = tf * p;
668  }
669 };
670 
671 //==============================================================================
672 template <typename S>
673 struct FCL_EXPORT ComputeBVImpl<S, RSS<S>, Plane<S>>
674 {
675  static void run(const Plane<S>& s, const Transform3<S>& tf, RSS<S>& bv)
676  {
677  const Vector3<S> n = tf.linear() * s.n;
678  bv.axis = generateCoordinateSystem(n);
679 
680  bv.l[0] = std::numeric_limits<S>::max();
681  bv.l[1] = std::numeric_limits<S>::max();
682 
683  bv.r = 0;
684 
685  Vector3<S> p = s.n * s.d;
686  bv.To = tf * p;
687  }
688 };
689 
690 //==============================================================================
691 template <typename S>
692 struct FCL_EXPORT ComputeBVImpl<S, OBBRSS<S>, Plane<S>>
693 {
694  static void run(const Plane<S>& s, const Transform3<S>& tf, OBBRSS<S>& bv)
695  {
696  computeBV(s, tf, bv.obb);
697  computeBV(s, tf, bv.rss);
698  }
699 };
700 
701 //==============================================================================
702 template <typename S>
703 struct FCL_EXPORT ComputeBVImpl<S, kIOS<S>, Plane<S>>
704 {
705  static void run(const Plane<S>& s, const Transform3<S>& tf, kIOS<S>& bv)
706  {
707  bv.num_spheres = 1;
708  computeBV(s, tf, bv.obb);
709  bv.spheres[0].o.setZero();
710  bv.spheres[0].r = std::numeric_limits<S>::max();
711  }
712 };
713 
714 //==============================================================================
715 template <typename S>
716 struct FCL_EXPORT ComputeBVImpl<S, KDOP<S, 16>, Plane<S>>
717 {
718  static void run(const Plane<S>& s, const Transform3<S>& tf, KDOP<S, 16>& bv)
719  {
720  Plane<S> new_s = transform(s, tf);
721  const Vector3<S>& n = new_s.n;
722  const S& d = new_s.d;
723 
724  const std::size_t D = 8;
725 
726  for(std::size_t i = 0; i < D; ++i)
727  bv.dist(i) = -std::numeric_limits<S>::max();
728  for(std::size_t i = D; i < 2 * D; ++i)
729  bv.dist(i) = std::numeric_limits<S>::max();
730 
731  if(n[1] == (S)0.0 && n[2] == (S)0.0)
732  {
733  if(n[0] > 0) bv.dist(0) = bv.dist(D) = d;
734  else bv.dist(0) = bv.dist(D) = -d;
735  }
736  else if(n[0] == (S)0.0 && n[2] == (S)0.0)
737  {
738  if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d;
739  else bv.dist(1) = bv.dist(D + 1) = -d;
740  }
741  else if(n[0] == (S)0.0 && n[1] == (S)0.0)
742  {
743  if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d;
744  else bv.dist(2) = bv.dist(D + 2) = -d;
745  }
746  else if(n[2] == (S)0.0 && n[0] == n[1])
747  {
748  bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
749  }
750  else if(n[1] == (S)0.0 && n[0] == n[2])
751  {
752  bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
753  }
754  else if(n[0] == (S)0.0 && n[1] == n[2])
755  {
756  bv.dist(6) = bv.dist(D + 5) = n[1] * d * 2;
757  }
758  else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0)
759  {
760  bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
761  }
762  else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0)
763  {
764  bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
765  }
766  }
767 };
768 
769 //==============================================================================
770 template <typename S>
771 struct FCL_EXPORT ComputeBVImpl<S, KDOP<S, 18>, Plane<S>>
772 {
773  static void run(const Plane<S>& s, const Transform3<S>& tf, KDOP<S, 18>& bv)
774  {
775  Plane<S> new_s = transform(s, tf);
776  const Vector3<S>& n = new_s.n;
777  const S& d = new_s.d;
778 
779  const std::size_t D = 9;
780 
781  for(std::size_t i = 0; i < D; ++i)
782  bv.dist(i) = -std::numeric_limits<S>::max();
783  for(std::size_t i = D; i < 2 * D; ++i)
784  bv.dist(i) = std::numeric_limits<S>::max();
785 
786  if(n[1] == (S)0.0 && n[2] == (S)0.0)
787  {
788  if(n[0] > 0) bv.dist(0) = bv.dist(D) = d;
789  else bv.dist(0) = bv.dist(D) = -d;
790  }
791  else if(n[0] == (S)0.0 && n[2] == (S)0.0)
792  {
793  if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d;
794  else bv.dist(1) = bv.dist(D + 1) = -d;
795  }
796  else if(n[0] == (S)0.0 && n[1] == (S)0.0)
797  {
798  if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d;
799  else bv.dist(2) = bv.dist(D + 2) = -d;
800  }
801  else if(n[2] == (S)0.0 && n[0] == n[1])
802  {
803  bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
804  }
805  else if(n[1] == (S)0.0 && n[0] == n[2])
806  {
807  bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
808  }
809  else if(n[0] == (S)0.0 && n[1] == n[2])
810  {
811  bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2;
812  }
813  else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0)
814  {
815  bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
816  }
817  else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0)
818  {
819  bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
820  }
821  else if(n[0] == (S)0.0 && n[1] + n[2] == (S)0.0)
822  {
823  bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2;
824  }
825  }
826 };
827 
828 //==============================================================================
829 template <typename S>
830 struct FCL_EXPORT ComputeBVImpl<S, KDOP<S, 24>, Plane<S>>
831 {
832  static void run(const Plane<S>& s, const Transform3<S>& tf, KDOP<S, 24>& bv)
833  {
834  Plane<S> new_s = transform(s, tf);
835  const Vector3<S>& n = new_s.n;
836  const S& d = new_s.d;
837 
838  const std::size_t D = 12;
839 
840  for(std::size_t i = 0; i < D; ++i)
841  bv.dist(i) = -std::numeric_limits<S>::max();
842  for(std::size_t i = D; i < 2 * D; ++i)
843  bv.dist(i) = std::numeric_limits<S>::max();
844 
845  if(n[1] == (S)0.0 && n[2] == (S)0.0)
846  {
847  if(n[0] > 0) bv.dist(0) = bv.dist(D) = d;
848  else bv.dist(0) = bv.dist(D) = -d;
849  }
850  else if(n[0] == (S)0.0 && n[2] == (S)0.0)
851  {
852  if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d;
853  else bv.dist(1) = bv.dist(D + 1) = -d;
854  }
855  else if(n[0] == (S)0.0 && n[1] == (S)0.0)
856  {
857  if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d;
858  else bv.dist(2) = bv.dist(D + 2) = -d;
859  }
860  else if(n[2] == (S)0.0 && n[0] == n[1])
861  {
862  bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
863  }
864  else if(n[1] == (S)0.0 && n[0] == n[2])
865  {
866  bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
867  }
868  else if(n[0] == (S)0.0 && n[1] == n[2])
869  {
870  bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2;
871  }
872  else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0)
873  {
874  bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
875  }
876  else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0)
877  {
878  bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
879  }
880  else if(n[0] == (S)0.0 && n[1] + n[2] == (S)0.0)
881  {
882  bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2;
883  }
884  else if(n[0] + n[2] == (S)0.0 && n[0] + n[1] == (S)0.0)
885  {
886  bv.dist(9) = bv.dist(D + 9) = n[0] * d * 3;
887  }
888  else if(n[0] + n[1] == (S)0.0 && n[1] + n[2] == (S)0.0)
889  {
890  bv.dist(10) = bv.dist(D + 10) = n[0] * d * 3;
891  }
892  else if(n[0] + n[1] == (S)0.0 && n[0] + n[2] == (S)0.0)
893  {
894  bv.dist(11) = bv.dist(D + 11) = n[1] * d * 3;
895  }
896  }
897 };
898 
899 //==============================================================================
900 template <typename S>
901 struct FCL_EXPORT ComputeBVImpl<S, AABB<S>, Sphere<S>>
902 {
903  static void run(const Sphere<S>& s, const Transform3<S>& tf, AABB<S>& bv)
904  {
905  const Vector3<S> v_delta = Vector3<S>::Constant(s.radius);
906  bv.max_ = tf.translation() + v_delta;
907  bv.min_ = tf.translation() - v_delta;
908  }
909 };
910 
911 //==============================================================================
912 template <typename S>
913 struct FCL_EXPORT ComputeBVImpl<S, OBB<S>, Sphere<S>>
914 {
915  static void run(const Sphere<S>& s, const Transform3<S>& tf, OBB<S>& bv)
916  {
917  bv.To = tf.translation();
918  bv.axis.setIdentity();
919  bv.extent.setConstant(s.radius);
920  }
921 };
922 
923 //==============================================================================
924 template <typename S>
925 struct FCL_EXPORT ComputeBVImpl<S, AABB<S>, TriangleP<S>>
926 {
927  static void run(const TriangleP<S>& s, const Transform3<S>& tf, AABB<S>& bv)
928  {
929  bv = AABB<S>(tf * s.a, tf * s.b, tf * s.c);
930  }
931 };
932 
933 //==============================================================================
934 extern template
935 struct ComputeBVImpl<double, AABB<double>, Box<double>>;
936 
937 //==============================================================================
938 extern template
939 struct ComputeBVImpl<double, OBB<double>, Box<double>>;
940 
941 //==============================================================================
942 extern template
943 struct ComputeBVImpl<double, AABB<double>, Capsule<double>>;
944 
945 //==============================================================================
946 extern template
947 struct ComputeBVImpl<double, OBB<double>, Capsule<double>>;
948 
949 //==============================================================================
950 extern template
951 struct ComputeBVImpl<double, AABB<double>, Cone<double>>;
952 
953 //==============================================================================
954 extern template
955 struct ComputeBVImpl<double, OBB<double>, Cone<double>>;
956 
957 //==============================================================================
958 extern template
959 struct ComputeBVImpl<double, AABB<double>, Cylinder<double>>;
960 
961 //==============================================================================
962 extern template
963 struct ComputeBVImpl<double, OBB<double>, Cylinder<double>>;
964 
965 //==============================================================================
966 extern template
967 struct ComputeBVImpl<double, AABB<double>, Ellipsoid<double>>;
968 
969 //==============================================================================
970 extern template
971 struct ComputeBVImpl<double, OBB<double>, Ellipsoid<double>>;
972 
973 //==============================================================================
974 extern template
975 struct ComputeBVImpl<double, AABB<double>, Halfspace<double>>;
976 
977 //==============================================================================
978 extern template
979 struct ComputeBVImpl<double, OBB<double>, Halfspace<double>>;
980 
981 //==============================================================================
982 extern template
983 struct ComputeBVImpl<double, RSS<double>, Halfspace<double>>;
984 
985 //==============================================================================
986 extern template
987 struct ComputeBVImpl<double, OBBRSS<double>, Halfspace<double>>;
988 
989 //==============================================================================
990 extern template
991 struct ComputeBVImpl<double, kIOS<double>, Halfspace<double>>;
992 
993 //==============================================================================
994 extern template
995 struct ComputeBVImpl<double, KDOP<double, 16>, Halfspace<double>>;
996 
997 //==============================================================================
998 extern template
999 struct ComputeBVImpl<double, KDOP<double, 18>, Halfspace<double>>;
1000 
1001 //==============================================================================
1002 extern template
1003 struct ComputeBVImpl<double, KDOP<double, 24>, Halfspace<double>>;
1004 
1005 //==============================================================================
1006 extern template
1007 struct ComputeBVImpl<double, AABB<double>, Plane<double>>;
1008 
1009 //==============================================================================
1010 extern template
1011 struct ComputeBVImpl<double, OBB<double>, Plane<double>>;
1012 
1013 //==============================================================================
1014 extern template
1015 struct ComputeBVImpl<double, RSS<double>, Plane<double>>;
1016 
1017 //==============================================================================
1018 extern template
1019 struct ComputeBVImpl<double, OBBRSS<double>, Plane<double>>;
1020 
1021 //==============================================================================
1022 extern template
1023 struct ComputeBVImpl<double, kIOS<double>, Plane<double>>;
1024 
1025 //==============================================================================
1026 extern template
1027 struct ComputeBVImpl<double, KDOP<double, 16>, Plane<double>>;
1028 
1029 //==============================================================================
1030 extern template
1031 struct ComputeBVImpl<double, KDOP<double, 18>, Plane<double>>;
1032 
1033 //==============================================================================
1034 extern template
1035 struct ComputeBVImpl<double, KDOP<double, 24>, Plane<double>>;
1036 
1037 //==============================================================================
1038 extern template
1039 struct ComputeBVImpl<double, AABB<double>, Sphere<double>>;
1040 
1041 //==============================================================================
1042 extern template
1043 struct ComputeBVImpl<double, OBB<double>, Sphere<double>>;
1044 
1045 //==============================================================================
1046 extern template
1047 struct ComputeBVImpl<double, AABB<double>, TriangleP<double>>;
1048 
1049 //==============================================================================
1050 } // namespace detail
1051 //==============================================================================
1052 
1053 //==============================================================================
1054 template <typename BV, typename Shape>
1055 FCL_EXPORT
1056 void computeBV(const Shape& s, const Transform3<typename BV::S>& tf, BV& bv)
1057 {
1058  using S = typename BV::S;
1059 
1060  detail::ComputeBVImpl<S, BV, Shape>::run(s, tf, bv);
1061 }
1062 
1063 //==============================================================================
1064 template <typename S>
1065 void constructBox(const AABB<S>& bv, Box<S>& box, Transform3<S>& tf)
1066 {
1067  box = Box<S>(bv.max_ - bv.min_);
1068  tf.linear().setIdentity();
1069  tf.translation() = bv.center();
1070 }
1071 
1072 //==============================================================================
1073 template <typename S>
1074 void constructBox(const OBB<S>& bv, Box<S>& box, Transform3<S>& tf)
1075 {
1076  box = Box<S>(bv.extent * 2);
1077  tf.linear() = bv.axis;
1078  tf.translation() = bv.To;
1079 }
1080 
1081 //==============================================================================
1082 template <typename S>
1083 void constructBox(const OBBRSS<S>& bv, Box<S>& box, Transform3<S>& tf)
1084 {
1085  box = Box<S>(bv.obb.extent * 2);
1086  tf.linear() = bv.obb.axis;
1087  tf.translation() = bv.obb.To;
1088 }
1089 
1090 //==============================================================================
1091 template <typename S>
1092 void constructBox(const kIOS<S>& bv, Box<S>& box, Transform3<S>& tf)
1093 {
1094  box = Box<S>(bv.obb.extent * 2);
1095  tf.linear() = bv.obb.axis;
1096  tf.translation() = bv.obb.To;
1097 }
1098 
1099 //==============================================================================
1100 template <typename S>
1101 void constructBox(const RSS<S>& bv, Box<S>& box, Transform3<S>& tf)
1102 {
1103  box = Box<S>(bv.width(), bv.height(), bv.depth());
1104  tf.linear() = bv.axis;
1105  tf.translation() = bv.To;
1106 }
1107 
1108 //==============================================================================
1109 template <typename S>
1110 void constructBox(const KDOP<S, 16>& bv, Box<S>& box, Transform3<S>& tf)
1111 {
1112  box = Box<S>(bv.width(), bv.height(), bv.depth());
1113  tf.linear().setIdentity();
1114  tf.translation() = bv.center();
1115 }
1116 
1117 //==============================================================================
1118 template <typename S>
1119 void constructBox(const KDOP<S, 18>& bv, Box<S>& box, Transform3<S>& tf)
1120 {
1121  box = Box<S>(bv.width(), bv.height(), bv.depth());
1122  tf.linear().setIdentity();
1123  tf.translation() = bv.center();
1124 }
1125 
1126 //==============================================================================
1127 template <typename S>
1128 void constructBox(const KDOP<S, 24>& bv, Box<S>& box, Transform3<S>& tf)
1129 {
1130  box = Box<S>(bv.width(), bv.height(), bv.depth());
1131  tf.linear().setIdentity();
1132  tf.translation() = bv.center();
1133 }
1134 
1135 //==============================================================================
1136 template <typename S>
1137 void constructBox(const AABB<S>& bv, const Transform3<S>& tf_bv, Box<S>& box, Transform3<S>& tf)
1138 {
1139  box = Box<S>(bv.max_ - bv.min_);
1140  tf = tf_bv * Translation3<S>(bv.center());
1141 }
1142 
1143 //==============================================================================
1144 template <typename S>
1145 void constructBox(const OBB<S>& bv, const Transform3<S>& tf_bv, Box<S>& box, Transform3<S>& tf)
1146 {
1147  FCL_UNUSED(tf_bv);
1148 
1149  box = Box<S>(bv.extent * 2);
1150  tf.linear() = bv.axis;
1151  tf.translation() = bv.To;
1152 }
1153 
1154 //==============================================================================
1155 template <typename S>
1156 void constructBox(const OBBRSS<S>& bv, const Transform3<S>& tf_bv, Box<S>& box, Transform3<S>& tf)
1157 {
1158  box = Box<S>(bv.obb.extent * 2);
1159  tf.linear() = bv.obb.axis;
1160  tf.translation() = bv.obb.To;
1161  tf = tf_bv * tf;
1162 }
1163 
1164 //==============================================================================
1165 template <typename S>
1166 void constructBox(const kIOS<S>& bv, const Transform3<S>& tf_bv, Box<S>& box, Transform3<S>& tf)
1167 {
1168  FCL_UNUSED(tf_bv);
1169 
1170  box = Box<S>(bv.obb.extent * 2);
1171  tf.linear() = bv.obb.axis;
1172  tf.translation() = bv.obb.To;
1173 }
1174 
1175 //==============================================================================
1176 template <typename S>
1177 void constructBox(const RSS<S>& bv, const Transform3<S>& tf_bv, Box<S>& box, Transform3<S>& tf)
1178 {
1179  box = Box<S>(bv.width(), bv.height(), bv.depth());
1180  tf.linear() = bv.axis;
1181  tf.translation() = bv.To;
1182  tf = tf_bv * tf;
1183 }
1184 
1185 //==============================================================================
1186 template <typename S>
1187 void constructBox(const KDOP<S, 16>& bv, const Transform3<S>& tf_bv, Box<S>& box, Transform3<S>& tf)
1188 {
1189  box = Box<S>(bv.width(), bv.height(), bv.depth());
1190  tf = tf_bv * Translation3<S>(bv.center());
1191 }
1192 
1193 //==============================================================================
1194 template <typename S>
1195 void constructBox(const KDOP<S, 18>& bv, const Transform3<S>& tf_bv, Box<S>& box, Transform3<S>& tf)
1196 {
1197  box = Box<S>(bv.width(), bv.height(), bv.depth());
1198  tf = tf_bv * Translation3<S>(bv.center());
1199 }
1200 
1201 //==============================================================================
1202 template <typename S>
1203 void constructBox(const KDOP<S, 24>& bv, const Transform3<S>& tf_bv, Box<S>& box, Transform3<S>& tf)
1204 {
1205  box = Box<S>(bv.width(), bv.height(), bv.depth());
1206  tf = tf_bv * Translation3<S>(bv.center());
1207 }
1208 
1209 } // namespace fcl
1210 
1211 #endif
fcl::TriangleP
Triangle stores the points instead of only indices of points.
Definition: triangle_p.h:50
fcl::detail::ComputeBVImpl< S, AABB< S >, TriangleP< S > >::run
static void run(const TriangleP< S > &s, const Transform3< S > &tf, AABB< S > &bv)
Definition: geometry/shape/utility-inl.h:927
fcl::detail::ComputeBVImpl< S, OBB< S >, Convex< S > >::run
static void run(const Convex< S > &s, const Transform3< S > &tf, OBB< S > &bv)
Definition: geometry/shape/utility-inl.h:252
fcl::detail::ComputeBVImpl< S, OBBRSS< S >, Plane< S > >::run
static void run(const Plane< S > &s, const Transform3< S > &tf, OBBRSS< S > &bv)
Definition: geometry/shape/utility-inl.h:694
fcl::Halfspace
Half Space: this is equivalent to the Planed in ODE. The separation plane is defined as n * x = d...
Definition: geometry/shape/halfspace.h:60
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::Capsule::lz
S lz
Length along z axis.
Definition: capsule.h:64
fcl::detail::ComputeBVImpl< S, OBB< S >, Plane< S > >::run
static void run(const Plane< S > &s, const Transform3< S > &tf, OBB< S > &bv)
Definition: geometry/shape/utility-inl.h:659
fcl::RSS::axis
Matrix3< S > axis
Frame T&#39;s orientation in frame F.
Definition: RSS.h:70
fcl::detail::ComputeBVImpl< S, KDOP< S, 18 >, Halfspace< S > >::run
static void run(const Halfspace< S > &s, const Transform3< S > &tf, KDOP< S, 18 > &bv)
Definition: geometry/shape/utility-inl.h:478
fcl::RSS::To
Vector3< S > To
Origin of frame T in frame F.
Definition: RSS.h:76
fcl::KDOP::depth
S depth() const
The (AABB) depth.
Definition: kDOP-inl.h:229
fcl::RSS
A class for rectangle swept sphere bounding volume.
Definition: RSS.h:58
fcl::Cylinder::radius
S radius
Radius of the cylinder.
Definition: cylinder.h:61
fcl::RSS< double >
template class FCL_EXPORT RSS< double >
fcl::transform
template Halfspace< double > transform(const Halfspace< double > &a, const Transform3< double > &tf)
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
triangle_p.h
fcl::AABB
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:49
fcl::detail::ComputeBVImpl::run
static void run(const Shape &s, const Transform3< S > &tf, BV &bv)
Definition: geometry/shape/utility-inl.h:127
fcl::KDOP::height
S height() const
The (AABB) height.
Definition: kDOP-inl.h:221
fcl::detail::ComputeBVImpl< S, OBB< S >, Cone< S > >::run
static void run(const Cone< S > &s, const Transform3< S > &tf, OBB< S > &bv)
Definition: geometry/shape/utility-inl.h:220
fcl::detail::ComputeBVImpl< S, kIOS< S >, Halfspace< S > >::run
static void run(const Halfspace< S > &s, const Transform3< S > &tf, kIOS< S > &bv)
Definition: geometry/shape/utility-inl.h:406
fcl::Ellipsoid
Center at zero point ellipsoid.
Definition: ellipsoid.h:51
fcl::TriangleP::b
Vector3< S > b
Definition: triangle_p.h:67
fcl::KDOP< double, 24 >
template class FCL_EXPORT KDOP< double, 24 >
fcl::detail::ComputeBVImpl< S, OBB< S >, Ellipsoid< S > >::run
static void run(const Ellipsoid< S > &s, const Transform3< S > &tf, OBB< S > &bv)
Definition: geometry/shape/utility-inl.h:315
fcl::detail::ComputeBVImpl< S, OBB< S >, Halfspace< S > >::run
static void run(const Halfspace< S > &s, const Transform3< S > &tf, OBB< S > &bv)
Definition: geometry/shape/utility-inl.h:363
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:85
fcl::AABB::max_
Vector3< S > max_
The max point in the AABB.
Definition: AABB.h:59
fcl::AABB::center
Vector3< S > center() const
Center of the AABB.
Definition: AABB-inl.h:230
fcl::Cylinder::lz
S lz
Length along z axis.
Definition: cylinder.h:64
fcl::KDOP::dist
S dist(std::size_t i) const
Definition: kDOP-inl.h:274
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::KDOP< double, 16 >
template class FCL_EXPORT KDOP< double, 16 >
fcl::Convex::getVertices
const std::vector< Vector3< S > > & getVertices() const
Gets the vertex positions in the geometry&#39;s frame G.
Definition: convex.h:126
fcl::computeBV
FCL_EXPORT void computeBV(const Shape &s, const Transform3< typename BV::S > &tf, BV &bv)
calculate a bounding volume for a shape in a specific configuration
Definition: geometry/shape/utility-inl.h:1056
fcl::detail::ComputeBVImpl< S, KDOP< S, 24 >, Halfspace< S > >::run
static void run(const Halfspace< S > &s, const Transform3< S > &tf, KDOP< S, 24 > &bv)
Definition: geometry/shape/utility-inl.h:543
fcl::detail::ComputeBVImpl< S, AABB< S >, Cone< S > >::run
static void run(const Cone< S > &s, const Transform3< S > &tf, AABB< S > &bv)
Definition: geometry/shape/utility-inl.h:201
fcl::KDOP< double, 18 >
template class FCL_EXPORT KDOP< double, 18 >
fcl::detail::ComputeBVImpl< S, KDOP< S, 16 >, Plane< S > >::run
static void run(const Plane< S > &s, const Transform3< S > &tf, KDOP< S, 16 > &bv)
Definition: geometry/shape/utility-inl.h:718
fcl::detail::ComputeBVImpl< S, KDOP< S, 18 >, Plane< S > >::run
static void run(const Plane< S > &s, const Transform3< S > &tf, KDOP< S, 18 > &bv)
Definition: geometry/shape/utility-inl.h:773
fcl::detail::ComputeBVImpl< S, AABB< S >, Convex< S > >::run
static void run(const Convex< S > &s, const Transform3< S > &tf, AABB< S > &bv)
Definition: geometry/shape/utility-inl.h:232
fcl::detail::ComputeBVImpl< S, OBB< S >, Sphere< S > >::run
static void run(const Sphere< S > &s, const Transform3< S > &tf, OBB< S > &bv)
Definition: geometry/shape/utility-inl.h:915
fcl::detail::ComputeBVImpl< S, RSS< S >, Halfspace< S > >::run
static void run(const Halfspace< S > &s, const Transform3< S > &tf, RSS< S > &bv)
Definition: geometry/shape/utility-inl.h:379
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:84
fcl::TriangleP::c
Vector3< S > c
Definition: triangle_p.h:68
fcl::OBBRSS::obb
OBB< S > obb
OBB member, for rotation.
Definition: OBBRSS.h:57
fcl::RSS::l
S l[2]
Side lengths of rectangle in frame T.
Definition: RSS.h:79
fcl::Capsule
Center at zero point capsule.
Definition: capsule.h:51
fcl::detail::ComputeBVImpl< S, KDOP< S, 24 >, Plane< S > >::run
static void run(const Plane< S > &s, const Transform3< S > &tf, KDOP< S, 24 > &bv)
Definition: geometry/shape/utility-inl.h:832
fcl::RSS::depth
S depth() const
Depth of the RSS.
Definition: RSS-inl.h:395
fcl::kIOS::obb
OBB< S > obb
OBB related with kIOS.
Definition: kIOS.h:72
fcl::Cone::lz
S lz
Length along z axis.
Definition: cone.h:63
fcl::RSS::width
S width() const
Width of the RSS.
Definition: RSS-inl.h:381
fcl::detail::ComputeBVImpl< S, OBB< S >, Capsule< S > >::run
static void run(const Capsule< S > &s, const Transform3< S > &tf, OBB< S > &bv)
Definition: geometry/shape/utility-inl.h:189
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:51
fcl::Cone::radius
S radius
Radius of the cone.
Definition: cone.h:60
fcl::detail::ComputeBVImpl< S, AABB< S >, Halfspace< S > >::run
static void run(const Halfspace< S > &s, const Transform3< S > &tf, AABB< S > &bv)
Definition: geometry/shape/utility-inl.h:327
fcl::OBBRSS::rss
RSS< S > rss
RSS member, for distance.
Definition: OBBRSS.h:60
fcl::AABB::min_
Vector3< S > min_
The min point in the AABB.
Definition: AABB.h:56
fcl::TriangleP::a
Vector3< S > a
Definition: triangle_p.h:66
fcl::Box::side
Vector3< S > side
box side length
Definition: box.h:67
fcl::kIOS::num_spheres
unsigned int num_spheres
The number of spheres, no larger than 5.
Definition: kIOS.h:69
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::detail::ComputeBVImpl< S, AABB< S >, Sphere< S > >::run
static void run(const Sphere< S > &s, const Transform3< S > &tf, AABB< S > &bv)
Definition: geometry/shape/utility-inl.h:903
fcl::constructBox
template void constructBox(const OBB< double > &bv, Box< double > &box, Transform3< double > &tf)
fcl::Translation3
Eigen::Translation< S, 3 > Translation3
Definition: types.h:94
fcl::detail::ComputeBVImpl< S, AABB< S >, Capsule< S > >::run
static void run(const Capsule< S > &s, const Transform3< S > &tf, AABB< S > &bv)
Definition: geometry/shape/utility-inl.h:170
FCL_UNUSED
#define FCL_UNUSED(x)
Definition: unused.h:39
fcl::kIOS< double >
template class FCL_EXPORT kIOS< double >
fcl::OBB::extent
Vector3< S > extent
Half dimensions of OBB.
Definition: OBB.h:68
fcl::AABB< double >
template class FCL_EXPORT AABB< double >
fcl::Convex
A convex polytope.
Definition: convex.h:84
fcl::Plane
Infinite plane.
Definition: geometry/shape/plane.h:51
fcl::Cylinder
Center at zero cylinder.
Definition: cylinder.h:51
fcl::OBB< double >
template class FCL_EXPORT OBB< double >
fcl::RSS::r
S r
Radius of sphere summed with rectangle to form RSS.
Definition: RSS.h:82
fcl::Ellipsoid::radii
Vector3< S > radii
Radii of the ellipsoid.
Definition: ellipsoid.h:64
fcl::generateCoordinateSystem
template Matrix3d generateCoordinateSystem(const Vector3d &x_axis)
fcl::Plane::d
S d
Plane offset.
Definition: geometry/shape/plane.h:79
fcl::OBB::axis
Matrix3< S > axis
Orientation of OBB. The axes of the rotation matrix are the principle directions of the box...
Definition: OBB.h:62
fcl::Cone
Center at zero cone.
Definition: cone.h:51
fcl::detail::ComputeBVImpl< S, KDOP< S, 16 >, Halfspace< S > >::run
static void run(const Halfspace< S > &s, const Transform3< S > &tf, KDOP< S, 16 > &bv)
Definition: geometry/shape/utility-inl.h:419
fcl::detail::ComputeBVImpl< S, AABB< S >, Ellipsoid< S > >::run
static void run(const Ellipsoid< S > &s, const Transform3< S > &tf, AABB< S > &bv)
Definition: geometry/shape/utility-inl.h:296
fcl::detail::ComputeBVImpl< S, AABB< S >, Plane< S > >::run
static void run(const Plane< S > &s, const Transform3< S > &tf, AABB< S > &bv)
Definition: geometry/shape/utility-inl.h:623
fcl::Sphere::radius
S radius
Radius of the sphere.
Definition: sphere.h:60
fcl::Halfspace::n
Vector3< S > n
Planed normal.
Definition: geometry/shape/halfspace.h:85
fcl::detail::ComputeBVImpl< S, kIOS< S >, Plane< S > >::run
static void run(const Plane< S > &s, const Transform3< S > &tf, kIOS< S > &bv)
Definition: geometry/shape/utility-inl.h:705
fcl::fit
FCL_EXPORT void fit(const Vector3< typename BV::S > *const ps, int n, BV &bv)
Compute a bounding volume that fits a set of n points.
Definition: math/bv/utility-inl.h:671
fcl::OBBRSS< double >
template class FCL_EXPORT OBBRSS< double >
fcl::detail::ComputeBVImpl< S, RSS< S >, Plane< S > >::run
static void run(const Plane< S > &s, const Transform3< S > &tf, RSS< S > &bv)
Definition: geometry/shape/utility-inl.h:675
fcl::Sphere
Center at zero point sphere.
Definition: sphere.h:51
fcl::detail::ComputeBVImpl< S, OBB< S >, Cylinder< S > >::run
static void run(const Cylinder< S > &s, const Transform3< S > &tf, OBB< S > &bv)
Definition: geometry/shape/utility-inl.h:284
fcl::OBBRSS
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: OBBRSS.h:50
fcl::detail::ComputeBVImpl< S, AABB< S >, Box< S > >::run
static void run(const Box< S > &s, const Transform3< S > &tf, AABB< S > &bv)
Definition: geometry/shape/utility-inl.h:139
fcl::Capsule::radius
S radius
Radius of capsule.
Definition: capsule.h:61
fcl::OBB::To
Vector3< S > To
Center of OBB.
Definition: OBB.h:65
fcl::KDOP::center
Vector3< S > center() const
The (AABB) center.
Definition: kDOP-inl.h:253
fcl::detail::ComputeBVImpl< S, AABB< S >, Cylinder< S > >::run
static void run(const Cylinder< S > &s, const Transform3< S > &tf, AABB< S > &bv)
Definition: geometry/shape/utility-inl.h:265
fcl::detail::ComputeBVImpl< S, OBBRSS< S >, Halfspace< S > >::run
static void run(const Halfspace< S > &s, const Transform3< S > &tf, OBBRSS< S > &bv)
Definition: geometry/shape/utility-inl.h:395
fcl::detail::ComputeBVImpl< S, OBB< S >, Box< S > >::run
static void run(const Box< S > &s, const Transform3< S > &tf, OBB< S > &bv)
Definition: geometry/shape/utility-inl.h:158
fcl::KDOP::width
S width() const
The (AABB) width.
Definition: kDOP-inl.h:213
fcl::kIOS
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:48
fcl::Box< double >
template class FCL_EXPORT Box< double >
fcl::RSS::height
S height() const
Height of the RSS.
Definition: RSS-inl.h:388
fcl::Plane::n
Vector3< S > n
Plane normal.
Definition: geometry/shape/plane.h:76
fcl::kIOS::spheres
kIOS_Sphere spheres[5]
The (at most) five spheres for intersection.
Definition: kIOS.h:66
fcl::OBB
Oriented bounding box class.
Definition: OBB.h:51

fcl_catkin
Author(s):
autogenerated on Thu Mar 23 2023 03:00:19