GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/shape/geometric_shapes_utility.cpp Lines: 205 645 31.8 %
Date: 2024-02-09 12:57:42 Branches: 235 2204 10.7 %

Line Branch Exec Source
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
36
/** \author Jia Pan */
37
38
#include <hpp/fcl/shape/geometric_shapes_utility.h>
39
#include <hpp/fcl/internal/BV_fitter.h>
40
#include <hpp/fcl/internal/tools.h>
41
42
namespace hpp {
43
namespace fcl {
44
45
namespace details {
46
47
202
std::vector<Vec3f> getBoundVertices(const Box& box, const Transform3f& tf) {
48
202
  std::vector<Vec3f> result(8);
49
202
  FCL_REAL a = box.halfSide[0];
50
202
  FCL_REAL b = box.halfSide[1];
51
202
  FCL_REAL c = box.halfSide[2];
52

202
  result[0] = tf.transform(Vec3f(a, b, c));
53

202
  result[1] = tf.transform(Vec3f(a, b, -c));
54

202
  result[2] = tf.transform(Vec3f(a, -b, c));
55

202
  result[3] = tf.transform(Vec3f(a, -b, -c));
56

202
  result[4] = tf.transform(Vec3f(-a, b, c));
57

202
  result[5] = tf.transform(Vec3f(-a, b, -c));
58

202
  result[6] = tf.transform(Vec3f(-a, -b, c));
59

202
  result[7] = tf.transform(Vec3f(-a, -b, -c));
60
61
404
  return result;
62
}
63
64
// we use icosahedron to bound the sphere
65
17
std::vector<Vec3f> getBoundVertices(const Sphere& sphere,
66
                                    const Transform3f& tf) {
67
17
  std::vector<Vec3f> result(12);
68
17
  const FCL_REAL m = (1 + sqrt(5.0)) / 2.0;
69
17
  FCL_REAL edge_size = sphere.radius * 6 / (sqrt(27.0) + sqrt(15.0));
70
71
17
  FCL_REAL a = edge_size;
72
17
  FCL_REAL b = m * edge_size;
73

17
  result[0] = tf.transform(Vec3f(0, a, b));
74

17
  result[1] = tf.transform(Vec3f(0, -a, b));
75

17
  result[2] = tf.transform(Vec3f(0, a, -b));
76

17
  result[3] = tf.transform(Vec3f(0, -a, -b));
77

17
  result[4] = tf.transform(Vec3f(a, b, 0));
78

17
  result[5] = tf.transform(Vec3f(-a, b, 0));
79

17
  result[6] = tf.transform(Vec3f(a, -b, 0));
80

17
  result[7] = tf.transform(Vec3f(-a, -b, 0));
81

17
  result[8] = tf.transform(Vec3f(b, 0, a));
82

17
  result[9] = tf.transform(Vec3f(b, 0, -a));
83

17
  result[10] = tf.transform(Vec3f(-b, 0, a));
84

17
  result[11] = tf.transform(Vec3f(-b, 0, -a));
85
86
34
  return result;
87
}
88
89
// we use scaled icosahedron to bound the ellipsoid
90
std::vector<Vec3f> getBoundVertices(const Ellipsoid& ellipsoid,
91
                                    const Transform3f& tf) {
92
  std::vector<Vec3f> result(12);
93
  const FCL_REAL phi = (1 + sqrt(5.0)) / 2.0;
94
95
  const FCL_REAL a = sqrt(3.0) / (phi * phi);
96
  const FCL_REAL b = phi * a;
97
98
  const FCL_REAL& A = ellipsoid.radii[0];
99
  const FCL_REAL& B = ellipsoid.radii[1];
100
  const FCL_REAL& C = ellipsoid.radii[2];
101
102
  FCL_REAL Aa = A * a;
103
  FCL_REAL Ab = A * b;
104
  FCL_REAL Ba = B * a;
105
  FCL_REAL Bb = B * b;
106
  FCL_REAL Ca = C * a;
107
  FCL_REAL Cb = C * b;
108
  result[0] = tf.transform(Vec3f(0, Ba, Cb));
109
  result[1] = tf.transform(Vec3f(0, -Ba, Cb));
110
  result[2] = tf.transform(Vec3f(0, Ba, -Cb));
111
  result[3] = tf.transform(Vec3f(0, -Ba, -Cb));
112
  result[4] = tf.transform(Vec3f(Aa, Bb, 0));
113
  result[5] = tf.transform(Vec3f(-Aa, Bb, 0));
114
  result[6] = tf.transform(Vec3f(Aa, -Bb, 0));
115
  result[7] = tf.transform(Vec3f(-Aa, -Bb, 0));
116
  result[8] = tf.transform(Vec3f(Ab, 0, Ca));
117
  result[9] = tf.transform(Vec3f(Ab, 0, -Ca));
118
  result[10] = tf.transform(Vec3f(-Ab, 0, Ca));
119
  result[11] = tf.transform(Vec3f(-Ab, 0, -Ca));
120
121
  return result;
122
}
123
124
2
std::vector<Vec3f> getBoundVertices(const Capsule& capsule,
125
                                    const Transform3f& tf) {
126
2
  std::vector<Vec3f> result(36);
127
2
  const FCL_REAL m = (1 + sqrt(5.0)) / 2.0;
128
129
2
  FCL_REAL hl = capsule.halfLength;
130
2
  FCL_REAL edge_size = capsule.radius * 6 / (sqrt(27.0) + sqrt(15.0));
131
2
  FCL_REAL a = edge_size;
132
2
  FCL_REAL b = m * edge_size;
133
2
  FCL_REAL r2 = capsule.radius * 2 / sqrt(3.0);
134
135

2
  result[0] = tf.transform(Vec3f(0, a, b + hl));
136

2
  result[1] = tf.transform(Vec3f(0, -a, b + hl));
137

2
  result[2] = tf.transform(Vec3f(0, a, -b + hl));
138

2
  result[3] = tf.transform(Vec3f(0, -a, -b + hl));
139

2
  result[4] = tf.transform(Vec3f(a, b, hl));
140

2
  result[5] = tf.transform(Vec3f(-a, b, hl));
141

2
  result[6] = tf.transform(Vec3f(a, -b, hl));
142

2
  result[7] = tf.transform(Vec3f(-a, -b, hl));
143

2
  result[8] = tf.transform(Vec3f(b, 0, a + hl));
144

2
  result[9] = tf.transform(Vec3f(b, 0, -a + hl));
145

2
  result[10] = tf.transform(Vec3f(-b, 0, a + hl));
146

2
  result[11] = tf.transform(Vec3f(-b, 0, -a + hl));
147
148

2
  result[12] = tf.transform(Vec3f(0, a, b - hl));
149

2
  result[13] = tf.transform(Vec3f(0, -a, b - hl));
150

2
  result[14] = tf.transform(Vec3f(0, a, -b - hl));
151

2
  result[15] = tf.transform(Vec3f(0, -a, -b - hl));
152

2
  result[16] = tf.transform(Vec3f(a, b, -hl));
153

2
  result[17] = tf.transform(Vec3f(-a, b, -hl));
154

2
  result[18] = tf.transform(Vec3f(a, -b, -hl));
155

2
  result[19] = tf.transform(Vec3f(-a, -b, -hl));
156

2
  result[20] = tf.transform(Vec3f(b, 0, a - hl));
157

2
  result[21] = tf.transform(Vec3f(b, 0, -a - hl));
158

2
  result[22] = tf.transform(Vec3f(-b, 0, a - hl));
159

2
  result[23] = tf.transform(Vec3f(-b, 0, -a - hl));
160
161
2
  FCL_REAL c = 0.5 * r2;
162
2
  FCL_REAL d = capsule.radius;
163

2
  result[24] = tf.transform(Vec3f(r2, 0, hl));
164

2
  result[25] = tf.transform(Vec3f(c, d, hl));
165

2
  result[26] = tf.transform(Vec3f(-c, d, hl));
166

2
  result[27] = tf.transform(Vec3f(-r2, 0, hl));
167

2
  result[28] = tf.transform(Vec3f(-c, -d, hl));
168

2
  result[29] = tf.transform(Vec3f(c, -d, hl));
169
170

2
  result[30] = tf.transform(Vec3f(r2, 0, -hl));
171

2
  result[31] = tf.transform(Vec3f(c, d, -hl));
172

2
  result[32] = tf.transform(Vec3f(-c, d, -hl));
173

2
  result[33] = tf.transform(Vec3f(-r2, 0, -hl));
174

2
  result[34] = tf.transform(Vec3f(-c, -d, -hl));
175

2
  result[35] = tf.transform(Vec3f(c, -d, -hl));
176
177
4
  return result;
178
}
179
180
2
std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf) {
181
2
  std::vector<Vec3f> result(7);
182
183
2
  FCL_REAL hl = cone.halfLength;
184
2
  FCL_REAL r2 = cone.radius * 2 / sqrt(3.0);
185
2
  FCL_REAL a = 0.5 * r2;
186
2
  FCL_REAL b = cone.radius;
187
188

2
  result[0] = tf.transform(Vec3f(r2, 0, -hl));
189

2
  result[1] = tf.transform(Vec3f(a, b, -hl));
190

2
  result[2] = tf.transform(Vec3f(-a, b, -hl));
191

2
  result[3] = tf.transform(Vec3f(-r2, 0, -hl));
192

2
  result[4] = tf.transform(Vec3f(-a, -b, -hl));
193

2
  result[5] = tf.transform(Vec3f(a, -b, -hl));
194
195

2
  result[6] = tf.transform(Vec3f(0, 0, hl));
196
197
4
  return result;
198
}
199
200
5
std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder,
201
                                    const Transform3f& tf) {
202
5
  std::vector<Vec3f> result(12);
203
204
5
  FCL_REAL hl = cylinder.halfLength;
205
5
  FCL_REAL r2 = cylinder.radius * 2 / sqrt(3.0);
206
5
  FCL_REAL a = 0.5 * r2;
207
5
  FCL_REAL b = cylinder.radius;
208
209

5
  result[0] = tf.transform(Vec3f(r2, 0, -hl));
210

5
  result[1] = tf.transform(Vec3f(a, b, -hl));
211

5
  result[2] = tf.transform(Vec3f(-a, b, -hl));
212

5
  result[3] = tf.transform(Vec3f(-r2, 0, -hl));
213

5
  result[4] = tf.transform(Vec3f(-a, -b, -hl));
214

5
  result[5] = tf.transform(Vec3f(a, -b, -hl));
215
216

5
  result[6] = tf.transform(Vec3f(r2, 0, hl));
217

5
  result[7] = tf.transform(Vec3f(a, b, hl));
218

5
  result[8] = tf.transform(Vec3f(-a, b, hl));
219

5
  result[9] = tf.transform(Vec3f(-r2, 0, hl));
220

5
  result[10] = tf.transform(Vec3f(-a, -b, hl));
221

5
  result[11] = tf.transform(Vec3f(a, -b, hl));
222
223
10
  return result;
224
}
225
226
std::vector<Vec3f> getBoundVertices(const ConvexBase& convex,
227
                                    const Transform3f& tf) {
228
  std::vector<Vec3f> result(convex.num_points);
229
  for (std::size_t i = 0; i < convex.num_points; ++i) {
230
    result[i] = tf.transform(convex.points[i]);
231
  }
232
233
  return result;
234
}
235
236
std::vector<Vec3f> getBoundVertices(const TriangleP& triangle,
237
                                    const Transform3f& tf) {
238
  std::vector<Vec3f> result(3);
239
  result[0] = tf.transform(triangle.a);
240
  result[1] = tf.transform(triangle.b);
241
  result[2] = tf.transform(triangle.c);
242
243
  return result;
244
}
245
246
}  // namespace details
247
248
234
Halfspace transform(const Halfspace& a, const Transform3f& tf) {
249
  /// suppose the initial halfspace is n * x <= d
250
  /// after transform (R, T), x --> x' = R x + T
251
  /// and the new half space becomes n' * x' <= d'
252
  /// where n' = R * n
253
  ///   and d' = d + n' * T
254
255

234
  Vec3f n = tf.getRotation() * a.n;
256
234
  FCL_REAL d = a.d + n.dot(tf.getTranslation());
257
258
468
  return Halfspace(n, d);
259
}
260
261
233
Plane transform(const Plane& a, const Transform3f& tf) {
262
  /// suppose the initial halfspace is n * x <= d
263
  /// after transform (R, T), x --> x' = R x + T
264
  /// and the new half space becomes n' * x' <= d'
265
  /// where n' = R * n
266
  ///   and d' = d + n' * T
267
268

233
  Vec3f n = tf.getRotation() * a.n;
269
233
  FCL_REAL d = a.d + n.dot(tf.getTranslation());
270
271
466
  return Plane(n, d);
272
}
273
274
template <>
275
14121
void computeBV<AABB, Box>(const Box& s, const Transform3f& tf, AABB& bv) {
276
14121
  const Matrix3f& R = tf.getRotation();
277
14121
  const Vec3f& T = tf.getTranslation();
278
279

14121
  Vec3f v_delta(R.cwiseAbs() * s.halfSide);
280

14121
  bv.max_ = T + v_delta;
281

14121
  bv.min_ = T - v_delta;
282
14121
}
283
284
template <>
285
16942
void computeBV<AABB, Sphere>(const Sphere& s, const Transform3f& tf, AABB& bv) {
286
16942
  const Vec3f& T = tf.getTranslation();
287
288

16942
  Vec3f v_delta(Vec3f::Constant(s.radius));
289

16942
  bv.max_ = T + v_delta;
290

16942
  bv.min_ = T - v_delta;
291
16942
}
292
293
template <>
294
void computeBV<AABB, Ellipsoid>(const Ellipsoid& e, const Transform3f& tf,
295
                                AABB& bv) {
296
  const Matrix3f& R = tf.getRotation();
297
  const Vec3f& T = tf.getTranslation();
298
299
  Vec3f v_delta = R * e.radii;
300
  bv.max_ = T + v_delta;
301
  bv.min_ = T - v_delta;
302
}
303
304
template <>
305
10010
void computeBV<AABB, Capsule>(const Capsule& s, const Transform3f& tf,
306
                              AABB& bv) {
307
10010
  const Matrix3f& R = tf.getRotation();
308
10010
  const Vec3f& T = tf.getTranslation();
309
310



10010
  Vec3f v_delta(R.col(2).cwiseAbs() * s.halfLength + Vec3f::Constant(s.radius));
311

10010
  bv.max_ = T + v_delta;
312

10010
  bv.min_ = T - v_delta;
313
10010
}
314
315
template <>
316
void computeBV<AABB, Cone>(const Cone& s, const Transform3f& tf, AABB& bv) {
317
  const Matrix3f& R = tf.getRotation();
318
  const Vec3f& T = tf.getTranslation();
319
320
  FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) +
