geometric_shapes_utility.cpp 28.8 KB
Newer Older
sachinc's avatar
sachinc committed
1
2
3
/*
 * Software License Agreement (BSD License)
 *
4
5
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
sachinc's avatar
sachinc committed
6
7
8
9
10
11
12
13
14
15
16
17
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
18
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
sachinc's avatar
sachinc committed
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

/** \author Jia Pan */


Lucile Remigy's avatar
Lucile Remigy committed
39
#include <hpp/fcl/shape/geometric_shapes_utility.h>
40
41
#include <hpp/fcl/internal/BV_fitter.h>
#include <hpp/fcl/internal/tools.h>
sachinc's avatar
sachinc committed
42

43
44
namespace hpp
{
sachinc's avatar
sachinc committed
45
46
47
namespace fcl
{

jpan's avatar
   
jpan committed
48
49
50
namespace details
{

51
std::vector<Vec3f> getBoundVertices(const Box& box, const Transform3f& tf)
jpan's avatar
   
jpan committed
52
53
{
  std::vector<Vec3f> result(8);
54
55
56
57
58
59
60
61
62
63
  FCL_REAL a = box.halfSide[0];
  FCL_REAL b = box.halfSide[1];
  FCL_REAL c = box.halfSide[2];
  result[0] = tf.transform(Vec3f( a,  b,  c));
  result[1] = tf.transform(Vec3f( a,  b, -c));
  result[2] = tf.transform(Vec3f( a, -b,  c));
  result[3] = tf.transform(Vec3f( a, -b, -c));
  result[4] = tf.transform(Vec3f(-a,  b,  c));
  result[5] = tf.transform(Vec3f(-a,  b, -c));
  result[6] = tf.transform(Vec3f(-a, -b,  c));
jpan's avatar
   
jpan committed
64
65
66
67
68
69
  result[7] = tf.transform(Vec3f(-a, -b, -c));
  
  return result;
}

// we use icosahedron to bound the sphere
70
std::vector<Vec3f> getBoundVertices(const Sphere& sphere, const Transform3f& tf)
jpan's avatar
   
jpan committed
71
72
{
  std::vector<Vec3f> result(12);
73
74
  const FCL_REAL m = (1 + sqrt(5.0)) / 2.0;
  FCL_REAL edge_size = sphere.radius * 6 / (sqrt(27.0) + sqrt(15.0));
jpan's avatar
   
jpan committed
75
  
76
77
  FCL_REAL a = edge_size;
  FCL_REAL b = m * edge_size;
jpan's avatar
   
jpan committed
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
  result[0] = tf.transform(Vec3f(0, a, b));
  result[1] = tf.transform(Vec3f(0, -a, b));
  result[2] = tf.transform(Vec3f(0, a, -b));
  result[3] = tf.transform(Vec3f(0, -a, -b));
  result[4] = tf.transform(Vec3f(a, b, 0));
  result[5] = tf.transform(Vec3f(-a, b, 0));
  result[6] = tf.transform(Vec3f(a, -b, 0));
  result[7] = tf.transform(Vec3f(-a, -b, 0));
  result[8] = tf.transform(Vec3f(b, 0, a));
  result[9] = tf.transform(Vec3f(b, 0, -a));
  result[10] = tf.transform(Vec3f(-b, 0, a));
  result[11] = tf.transform(Vec3f(-b, 0, -a));

  return result;
}

94
std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const Transform3f& tf)
jpan's avatar
   
jpan committed
95
96
{
  std::vector<Vec3f> result(36);
97
  const FCL_REAL m = (1 + sqrt(5.0)) / 2.0;
jpan's avatar
   
jpan committed
98

99
  FCL_REAL hl = capsule.halfLength;
100
101
102
103
  FCL_REAL edge_size = capsule.radius * 6 / (sqrt(27.0) + sqrt(15.0));
  FCL_REAL a = edge_size;
  FCL_REAL b = m * edge_size;
  FCL_REAL r2 = capsule.radius * 2 / sqrt(3.0);
jpan's avatar
   
jpan committed
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131


  result[0] = tf.transform(Vec3f(0, a, b + hl));
  result[1] = tf.transform(Vec3f(0, -a, b + hl));
  result[2] = tf.transform(Vec3f(0, a, -b + hl));
  result[3] = tf.transform(Vec3f(0, -a, -b + hl));
  result[4] = tf.transform(Vec3f(a, b, hl));
  result[5] = tf.transform(Vec3f(-a, b, hl));
  result[6] = tf.transform(Vec3f(a, -b, hl));
  result[7] = tf.transform(Vec3f(-a, -b, hl));
  result[8] = tf.transform(Vec3f(b, 0, a + hl));
  result[9] = tf.transform(Vec3f(b, 0, -a + hl));
  result[10] = tf.transform(Vec3f(-b, 0, a + hl));
  result[11] = tf.transform(Vec3f(-b, 0, -a + hl));

  result[12] = tf.transform(Vec3f(0, a, b - hl));
  result[13] = tf.transform(Vec3f(0, -a, b - hl));
  result[14] = tf.transform(Vec3f(0, a, -b - hl));
  result[15] = tf.transform(Vec3f(0, -a, -b - hl));
  result[16] = tf.transform(Vec3f(a, b, -hl));
  result[17] = tf.transform(Vec3f(-a, b, -hl));
  result[18] = tf.transform(Vec3f(a, -b, -hl));
  result[19] = tf.transform(Vec3f(-a, -b, -hl));
  result[20] = tf.transform(Vec3f(b, 0, a - hl));
  result[21] = tf.transform(Vec3f(b, 0, -a - hl));
  result[22] = tf.transform(Vec3f(-b, 0, a - hl));
  result[23] = tf.transform(Vec3f(-b, 0, -a - hl));

132
133
  FCL_REAL c = 0.5 * r2;
  FCL_REAL d = capsule.radius;
jpan's avatar
   
jpan committed
134
135
136
  result[24] = tf.transform(Vec3f(r2, 0, hl));
  result[25] = tf.transform(Vec3f(c, d, hl));
  result[26] = tf.transform(Vec3f(-c, d, hl));
jpan's avatar
   
jpan committed
137
  result[27] = tf.transform(Vec3f(-r2, 0, hl));
jpan's avatar
   
jpan committed
138
139
140
141
142
143
  result[28] = tf.transform(Vec3f(-c, -d, hl));
  result[29] = tf.transform(Vec3f(c, -d, hl));

  result[30] = tf.transform(Vec3f(r2, 0, -hl));
  result[31] = tf.transform(Vec3f(c, d, -hl));
  result[32] = tf.transform(Vec3f(-c, d, -hl));
jpan's avatar
   
jpan committed
144
  result[33] = tf.transform(Vec3f(-r2, 0, -hl));
jpan's avatar
   
jpan committed
145
146
147
148
149
150
151
  result[34] = tf.transform(Vec3f(-c, -d, -hl));
  result[35] = tf.transform(Vec3f(c, -d, -hl));

  return result;
}


152
std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf)
jpan's avatar
   
jpan committed
153
154
155
{
  std::vector<Vec3f> result(7);
  
156
  FCL_REAL hl = cone.halfLength;
157
158
159
  FCL_REAL r2 = cone.radius * 2 / sqrt(3.0);
  FCL_REAL a = 0.5 * r2;
  FCL_REAL b = cone.radius;
jpan's avatar
   
jpan committed
160
161

  result[0] = tf.transform(Vec3f(r2, 0, -hl));
jpan's avatar
   
jpan committed
162
163
164
165
166
  result[1] = tf.transform(Vec3f(a, b, -hl));
  result[2] = tf.transform(Vec3f(-a, b, -hl));
  result[3] = tf.transform(Vec3f(-r2, 0, -hl));
  result[4] = tf.transform(Vec3f(-a, -b, -hl));
  result[5] = tf.transform(Vec3f(a, -b, -hl));
jpan's avatar
   
jpan committed
167
168
169
170
171
172

  result[6] = tf.transform(Vec3f(0, 0, hl));
                          
  return result;
}

173
std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, const Transform3f& tf)
jpan's avatar
   
