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