321
                     fabs(R(0, 2) * s.halfLength);
322
  FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) +
323
                     fabs(R(1, 2) * s.halfLength);
324
  FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) +
325
                     fabs(R(2, 2) * s.halfLength);
326
327
  Vec3f v_delta(x_range, y_range, z_range);
328
  bv.max_ = T + v_delta;
329
  bv.min_ = T - v_delta;
330
}
331
332
template <>
333
8912
void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3f& tf,
334
                               AABB& bv) {
335
8912
  const Matrix3f& R = tf.getRotation();
336
8912
  const Vec3f& T = tf.getTranslation();
337
338

8912
  FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) +
339
8912
                     fabs(R(0, 2) * s.halfLength);
340

8912
  FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) +
341
8912
                     fabs(R(1, 2) * s.halfLength);
342

8912
  FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) +
343
8912
                     fabs(R(2, 2) * s.halfLength);
344
345
8912
  Vec3f v_delta(x_range, y_range, z_range);
346

8912
  bv.max_ = T + v_delta;
347

8912
  bv.min_ = T - v_delta;
348
8912
}
349
350
template <>
351
1
void computeBV<AABB, ConvexBase>(const ConvexBase& s, const Transform3f& tf,
352
                                 AABB& bv) {
353
1
  const Matrix3f& R = tf.getRotation();
354
1
  const Vec3f& T = tf.getTranslation();
355
356
1
  AABB bv_;
357
9
  for (std::size_t i = 0; i < s.num_points; ++i) {
358

8
    Vec3f new_p = R * s.points[i] + T;
359
8
    bv_ += new_p;
360
  }
361
362
1
  bv = bv_;
363
1
}
364
365
template <>
366
void computeBV<AABB, TriangleP>(const TriangleP& s, const Transform3f& tf,
367
                                AABB& bv) {
368
  bv = AABB(tf.transform(s.a), tf.transform(s.b), tf.transform(s.c));
369
}
370
371
template <>
372
void computeBV<AABB, Halfspace>(const Halfspace& s, const Transform3f& tf,
373
                                AABB& bv) {
374
  Halfspace new_s = transform(s, tf);
375
  const Vec3f& n = new_s.n;
376
  const FCL_REAL& d = new_s.d;
377
378
  AABB bv_;
379
  bv_.min_ = Vec3f::Constant(-(std::numeric_limits<FCL_REAL>::max)());
380
  bv_.max_ = Vec3f::Constant((std::numeric_limits<FCL_REAL>::max)());
381
  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
382
    // normal aligned with x axis
383
    if (n[0] < 0)
384
      bv_.min_[0] = -d;
385
    else if (n[0] > 0)
386
      bv_.max_[0] = d;
387
  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
388
    // normal aligned with y axis
389
    if (n[1] < 0)
390
      bv_.min_[1] = -d;
391
    else if (n[1] > 0)
392
      bv_.max_[1] = d;
393
  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
394
    // normal aligned with z axis
395
    if (n[2] < 0)
396
      bv_.min_[2] = -d;
397
    else if (n[2] > 0)
398
      bv_.max_[2] = d;
399
  }