jpan committed
174
{
175
  std::vector<Vec3f> result(12);
jpan's avatar
   
jpan committed
176

177
  FCL_REAL hl = cylinder.halfLength;
178
179
180
  FCL_REAL r2 = cylinder.radius * 2 / sqrt(3.0);
  FCL_REAL a = 0.5 * r2;
  FCL_REAL b = cylinder.radius;
jpan's avatar
   
jpan committed
181
182

  result[0] = tf.transform(Vec3f(r2, 0, -hl));
jpan's avatar
   
jpan committed
183
184
185
186
187
  result[1] = tf.transform(Vec3f(a, b, -hl));
  result[2] = tf.transform(Vec3f(-a, b, -hl));
  result[3] = tf.transform(Vec3f(-r2, 0, -hl));
  result[4] = tf.transform(Vec3f(-a, -b, -hl));
  result[5] = tf.transform(Vec3f(a, -b, -hl));
jpan's avatar
   
jpan committed
188
189

  result[6] = tf.transform(Vec3f(r2, 0, hl));
jpan's avatar
   
jpan committed
190
191
192
193
194
195
196
197
198
  result[7] = tf.transform(Vec3f(a, b, hl));
  result[8] = tf.transform(Vec3f(-a, b, hl));
  result[9] = tf.transform(Vec3f(-r2, 0, hl));
  result[10] = tf.transform(Vec3f(-a, -b, hl));
  result[11] = tf.transform(Vec3f(a, -b, hl));

  return result;
}

