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

hpp-fcl
Author(s):
autogenerated on Sat Nov 23 2024 03:44:58