400
401
  bv = bv_;
402
}
403
404
template <>
405
void computeBV<AABB, Plane>(const Plane& s, const Transform3f& tf, AABB& bv) {
406
  Plane new_s = transform(s, tf);
407
  const Vec3f& n = new_s.n;
408
  const FCL_REAL& d = new_s.d;
409
410
  AABB bv_;
411
  bv_.min_ = Vec3f::Constant(-(std::numeric_limits<FCL_REAL>::max)());
412
  bv_.max_ = Vec3f::Constant((std::numeric_limits<FCL_REAL>::max)());
413
  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
414
    // normal aligned with x axis
415
    if (n[0] < 0) {
416
      bv_.min_[0] = bv_.max_[0] = -d;
417
    } else if (n[0] > 0) {
418
      bv_.min_[0] = bv_.max_[0] = d;
419
    }
420
  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
421
    // normal aligned with y axis
422
    if (n[1] < 0) {
423
      bv_.min_[1] = bv_.max_[1] = -d;
424
    } else if (n[1] > 0) {
425
      bv_.min_[1] = bv_.max_[1] = d;
426
    }
427
  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
428
    // normal aligned with z axis
429
    if (n[2] < 0) {
430
      bv_.min_[2] = bv_.max_[2] = -d;
431
    } else if (n[2] > 0) {
432
      bv_.min_[2] = bv_.max_[2] = d;
433
    }
434
  }