199
std::vector<Vec3f> getBoundVertices(const ConvexBase& convex, const Transform3f& tf)
jpan's avatar
   
jpan committed
200
201
202
203
204
205
{
  std::vector<Vec3f> result(convex.num_points);
  for(int i = 0; i < convex.num_points; ++i)
  {
    result[i] = tf.transform(convex.points[i]);
  }
jpan's avatar
   
jpan committed
206
207
208
209

  return result;
}

210
std::vector<Vec3f> getBoundVertices(const TriangleP& triangle, const Transform3f& tf)
211
212
213
214
215
216
217
218
219
{
  std::vector<Vec3f> result(3);
  result[0] = tf.transform(triangle.a);
  result[1] = tf.transform(triangle.b);
  result[2] = tf.transform(triangle.c);

  return result;
}

jpan's avatar
   
jpan committed
220
221
} // end detail

222
223
224
225
226
227
228
229
Halfspace transform(const Halfspace& a, const Transform3f& tf)
{
  /// suppose the initial halfspace is n * x <= d
  /// after transform (R, T), x --> x' = R x + T
  /// and the new half space becomes n' * x' <= d'
  /// where n' = R * n
  ///   and d' = d + n' * T

230
  Vec3f n = tf.getRotation() * a.n;
231
232
233
234
235
236
237
238
239
240
241
242
243
244
  FCL_REAL d = a.d + n.dot(tf.getTranslation());

  return Halfspace(n, d);
}


Plane transform(const Plane& a, const Transform3f& tf)
{
  /// suppose the initial halfspace is n * x <= d
  /// after transform (R, T), x --> x' = R x + T
  /// and the new half space becomes n' * x' <= d'
  /// where n' = R * n
  ///   and d' = d + n' * T

245
  Vec3f n = tf.getRotation() * a.n;
246
247
248
249
250
251
  FCL_REAL d = a.d + n.dot(tf.getTranslation());

  return Plane(n, d);
}


jpan's avatar
   
jpan committed
252

sachinc's avatar
sachinc committed
253
template<>
254
void computeBV<AABB, Box>(const Box& s, const Transform3f& tf, AABB& bv)
sachinc's avatar
sachinc committed
255
{
256
257
  const Matrix3f& R = tf.getRotation();
  const Vec3f& T = tf.getTranslation();
sachinc's avatar
sachinc committed
258

259
  Vec3f v_delta (R.cwiseAbs() * s.halfSide);
260
261
  bv.max_ = T + v_delta;
  bv.min_ = T - v_delta;
sachinc's avatar
sachinc committed
262
263
264
}

template<>
265
void computeBV<AABB, Sphere>(const Sphere& s, const Transform3f& tf, AABB& bv)
sachinc's avatar
sachinc committed
266
{
267
  const Vec3f& T = tf.getTranslation();
268

269
  Vec3f v_delta(Vec3f::Constant(s.radius));
270
271
  bv.max_ = T + v_delta;
  bv.min_ = T - v_delta;
sachinc's avatar
sachinc committed
272
273
274
}

template<>
275
void computeBV<AABB, Capsule>(const Capsule& s, const Transform3f& tf, AABB& bv)
sachinc's avatar
sachinc committed
276
{
277
278
  const Matrix3f& R = tf.getRotation();
  const Vec3f& T = tf.getTranslation();
279

280
  Vec3f v_delta(R.col(2).cwiseAbs() * s.halfLength + Vec3f::Constant(s.radius));
281
282
  bv.max_ = T + v_delta;
  bv.min_ = T - v_delta;
sachinc's avatar
sachinc committed
283
284
285
}

template<>
286
void computeBV<AABB, Cone>(const Cone& s, const Transform3f& tf, AABB& bv)
sachinc's avatar
sachinc committed
287
{
288
289
  const Matrix3f& R = tf.getRotation();
  const Vec3f& T = tf.getTranslation();
sachinc's avatar
sachinc committed
290

291
292
293
  FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + fabs(R(0, 2) * s.halfLength);
  FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + fabs(R(1, 2) * s.halfLength);
  FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + fabs(R(2, 2) * s.halfLength);
294

295
296
297
  Vec3f v_delta(x_range, y_range, z_range);
  bv.max_ = T + v_delta;
  bv.min_ = T - v_delta;
sachinc's avatar
sachinc committed
298
299
300
}