435
436
  bv = bv_;
437
}
438
439
template <>
440
2
void computeBV<OBB, Box>(const Box& s, const Transform3f& tf, OBB& bv) {
441
2
  const Matrix3f& R = tf.getRotation();
442
2
  const Vec3f& T = tf.getTranslation();
443
444
2
  bv.To = T;
445
2
  bv.axes = R;
446
2
  bv.extent = s.halfSide;
447
2
}
448
449
template <>
450
1002
void computeBV<OBB, Sphere>(const Sphere& s, const Transform3f& tf, OBB& bv) {
451
1002
  const Vec3f& T = tf.getTranslation();
452
453
1002
  bv.To.noalias() = T;
454
1002
  bv.axes.setIdentity();
455
1002
  bv.extent.setConstant(s.radius);
456
1002
}
457
458
template <>
459
1002
void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv) {
460
1002
  const Matrix3f& R = tf.getRotation();
461
1002
  const Vec3f& T = tf.getTranslation();
462
463
1002
  bv.To.noalias() = T;
464
1002
  bv.axes.noalias() = R;
465

1002
  bv.extent << s.radius, s.radius, s.halfLength + s.radius;
466
1002
}
467
468
template <>
469
1002
void computeBV<OBB, Cone>(const Cone& s, const Transform3f& tf, OBB& bv) {
470
1002
  const Matrix3f& R = tf.getRotation();
471
1002
  const Vec3f& T = tf.getTranslation();
472
473
1002
  bv.To.noalias() = T;
474
1002
  bv.axes.noalias() = R;
475

1002
  bv.extent << s.radius, s.radius, s.halfLength;
476
1002
}
477
478
template <>
479
1003
void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3f& tf,
480
                              OBB& bv) {
481
1003
  const Matrix3f& R = tf.getRotation();
482
1003
  const Vec3f& T = tf.getTranslation();
483
484
1003
  bv.To.noalias() = T;
485
1003
  bv.axes.noalias() = R;
486

1003
  bv.extent << s.radius, s.radius, s.halfLength;
487
1003
}
488
489
template <>
490
void computeBV<OBB, ConvexBase>(const ConvexBase& s, const Transform3f& tf,
491
                                OBB& bv) {
492
  const Matrix3f& R = tf.getRotation();
493
  const Vec3f& T = tf.getTranslation();
494
495
  fit(s.points, s.num_points, bv);
496
497
  bv.axes.applyOnTheLeft(R);
498
499
  bv.To = R * bv.To + T;
500
}
501
502
template <>
503
void computeBV<OBB, Halfspace>(const Halfspace&, const Transform3f&, OBB& bv) {
504
  /// Half space can only have very rough OBB
505
  bv.axes.setIdentity();
506
  bv.To.setZero();
507
  bv.extent.setConstant(((std::numeric_limits<FCL_REAL>::max)()));
508
}
509
510
template <>
511
void computeBV<RSS, Halfspace>(const Halfspace&, const Transform3f&, RSS& bv) {
512
  /// Half space can only have very rough RSS
513
  bv.axes.setIdentity();
514
  bv.Tr.setZero();
515
  bv.length[0] = bv.length[1] = bv.radius =
516
      (std::numeric_limits<FCL_REAL>::max)();
517
}
518
519
template <>
520
void computeBV<OBBRSS, Halfspace>(const Halfspace& s, const Transform3f& tf,
521
                                  OBBRSS& bv) {
522
  computeBV<OBB, Halfspace>(s, tf, bv.obb);
523
  computeBV<RSS, Halfspace>(s, tf, bv.rss);
524
}
525
526
template <>
527
void computeBV<kIOS, Halfspace>(const Halfspace& s, const Transform3f& tf,
528
                                kIOS& bv) {
529
  bv.num_spheres = 1;
530
  computeBV<OBB, Halfspace>(s, tf, bv.obb);
531
  bv.spheres[0].o = Vec3f();
532
  bv.spheres[0].r = (std::numeric_limits<FCL_REAL>::max)();
533
}
534
535
template <>
536
void computeBV<KDOP<16>, Halfspace>(const Halfspace& s, const Transform3f& tf,
537
                                    KDOP<16>& bv) {
538
  Halfspace new_s = transform(s, tf);
539
  const Vec3f& n = new_s.n;
540
  const FCL_REAL& d = new_s.d;
541
542
  const short D = 8;
543
  for (short i = 0; i < D; ++i)
544
    bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)();