template<>
301
void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3f& tf, AABB& bv)
sachinc's avatar
sachinc committed
302
{
303
304
  const Matrix3f& R = tf.getRotation();
  const Vec3f& T = tf.getTranslation();
305

306
307
308
  FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + fabs(R(0, 2) * s.halfLength);
  FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + fabs(R(1, 2) * s.halfLength);
  FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + fabs(R(2, 2) * s.halfLength);
sachinc's avatar
sachinc committed
309

310
311
312
  Vec3f v_delta(x_range, y_range, z_range);
  bv.max_ = T + v_delta;
  bv.min_ = T - v_delta;
sachinc's avatar
sachinc committed
313
314
315
}

template<>
316
void computeBV<AABB, ConvexBase>(const ConvexBase& s, const Transform3f& tf, AABB& bv)
sachinc's avatar
sachinc committed
317
{
318
319
  const Matrix3f& R = tf.getRotation();
  const Vec3f& T = tf.getTranslation();
320

sachinc's avatar
sachinc committed
321
322
323
  AABB bv_;
  for(int i = 0; i < s.num_points; ++i)
  {
324
    Vec3f new_p = R * s.points[i] + T;
sachinc's avatar
sachinc committed
325
326
327
328
329
330
    bv_ += new_p;
  }

  bv = bv_;
}

331
template<>
332
void computeBV<AABB, TriangleP>(const TriangleP& s, const Transform3f& tf, AABB& bv)
333
{
334
  bv = AABB(tf.transform(s.a), tf.transform(s.b), tf.transform(s.c));
335
336
}

337

sachinc's avatar
sachinc committed
338
template<>
339
void computeBV<AABB, Halfspace>(const Halfspace& s, const Transform3f& tf, AABB& bv)
sachinc's avatar
sachinc committed
340
{
341
342
343
344
345
  Halfspace new_s = transform(s, tf);
  const Vec3f& n = new_s.n;
  const FCL_REAL& d = new_s.d;

  AABB bv_;
346
347
  bv_.min_ = Vec3f::Constant(-std::numeric_limits<FCL_REAL>::max());
  bv_.max_ = Vec3f::Constant(std::numeric_limits<FCL_REAL>::max());
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
  if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
  {
    // normal aligned with x axis
    if(n[0] < 0) bv_.min_[0] = -d;
    else if(n[0] > 0) bv_.max_[0] = d;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
  {
    // normal aligned with y axis
    if(n[1] < 0) bv_.min_[1] = -d;
    else if(n[1] > 0) bv_.max_[1] = d;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
  {
    // normal aligned with z axis
    if(n[2] < 0) bv_.min_[2] = -d;
    else if(n[2] > 0) bv_.max_[2] = d;
  }
366

367
368
369
370
371
372
373
374
375
  bv = bv_;  
}

template<>
void computeBV<AABB, Plane>(const Plane& s, const Transform3f& tf, AABB& bv)
{
  Plane new_s = transform(s, tf);
  const Vec3f& n = new_s.n;
  const FCL_REAL& d = new_s.d;  
376

sachinc's avatar
sachinc committed
377
  AABB bv_;
378
379
  bv_.min_ = Vec3f::Constant(-std::numeric_limits<FCL_REAL>::max());
  bv_.max_ = Vec3f::Constant(std::numeric_limits<FCL_REAL>::max());
380
  if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
sachinc's avatar
sachinc committed
381
382
  {
    // normal aligned with x axis
383
384
    if(n[0] < 0) { bv_.min_[0] = bv_.max_[0] = -d; }
    else if(n[0] > 0) { bv_.min_[0] = bv_.max_[0] = d; }
sachinc's avatar
sachinc committed
385
  }
386
  else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
sachinc's avatar
sachinc committed
387
388
  {
    // normal aligned with y axis
389
390
    if(n[1] < 0) { bv_.min_[1] = bv_.max_[1] = -d; }
    else if(n[1] > 0) { bv_.min_[1] = bv_.max_[1] = d; }
sachinc's avatar
sachinc committed
391
  }
392
  else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
sachinc's avatar
sachinc committed
393
394
  {
    // normal aligned with z axis
395
396
    if(n[2] < 0) { bv_.min_[2] = bv_.max_[2] = -d; }
    else if(n[2] > 0) { bv_.min_[2] = bv_.max_[2] = d; }
sachinc's avatar
sachinc committed
397
398
399
400
401
402
403
  }

  bv = bv_;
}


template<>
404
void computeBV<OBB, Box>(const Box& s, const Transform3f& tf, OBB& bv)
sachinc's avatar
sachinc committed
405
{
406
407
  const Matrix3f& R = tf.getRotation();
  const Vec3f& T = tf.getTranslation();
408

409
410
411
  bv.To = T;
  bv.axes = R;
  bv.extent = s.halfSide;
sachinc's avatar
sachinc committed
412
413
414
}

template<>
415
void computeBV<OBB, Sphere>(const Sphere& s, const Transform3f& tf, OBB& bv)
sachinc's avatar
sachinc committed
416
{
417
  const Vec3f& T = tf.getTranslation();
418

419
  bv.To.noalias() = T;
Joseph Mirabel's avatar
Joseph Mirabel committed
420
  bv.axes.setIdentity();
421
  bv.extent.setConstant(s.radius);
sachinc's avatar
sachinc committed
422
423
424
}

template<>
425
void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv)
sachinc's avatar
sachinc committed
426
{
427
428
  const Matrix3f& R = tf.getRotation();
  const Vec3f& T = tf.getTranslation();
429

430
  bv.To.noalias() = T;
Joseph Mirabel's avatar
Joseph Mirabel committed
431
  bv.axes.noalias() = R;
432
  bv.extent << s.radius, s.radius, s.halfLength + s.radius;
sachinc's avatar
sachinc committed
433
434
435
}

template<>
436
void computeBV<OBB, Cone>(const Cone& s, const Transform3f& tf, OBB& bv)
sachinc's avatar
sachinc committed
437
{
438
439
  const Matrix3f& R = tf.getRotation();
  const Vec3f& T = tf.getTranslation();
440

441
  bv.To.noalias() = T;
Joseph Mirabel's avatar
Joseph Mirabel committed
442
  bv.axes.noalias() = R;
443
  bv.extent << s.radius, s.radius, s.halfLength;
sachinc's avatar
sachinc committed
444
445
446
}

template<>
447
void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3f& tf, OBB& bv)
sachinc's avatar
sachinc committed
448
{
449
450
  const Matrix3f& R = tf.getRotation();
  const Vec3f& T = tf.getTranslation();
451

452
  bv.To.noalias() = T;
Joseph Mirabel's avatar
Joseph Mirabel committed
453
  bv.axes.noalias() = R;
454
  bv.extent << s.radius, s.radius, s.halfLength;
sachinc's avatar
sachinc committed
455
456
457
}

template<>
458
void computeBV<OBB, ConvexBase>(const ConvexBase& s, const Transform3f& tf, OBB& bv)
sachinc's avatar
sachinc committed
459
{
460
461
  const Matrix3f& R = tf.getRotation();
  const Vec3f& T = tf.getTranslation();
462

sachinc's avatar
sachinc committed
463
464
  fit(s.points, s.num_points, bv);

465
  bv.axes.applyOnTheLeft(R);
sachinc's avatar
sachinc committed
466

467
  bv.To = R * bv.To + T;
sachinc's avatar
sachinc committed
468
469
470
}

template<>
471
void computeBV<OBB, Halfspace>(const Halfspace&, const Transform3f&, OBB& bv)
472
473
{
  /// Half space can only have very rough OBB
Joseph Mirabel's avatar
Joseph Mirabel committed
474
475
  bv.axes.setIdentity();
  bv.To.setZero();
476
  bv.extent.setConstant(std::numeric_limits<FCL_REAL>::max());
477
478
479
}

template<>
480
void computeBV<RSS, Halfspace>(const Halfspace&, const Transform3f&, RSS& bv)
481
482
{
  /// Half space can only have very rough RSS
Joseph Mirabel's avatar
Joseph Mirabel committed
483
484
  bv.axes.setIdentity();
  bv.Tr.setZero();
485
  bv.length[0] = bv.length[1] = bv.radius = std::numeric_limits<FCL_REAL>::max();
486
487
488
489
}

template<>
void computeBV<OBBRSS, Halfspace>(const Halfspace& s, const Transform3f& tf, OBBRSS& bv)
sachinc's avatar
sachinc committed
490
{
491
492
493
  computeBV<OBB, Halfspace>(s, tf, bv.obb);
  computeBV<RSS, Halfspace>(s, tf, bv.rss);
}
494

495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
template<>
void computeBV<kIOS, Halfspace>(const Halfspace& s, const Transform3f& tf, kIOS& bv)
{
  bv.num_spheres = 1;
  computeBV<OBB, Halfspace>(s, tf, bv.obb);
  bv.spheres[0].o = Vec3f();
  bv.spheres[0].r = std::numeric_limits<FCL_REAL>::max();
}