545
  for (short i = D; i < 2 * D; ++i)
546
    bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
547
548
  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
549
    if (n[0] > 0)
550
      bv.dist(D) = d;
551
    else
552
      bv.dist(0) = -d;
553
  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
554
    if (n[1] > 0)
555
      bv.dist(D + 1) = d;
556
    else
557
      bv.dist(1) = -d;
558
  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
559
    if (n[2] > 0)
560
      bv.dist(D + 2) = d;
561
    else
562
      bv.dist(2) = -d;
563
  } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) {
564
    if (n[0] > 0)
565
      bv.dist(D + 3) = n[0] * d * 2;
566
    else
567
      bv.dist(3) = n[0] * d * 2;
568
  } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) {
569
    if (n[1] > 0)
570
      bv.dist(D + 4) = n[0] * d * 2;
571
    else
572
      bv.dist(4) = n[0] * d * 2;
573
  } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) {
574
    if (n[1] > 0)
575
      bv.dist(D + 5) = n[1] * d * 2;
576
    else
577
      bv.dist(5) = n[1] * d * 2;
578
  } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
579
    if (n[0] > 0)
580
      bv.dist(D + 6) = n[0] * d * 2;
581
    else
582
      bv.dist(6) = n[0] * d * 2;
583
  } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
584
    if (n[0] > 0)
585
      bv.dist(D + 7) = n[0] * d * 2;
586
    else
587
      bv.dist(7) = n[0] * d * 2;
588
  }
589
}
590
591
template <>
592
void computeBV<KDOP<18>, Halfspace>(const Halfspace& s, const Transform3f& tf,
593
                                    KDOP<18>& bv) {
594
  Halfspace new_s = transform(s, tf);
595
  const Vec3f& n = new_s.n;
596
  const FCL_REAL& d = new_s.d;
597
598
  const short D = 9;
599
600
  for (short i = 0; i < D; ++i)
601
    bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)();
602
  for (short i = D; i < 2 * D; ++i)
603
    bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
604
605
  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
606
    if (n[0] > 0)
607
      bv.dist(D) = d;
608
    else
609
      bv.dist(0) = -d;
610
  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
611
    if (n[1] > 0)
612
      bv.dist(D + 1) = d;
613
    else
614
      bv.dist(1) = -d;
615
  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
616
    if (n[2] > 0)
617
      bv.dist(D + 2) = d;
618
    else
619
      bv.dist(2) = -d;
620
  } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) {
621
    if (n[0] > 0)
622
      bv.dist(D + 3) = n[0] * d * 2;
623
    else
624
      bv.dist(3) = n[0] * d * 2;
625
  } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) {
626
    if (n[1] > 0)
627
      bv.dist(D + 4) = n[0] * d * 2;
628
    else
629
      bv.dist(4) = n[0] * d * 2;
630
  } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) {
631
    if (n[1] > 0)
632
      bv.dist(D + 5) = n[1] * d * 2;
633
    else
634
      bv.dist(5) = n[1] * d * 2;
635
  } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
636
    if (n[0] > 0)
637
      bv.dist(D + 6) = n[0] * d * 2;
638
    else
639
      bv.dist(6) = n[0] * d * 2;
640
  } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
641
    if (n[0] > 0)
642
      bv.dist(D + 7) = n[0] * d * 2;
643
    else
644
      bv.dist(7) = n[0] * d * 2;
645
  } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) {
646
    if (n[1] > 0)
647
      bv.dist(D + 8) = n[1] * d * 2;
648
    else
649
      bv.dist(8) = n[1] * d * 2;
650
  }
651
}
652
653
template <>
654
void computeBV<KDOP<24>, Halfspace>(const Halfspace& s, const Transform3f& tf,
655
                                    KDOP<24>& bv) {
656
  Halfspace new_s = transform(s, tf);
657
  const Vec3f& n = new_s.n;
658
  const FCL_REAL& d = new_s.d;
659
660
  const short D = 12;
661
662
  for (short i = 0; i < D; ++i)
663
    bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)();
664
  for (short i = D; i < 2 * D; ++i)
665
    bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
666
667
  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
668
    if (n[0] > 0)
669
      bv.dist(D) = d;
670
    else
671
      bv.dist(0) = -d;
672
  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
673
    if (n[1] > 0)
674
      bv.dist(D + 1) = d;
675
    else
676
      bv.dist(1) = -d;
677
  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
678
    if (n[2] > 0)
679
      bv.dist(D + 2) = d;
680
    else
681
      bv.dist(2) = -d;
682
  } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) {
683
    if (n[0] > 0)
684
      bv.dist(D + 3) = n[0] * d * 2;
685
    else
686
      bv.dist(3) = n[0] * d * 2;
687
  } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) {
688
    if (n[1] > 0)
689
      bv.dist(D + 4) = n[0] * d * 2;
690
    else
691
      bv.dist(4) = n[0] * d * 2;
692
  } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) {
693
    if (n[1] > 0)
694
      bv.dist(D + 5) = n[1] * d * 2;
695
    else
696
      bv.dist(5) = n[1] * d * 2;
697
  } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
698
    if (n[0] > 0)
699
      bv.dist(D + 6) = n[0] * d * 2;
700
    else
701
      bv.dist(6) = n[0] * d * 2;
702
  } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
703
    if (n[0] > 0)
704
      bv.dist(D + 7) = n[0] * d * 2;
705
    else
706
      bv.dist(7) = n[0] * d * 2;