template<>
void computeBV<KDOP<16>, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<16>& bv)
{
  Halfspace new_s = transform(s, tf);
  const Vec3f& n = new_s.n;
  const FCL_REAL& d = new_s.d;

  const std::size_t D = 8;
  for(std::size_t i = 0; i < D; ++i)
    bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
  for(std::size_t i = D; i < 2 * D; ++i)
    bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
  
  if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
  {
    if(n[0] > 0) bv.dist(D) = d;
    else bv.dist(0) = -d;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
  {
    if(n[1] > 0) bv.dist(D + 1) = d;
    else bv.dist(1) = -d;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
  {
    if(n[2] > 0) bv.dist(D + 2) = d;
    else bv.dist(2) = -d;
  }
  else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
  {
    if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2;
    else bv.dist(3) = n[0] * d * 2;
  }
  else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
  {
    if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2;
    else bv.dist(4) = n[0] * d * 2;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
  {
    if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2;
    else bv.dist(5) = n[1] * d * 2;
  }
  else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
  {
    if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2;
    else bv.dist(6) = n[0] * d * 2;
  }
  else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
  {
    if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2;
    else bv.dist(7) = n[0] * d * 2;
  }
}

template<>
void computeBV<KDOP<18>, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<18>& bv)
{
  Halfspace new_s = transform(s, tf);
  const Vec3f& n = new_s.n;
  const FCL_REAL& d = new_s.d;

  const std::size_t D = 9;

  for(std::size_t i = 0; i < D; ++i)
    bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
  for(std::size_t i = D; i < 2 * D; ++i)
    bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
  
  if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
  {
    if(n[0] > 0) bv.dist(D) = d;
    else bv.dist(0) = -d;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
  {
    if(n[1] > 0) bv.dist(D + 1) = d;
    else bv.dist(1) = -d;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
  {
    if(n[2] > 0) bv.dist(D + 2) = d;
    else bv.dist(2) = -d;
  }
  else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
  {
    if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2;
    else bv.dist(3) = n[0] * d * 2;
  }
  else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
  {
    if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2;
    else bv.dist(4) = n[0] * d * 2;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
  {
    if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2;
    else bv.dist(5) = n[1] * d * 2;
  }
  else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
  {
    if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2;
    else bv.dist(6) = n[0] * d * 2;
  }
  else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
  {
    if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2;
    else bv.dist(7) = n[0] * d * 2;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
  {
    if(n[1] > 0) bv.dist(D + 8) = n[1] * d * 2;
    else bv.dist(8) = n[1] * d * 2;
  }
}

template<>
void computeBV<KDOP<24>, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<24>& bv)
{
  Halfspace new_s = transform(s, tf);
  const Vec3f& n = new_s.n;
  const FCL_REAL& d = new_s.d;

  const std::size_t D = 12;

  for(std::size_t i = 0; i < D; ++i)
    bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
  for(std::size_t i = D; i < 2 * D; ++i)
    bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
  
  if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
  {
    if(n[0] > 0) bv.dist(D) = d;
    else bv.dist(0) = -d;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
  {
    if(n[1] > 0) bv.dist(D + 1) = d;
    else bv.dist(1) = -d;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
  {
    if(n[2] > 0) bv.dist(D + 2) = d;
    else bv.dist(2) = -d;
  }
  else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
  {
    if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2;
    else bv.dist(3) = n[0] * d * 2;
  }
  else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
  {
    if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2;
    else bv.dist(4) = n[0] * d * 2;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
  {
    if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2;
    else bv.dist(5) = n[1] * d * 2;
  }
  else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
  {
    if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2;
    else bv.dist(6) = n[0] * d * 2;
  }
  else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
  {
    if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2;
    else bv.dist(7) = n[0] * d * 2;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
  {
    if(n[1] > 0) bv.dist(D + 8) = n[1] * d * 2;
    else bv.dist(8) = n[1] * d * 2;
  }
  else if(n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
  {
    if(n[0] > 0) bv.dist(D + 9) = n[0] * d * 3;
    else bv.dist(9) = n[0] * d * 3;
  }
  else if(n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
  {
    if(n[0] > 0) bv.dist(D + 10) = n[0] * d * 3;
    else bv.dist(10) = n[0] * d * 3;
  }
  else if(n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
  {
    if(n[1] > 0) bv.dist(D + 11) = n[1] * d * 3;
    else bv.dist(11) = n[1] * d * 3;
  }
}



template<>
void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv)
{
701
  Vec3f n = tf.getRotation() * s.n;
Joseph Mirabel's avatar
Joseph Mirabel committed
702
703
  generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2));
  bv.axes.col(0).noalias() = n;
sachinc's avatar
sachinc committed
704

705
  bv.extent << 0, std::numeric_limits<FCL_REAL>::max(), std::numeric_limits<FCL_REAL>::max();
sachinc's avatar
sachinc committed
706

707
708
  Vec3f p = s.n * s.d; 
  bv.To = tf.transform(p); /// n'd' = R * n * (d + (R * n) * T) = R * (n * d) + T 
sachinc's avatar
sachinc committed
709
710
}

jpan's avatar
   
jpan committed
711
template<>
712
void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv)
jpan's avatar
   
jpan committed
713
{
714
  Vec3f n = tf.getRotation() * s.n;
jpan's avatar
   
jpan committed
715

Joseph Mirabel's avatar
Joseph Mirabel committed
716
717
  generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2));
  bv.axes.col(0).noalias() = n;
jpan's avatar
   
jpan committed
718

719
720
  bv.length[0] = std::numeric_limits<FCL_REAL>::max();
  bv.length[1] = std::numeric_limits<FCL_REAL>::max();
jpan's avatar
   
jpan committed
721

722
  bv.radius = 0;
723
724
725
  
  Vec3f p = s.n * s.d;
  bv.Tr = tf.transform(p);
jpan's avatar
   
jpan committed
726
727
728
}

template<>
729
void computeBV<OBBRSS, Plane>(const Plane& s, const Transform3f& tf, OBBRSS& bv)
jpan's avatar
   
jpan committed
730
{
731
732
  computeBV<OBB, Plane>(s, tf, bv.obb);
  computeBV<RSS, Plane>(s, tf, bv.rss);
jpan's avatar
   
jpan committed
733
734
735
}

template<>
736
void computeBV<kIOS, Plane>(const Plane& s, const Transform3f& tf, kIOS& bv)
jpan's avatar
   
jpan committed
737
{
738
739
740
741
  bv.num_spheres = 1;
  computeBV<OBB, Plane>(s, tf, bv.obb);
  bv.spheres[0].o = Vec3f();
  bv.spheres[0].r = std::numeric_limits<FCL_REAL>::max();
jpan's avatar
   
jpan committed
742
743
744
}

template<>
745
void computeBV<KDOP<16>, Plane>(const Plane& s, const Transform3f& tf, KDOP<16>& bv)
jpan's avatar
   
jpan committed
746
{
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
  Plane new_s = transform(s, tf);
  const Vec3f& n = new_s.n;
  const FCL_REAL& d = new_s.d;

  const std::size_t D = 8;

  for(std::size_t i = 0; i < D; ++i)
    bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
  for(std::size_t i = D; i < 2 * D; ++i)
    bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
  
  if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
  {
    if(n[0] > 0) bv.dist(0) = bv.dist(D) = d;
    else bv.dist(0) = bv.dist(D) = -d;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
  {
    if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d;
    else bv.dist(1) = bv.dist(D + 1) = -d;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
  {
    if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d;
    else bv.dist(2) = bv.dist(D + 2) = -d;
  }
  else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
  {
    bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
  }
  else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
  {
    bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
  {
    bv.dist(6) = bv.dist(D + 5) = n[1] * d * 2;
  }
  else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
  {
    bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
  }
  else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
  {
    bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
  }
jpan's avatar
   
jpan committed
793
794
795
}

template<>
796
void computeBV<KDOP<18>, Plane>(const Plane& s, const Transform3f& tf, KDOP<18>& bv)
jpan's avatar
   
jpan committed
797
{
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
  Plane new_s = transform(s, tf);
  const Vec3f& n = new_s.n;
  const FCL_REAL& d = new_s.d;

  const std::size_t D = 9;

  for(std::size_t i = 0; i < D; ++i)
    bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
  for(std::size_t i = D; i < 2 * D; ++i)
    bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
  
  if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
  {
    if(n[0] > 0) bv.dist(0) = bv.dist(D) = d;
    else bv.dist(0) = bv.dist(D) = -d;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
  {
    if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d;
    else bv.dist(1) = bv.dist(D + 1) = -d;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
  {
    if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d;
    else bv.dist(2) = bv.dist(D + 2) = -d;
  }
  else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
  {
    bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
  }
  else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
  {
    bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
  {
    bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2;
  }
  else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
  {
    bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
  }
  else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
  {
    bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
  {
    bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2;
  }
jpan's avatar
   
jpan committed
848
849
850
}

template<>
851
void computeBV<KDOP<24>, Plane>(const Plane& s, const Transform3f& tf, KDOP<24>& bv)
jpan's avatar
   
jpan committed
852
{
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
  Plane new_s = transform(s, tf);
  const Vec3f& n = new_s.n;
  const FCL_REAL& d = new_s.d;

  const std::size_t D = 12;

  for(std::size_t i = 0; i < D; ++i)
    bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
  for(std::size_t i = D; i < 2 * D; ++i)
    bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
  
  if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
  {
    if(n[0] > 0) bv.dist(0) = bv.dist(D) = d;
    else bv.dist(0) = bv.dist(D) = -d;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
  {
    if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d;
    else bv.dist(1) = bv.dist(D + 1) = -d;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
  {
    if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d;
    else bv.dist(2) = bv.dist(D + 2) = -d;
  }
  else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
  {
    bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
  }
  else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
  {
    bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
  {
    bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2;
  }
  else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
  {
    bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
  }
  else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
  {
    bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
  }
  else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
  {
    bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2;
  }
  else if(n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
  {
    bv.dist(9) = bv.dist(D + 9) = n[0] * d * 3;
  }
  else if(n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
  {
    bv.dist(10) = bv.dist(D + 10) = n[0] * d * 3;
  }
  else if(n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
  {
    bv.dist(11) = bv.dist(D + 11) = n[1] * d * 3;
  }
jpan's avatar
   
jpan committed
915
916
}

917

918
void constructBox(const AABB& bv, Box& box, Transform3f& tf)
jpan's avatar
jpan committed
919
920
{
  box = Box(bv.max_ - bv.min_);
921
  tf = Transform3f(bv.center());
jpan's avatar
jpan committed
922
923
}

924
void constructBox(const OBB& bv, Box& box, Transform3f& tf)
jpan's avatar
jpan committed
925
926
{
  box = Box(bv.extent * 2);
Joseph Mirabel's avatar
Joseph Mirabel committed
927
  tf = Transform3f(bv.axes, bv.To);
jpan's avatar
jpan committed
928
929
}

930
void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf)
jpan's avatar
jpan committed
931
932
{
  box = Box(bv.obb.extent * 2);
Joseph Mirabel's avatar
Joseph Mirabel committed
933
  tf = Transform3f(bv.obb.axes, bv.obb.To);
jpan's avatar
jpan committed
934
935
}

936
void constructBox(const kIOS& bv, Box& box, Transform3f& tf)
jpan's avatar
jpan committed
937
{
938
  box = Box(bv.obb.extent * 2);
Joseph Mirabel's avatar
Joseph Mirabel committed
939
  tf = Transform3f(bv.obb.axes, bv.obb.To);
jpan's avatar
jpan committed
940
941
}

942
void constructBox(const RSS& bv, Box& box, Transform3f& tf)
jpan's avatar
jpan committed
943
944
{
  box = Box(bv.width(), bv.height(), bv.depth());
Joseph Mirabel's avatar
Joseph Mirabel committed
945
  tf = Transform3f(bv.axes, bv.Tr);
jpan's avatar
jpan committed
946
947
}

948
void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf)
jpan's avatar
jpan committed
949
950
{
  box = Box(bv.width(), bv.height(), bv.depth());
951
  tf = Transform3f(bv.center());
jpan's avatar
jpan committed
952
953
}

954
void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf)
jpan's avatar
jpan committed
955
956
{
  box = Box(bv.width(), bv.height(), bv.depth());
957
  tf = Transform3f(bv.center());
jpan's avatar
jpan committed
958
959
}

960
void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf)
jpan's avatar
jpan committed
961
962
{
  box = Box(bv.width(), bv.height(), bv.depth());
963
  tf = Transform3f(bv.center());
jpan's avatar
jpan committed
964
965
966
967
}



968
void constructBox(const AABB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
jpan's avatar
jpan committed
969
970
{
  box = Box(bv.max_ - bv.min_);
971
  tf = tf_bv * Transform3f(bv.center());
jpan's avatar
jpan committed
972
973
}

974
void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
jpan's avatar
jpan committed
975
976
{
  box = Box(bv.extent * 2);
Joseph Mirabel's avatar
Joseph Mirabel committed
977
  tf = tf_bv * Transform3f(bv.axes, bv.To);
jpan's avatar
jpan committed
978
979
}

980
void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
jpan's avatar
jpan committed
981
982
{
  box = Box(bv.obb.extent * 2);
Joseph Mirabel's avatar
Joseph Mirabel committed
983
  tf = tf_bv * Transform3f(bv.obb.axes, bv.obb.To);
jpan's avatar
jpan committed
984
985
}

986
void constructBox(const kIOS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
jpan's avatar
jpan committed
987
{
988
  box = Box(bv.obb.extent * 2);
Joseph Mirabel's avatar
Joseph Mirabel committed
989
  tf = tf_bv * Transform3f(bv.obb.axes, bv.obb.To);
jpan's avatar
jpan committed
990
991
}

992
void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
jpan's avatar
jpan committed
993
994
{
  box = Box(bv.width(), bv.height(), bv.depth());
Joseph Mirabel's avatar
Joseph Mirabel committed
995
  tf = tf_bv * Transform3f(bv.axes, bv.Tr);
jpan's avatar
jpan committed
996
997
}

998
void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
jpan's avatar
jpan committed
999
1000
{
  box = Box(bv.width(), bv.height(), bv.depth());