707
  } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) {
708
    if (n[1] > 0)
709
      bv.dist(D + 8) = n[1] * d * 2;
710
    else
711
      bv.dist(8) = n[1] * d * 2;
712
  } else if (n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
713
    if (n[0] > 0)
714
      bv.dist(D + 9) = n[0] * d * 3;
715
    else
716
      bv.dist(9) = n[0] * d * 3;
717
  } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) {
718
    if (n[0] > 0)
719
      bv.dist(D + 10) = n[0] * d * 3;
720
    else
721
      bv.dist(10) = n[0] * d * 3;
722
  } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
723
    if (n[1] > 0)
724
      bv.dist(D + 11) = n[1] * d * 3;
725
    else
726
      bv.dist(11) = n[1] * d * 3;
727
  }
728
}
729
730
template <>
731
void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv) {
732
  Vec3f n = tf.getRotation() * s.n;
733
  generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2));
734
  bv.axes.col(0).noalias() = n;
735
736
  bv.extent << 0, (std::numeric_limits<FCL_REAL>::max)(),
737
      (std::numeric_limits<FCL_REAL>::max)();
738
739
  Vec3f p = s.n * s.d;
740
  bv.To =
741
      tf.transform(p);  /// n'd' = R * n * (d + (R * n) * T) = R * (n * d) + T
742
}
743
744
template <>
745
void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv) {
746
  Vec3f n = tf.getRotation() * s.n;
747
748
  generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2));
749
  bv.axes.col(0).noalias() = n;
750
751
  bv.length[0] = (std::numeric_limits<FCL_REAL>::max)();
752
  bv.length[1] = (std::numeric_limits<FCL_REAL>::max)();
753
754
  bv.radius = 0;
755
756
  Vec3f p = s.n * s.d;
757
  bv.Tr = tf.transform(p);
758
}
759
760
template <>
761
void computeBV<OBBRSS, Plane>(const Plane& s, const Transform3f& tf,
762
                              OBBRSS& bv) {
763
  computeBV<OBB, Plane>(s, tf, bv.obb);
764
  computeBV<RSS, Plane>(s, tf, bv.rss);
765
}
766
767
template <>
768
void computeBV<kIOS, Plane>(const Plane& s, const Transform3f& tf, kIOS& bv) {
769
  bv.num_spheres = 1;
770
  computeBV<OBB, Plane>(s, tf, bv.obb);
771
  bv.spheres[0].o = Vec3f();
772
  bv.spheres[0].r = (std::numeric_limits<FCL_REAL>::max)();
773
}
774
775
template <>
776
void computeBV<KDOP<16>, Plane>(const Plane& s, const Transform3f& tf,
777
                                KDOP<16>& bv) {
778
  Plane new_s = transform(s, tf);
779
  const Vec3f& n = new_s.n;
780
  const FCL_REAL& d = new_s.d;
781
782
  const short D = 8;
783
784
  for (short i = 0; i < D; ++i)
785
    bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)();
786
  for (short i = D; i < 2 * D; ++i)
787
    bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
788
789
  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
790
    if (n[0] > 0)
791
      bv.dist(0) = bv.dist(D) = d;
792
    else
793
      bv.dist(0) = bv.dist(D) = -d;
794
  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
795
    if (n[1] > 0)
796
      bv.dist(1) = bv.dist(D + 1) = d;
797
    else
798
      bv.dist(1) = bv.dist(D + 1) = -d;
799
  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
800
    if (n[2] > 0)
801
      bv.dist(2) = bv.dist(D + 2) = d;
802
    else
803
      bv.dist(2) = bv.dist(D + 2) = -d;
804
  } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) {
805
    bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
806
  } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) {
807
    bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
808
  } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) {
809
    bv.dist(6) = bv.dist(D + 5) = n[1] * d * 2;
810
  } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
811
    bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
812
  } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
813
    bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
814
  }
815
}
816
817
template <>
818
void computeBV<KDOP<18>, Plane>(const Plane& s, const Transform3f& tf,
819
                                KDOP<18>& bv) {
820
  Plane new_s = transform(s, tf);
821
  const Vec3f& n = new_s.n;
822
  const FCL_REAL& d = new_s.d;
823
824
  const short D = 9;
825
826
  for (short i = 0; i < D; ++i)
827
    bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)();
828
  for (short i = D; i < 2 * D; ++i)
829
    bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
830
831
  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
832
    if (n[0] > 0)
833
      bv.dist(0) = bv.dist(D) = d;
834
    else
835
      bv.dist(0) = bv.dist(D) = -d;
836
  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
837
    if (n[1] > 0)
838
      bv.dist(1) = bv.dist(D + 1) = d;
839
    else
840
      bv.dist(1) = bv.dist(D + 1) = -d;
841
  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
842
    if (n[2] > 0)
843
      bv.dist(2) = bv.dist(D + 2) = d;
844
    else
845
      bv.dist(2) = bv.dist(D + 2) = -d;
846
  } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) {
847
    bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
848
  } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) {
849
    bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
850
  } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) {
851
    bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2;
852
  } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
853
    bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
854
  } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
855
    bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
856
  } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) {
857
    bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2;
858
  }
859
}
860
861
template <>
862
void computeBV<KDOP<24>, Plane>(const Plane& s, const Transform3f& tf,
863
                                KDOP<24>& bv) {
864
  Plane new_s = transform(s, tf);
865
  const Vec3f& n = new_s.n;
866
  const FCL_REAL& d = new_s.d;
867
868
  const short D = 12;
869
870
  for (short i = 0; i < D; ++i)
871
    bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)();
872
  for (short i = D; i < 2 * D; ++i)
873
    bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
874
875
  if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
876
    if (n[0] > 0)
877
      bv.dist(0) = bv.dist(D) = d;
878
    else
879
      bv.dist(0) = bv.dist(D) = -d;
880
  } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
881
    if (n[1] > 0)
882
      bv.dist(1) = bv.dist(D + 1) = d;
883
    else
884
      bv.dist(1) = bv.dist(D + 1) = -d;
885
  } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) {
886
    if (n[2] > 0)
887
      bv.dist(2) = bv.dist(D + 2) = d;
888
    else
889
      bv.dist(2) = bv.dist(D + 2) = -d;
890
  } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) {
891
    bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
892
  } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) {
893
    bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
894
  } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) {
895
    bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2;
896
  } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
897
    bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
898
  } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
899
    bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
900
  } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) {
901
    bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2;
902
  } else if (n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) {
903
    bv.dist(9) = bv.dist(D + 9) = n[0] * d * 3;
904
  } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) {
905
    bv.dist(10) = bv.dist(D + 10) = n[0] * d * 3;
906
  } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) {
907
    bv.dist(11) = bv.dist(D + 11) = n[1] * d * 3;
908
  }
909
}
910
911
40
void constructBox(const AABB& bv, Box& box, Transform3f& tf) {
912

40
  box = Box(bv.max_ - bv.min_);
913

40
  tf = Transform3f(bv.center());
914
40
}
915
916
void constructBox(const OBB& bv, Box& box, Transform3f& tf) {
917
  box = Box(bv.extent * 2);
918
  tf = Transform3f(bv.axes, bv.To);
919
}
920
921
void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf) {
922
  box = Box(bv.obb.extent * 2);
923
  tf = Transform3f(bv.obb.axes, bv.obb.To);
924
}
925
926
void constructBox(const kIOS& bv, Box& box, Transform3f& tf) {
927
  box = Box(bv.obb.extent * 2);
928
  tf = Transform3f(bv.obb.axes, bv.obb.To);
929
}
930
931
void constructBox(const RSS& bv, Box& box, Transform3f& tf) {
932
  box = Box(bv.width(), bv.height(), bv.depth());
933
  tf = Transform3f(bv.axes, bv.Tr);
934
}
935
936
void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf) {
937
  box = Box(bv.width(), bv.height(), bv.depth());
938
  tf = Transform3f(bv.center());
939
}
940
941
void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf) {
942
  box = Box(bv.width(), bv.height(), bv.depth());
943
  tf = Transform3f(bv.center());
944
}
945
946
void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf) {
947
  box = Box(bv.width(), bv.height(), bv.depth());
948
  tf = Transform3f(bv.center());
949
}
950
951
1541
void constructBox(const AABB& bv, const Transform3f& tf_bv, Box& box,
952
                  Transform3f& tf) {
953

1541
  box = Box(bv.max_ - bv.min_);
954

1541
  tf = tf_bv * Transform3f(bv.center());
955
1541
}
956
957
void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box,
958
                  Transform3f& tf) {
959
  box = Box(bv.extent * 2);
960
  tf = tf_bv * Transform3f(bv.axes, bv.To);
961
}
962
963
void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, Box& box,
964
                  Transform3f& tf) {
965
  box = Box(bv.obb.extent * 2);
966
  tf = tf_bv * Transform3f(bv.obb.axes, bv.obb.To);
967
}
968
969
void constructBox(const kIOS& bv, const Transform3f& tf_bv, Box& box,
970
                  Transform3f& tf) {
971
  box = Box(bv.obb.extent * 2);
972
  tf = tf_bv * Transform3f(bv.obb.axes, bv.obb.To);
973
}
974
975
void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box,
976
                  Transform3f& tf) {
977
  box = Box(bv.width(), bv.height(), bv.depth());
978
  tf = tf_bv * Transform3f(bv.axes, bv.Tr);
979
}
980
981
void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, Box& box,
982
                  Transform3f& tf) {
983
  box = Box(bv.width(), bv.height(), bv.depth());
984
  tf = tf_bv * Transform3f(bv.center());
985
}
986
987
void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, Box& box,
988
                  Transform3f& tf) {
989
  box = Box(bv.width(), bv.height(), bv.depth());
990
  tf = tf_bv * Transform3f(bv.center());
991
}
992
993
void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, Box& box,
994
                  Transform3f& tf) {
995
  box = Box(bv.width(), bv.height(), bv.depth());
996
  tf = tf_bv * Transform3f(bv.center());
997
}
998
999
}  // namespace fcl
1000
1001
}  // namespace hpp