00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00038 #include "fcl/shape/geometric_shapes_utility.h"
00039 #include "fcl/BVH/BV_fitter.h"
00040
00041 namespace fcl
00042 {
00043
00044 namespace details
00045 {
00046
00047 std::vector<Vec3f> getBoundVertices(const Box& box, const Transform3f& tf)
00048 {
00049 std::vector<Vec3f> result(8);
00050 FCL_REAL a = box.side[0] / 2;
00051 FCL_REAL b = box.side[1] / 2;
00052 FCL_REAL c = box.side[2] / 2;
00053 result[0] = tf.transform(Vec3f(a, b, c));
00054 result[1] = tf.transform(Vec3f(a, b, -c));
00055 result[2] = tf.transform(Vec3f(a, -b, c));
00056 result[3] = tf.transform(Vec3f(a, -b, -c));
00057 result[4] = tf.transform(Vec3f(-a, b, c));
00058 result[5] = tf.transform(Vec3f(-a, b, -c));
00059 result[6] = tf.transform(Vec3f(-a, -b, c));
00060 result[7] = tf.transform(Vec3f(-a, -b, -c));
00061
00062 return result;
00063 }
00064
00065
00066 std::vector<Vec3f> getBoundVertices(const Sphere& sphere, const Transform3f& tf)
00067 {
00068 std::vector<Vec3f> result(12);
00069 const FCL_REAL m = (1 + sqrt(5.0)) / 2.0;
00070 FCL_REAL edge_size = sphere.radius * 6 / (sqrt(27.0) + sqrt(15.0));
00071
00072 FCL_REAL a = edge_size;
00073 FCL_REAL b = m * edge_size;
00074 result[0] = tf.transform(Vec3f(0, a, b));
00075 result[1] = tf.transform(Vec3f(0, -a, b));
00076 result[2] = tf.transform(Vec3f(0, a, -b));
00077 result[3] = tf.transform(Vec3f(0, -a, -b));
00078 result[4] = tf.transform(Vec3f(a, b, 0));
00079 result[5] = tf.transform(Vec3f(-a, b, 0));
00080 result[6] = tf.transform(Vec3f(a, -b, 0));
00081 result[7] = tf.transform(Vec3f(-a, -b, 0));
00082 result[8] = tf.transform(Vec3f(b, 0, a));
00083 result[9] = tf.transform(Vec3f(b, 0, -a));
00084 result[10] = tf.transform(Vec3f(-b, 0, a));
00085 result[11] = tf.transform(Vec3f(-b, 0, -a));
00086
00087 return result;
00088 }
00089
00090 std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const Transform3f& tf)
00091 {
00092 std::vector<Vec3f> result(36);
00093 const FCL_REAL m = (1 + sqrt(5.0)) / 2.0;
00094
00095 FCL_REAL hl = capsule.lz * 0.5;
00096 FCL_REAL edge_size = capsule.radius * 6 / (sqrt(27.0) + sqrt(15.0));
00097 FCL_REAL a = edge_size;
00098 FCL_REAL b = m * edge_size;
00099 FCL_REAL r2 = capsule.radius * 2 / sqrt(3.0);
00100
00101
00102 result[0] = tf.transform(Vec3f(0, a, b + hl));
00103 result[1] = tf.transform(Vec3f(0, -a, b + hl));
00104 result[2] = tf.transform(Vec3f(0, a, -b + hl));
00105 result[3] = tf.transform(Vec3f(0, -a, -b + hl));
00106 result[4] = tf.transform(Vec3f(a, b, hl));
00107 result[5] = tf.transform(Vec3f(-a, b, hl));
00108 result[6] = tf.transform(Vec3f(a, -b, hl));
00109 result[7] = tf.transform(Vec3f(-a, -b, hl));
00110 result[8] = tf.transform(Vec3f(b, 0, a + hl));
00111 result[9] = tf.transform(Vec3f(b, 0, -a + hl));
00112 result[10] = tf.transform(Vec3f(-b, 0, a + hl));
00113 result[11] = tf.transform(Vec3f(-b, 0, -a + hl));
00114
00115 result[12] = tf.transform(Vec3f(0, a, b - hl));
00116 result[13] = tf.transform(Vec3f(0, -a, b - hl));
00117 result[14] = tf.transform(Vec3f(0, a, -b - hl));
00118 result[15] = tf.transform(Vec3f(0, -a, -b - hl));
00119 result[16] = tf.transform(Vec3f(a, b, -hl));
00120 result[17] = tf.transform(Vec3f(-a, b, -hl));
00121 result[18] = tf.transform(Vec3f(a, -b, -hl));
00122 result[19] = tf.transform(Vec3f(-a, -b, -hl));
00123 result[20] = tf.transform(Vec3f(b, 0, a - hl));
00124 result[21] = tf.transform(Vec3f(b, 0, -a - hl));
00125 result[22] = tf.transform(Vec3f(-b, 0, a - hl));
00126 result[23] = tf.transform(Vec3f(-b, 0, -a - hl));
00127
00128 FCL_REAL c = 0.5 * r2;
00129 FCL_REAL d = capsule.radius;
00130 result[24] = tf.transform(Vec3f(r2, 0, hl));
00131 result[25] = tf.transform(Vec3f(c, d, hl));
00132 result[26] = tf.transform(Vec3f(-c, d, hl));
00133 result[27] = tf.transform(Vec3f(-r2, 0, hl));
00134 result[28] = tf.transform(Vec3f(-c, -d, hl));
00135 result[29] = tf.transform(Vec3f(c, -d, hl));
00136
00137 result[30] = tf.transform(Vec3f(r2, 0, -hl));
00138 result[31] = tf.transform(Vec3f(c, d, -hl));
00139 result[32] = tf.transform(Vec3f(-c, d, -hl));
00140 result[33] = tf.transform(Vec3f(-r2, 0, -hl));
00141 result[34] = tf.transform(Vec3f(-c, -d, -hl));
00142 result[35] = tf.transform(Vec3f(c, -d, -hl));
00143
00144 return result;
00145 }
00146
00147
00148 std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf)
00149 {
00150 std::vector<Vec3f> result(7);
00151
00152 FCL_REAL hl = cone.lz * 0.5;
00153 FCL_REAL r2 = cone.radius * 2 / sqrt(3.0);
00154 FCL_REAL a = 0.5 * r2;
00155 FCL_REAL b = cone.radius;
00156
00157 result[0] = tf.transform(Vec3f(r2, 0, -hl));
00158 result[1] = tf.transform(Vec3f(a, b, -hl));
00159 result[2] = tf.transform(Vec3f(-a, b, -hl));
00160 result[3] = tf.transform(Vec3f(-r2, 0, -hl));
00161 result[4] = tf.transform(Vec3f(-a, -b, -hl));
00162 result[5] = tf.transform(Vec3f(a, -b, -hl));
00163
00164 result[6] = tf.transform(Vec3f(0, 0, hl));
00165
00166 return result;
00167 }
00168
00169 std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, const Transform3f& tf)
00170 {
00171 std::vector<Vec3f> result(12);
00172
00173 FCL_REAL hl = cylinder.lz * 0.5;
00174 FCL_REAL r2 = cylinder.radius * 2 / sqrt(3.0);
00175 FCL_REAL a = 0.5 * r2;
00176 FCL_REAL b = cylinder.radius;
00177
00178 result[0] = tf.transform(Vec3f(r2, 0, -hl));
00179 result[1] = tf.transform(Vec3f(a, b, -hl));
00180 result[2] = tf.transform(Vec3f(-a, b, -hl));
00181 result[3] = tf.transform(Vec3f(-r2, 0, -hl));
00182 result[4] = tf.transform(Vec3f(-a, -b, -hl));
00183 result[5] = tf.transform(Vec3f(a, -b, -hl));
00184
00185 result[6] = tf.transform(Vec3f(r2, 0, hl));
00186 result[7] = tf.transform(Vec3f(a, b, hl));
00187 result[8] = tf.transform(Vec3f(-a, b, hl));
00188 result[9] = tf.transform(Vec3f(-r2, 0, hl));
00189 result[10] = tf.transform(Vec3f(-a, -b, hl));
00190 result[11] = tf.transform(Vec3f(a, -b, hl));
00191
00192 return result;
00193 }
00194
00195 std::vector<Vec3f> getBoundVertices(const Convex& convex, const Transform3f& tf)
00196 {
00197 std::vector<Vec3f> result(convex.num_points);
00198 for(int i = 0; i < convex.num_points; ++i)
00199 {
00200 result[i] = tf.transform(convex.points[i]);
00201 }
00202
00203 return result;
00204 }
00205
00206 std::vector<Vec3f> getBoundVertices(const Triangle2& triangle, const Transform3f& tf)
00207 {
00208 std::vector<Vec3f> result(3);
00209 result[0] = tf.transform(triangle.a);
00210 result[1] = tf.transform(triangle.b);
00211 result[2] = tf.transform(triangle.c);
00212
00213 return result;
00214 }
00215
00216 }
00217
00218
00219 template<>
00220 void computeBV<AABB, Box>(const Box& s, const Transform3f& tf, AABB& bv)
00221 {
00222 const Matrix3f& R = tf.getRotation();
00223 const Vec3f& T = tf.getTranslation();
00224
00225 FCL_REAL x_range = 0.5 * (fabs(R(0, 0) * s.side[0]) + fabs(R(0, 1) * s.side[1]) + fabs(R(0, 2) * s.side[2]));
00226 FCL_REAL y_range = 0.5 * (fabs(R(1, 0) * s.side[0]) + fabs(R(1, 1) * s.side[1]) + fabs(R(1, 2) * s.side[2]));
00227 FCL_REAL z_range = 0.5 * (fabs(R(2, 0) * s.side[0]) + fabs(R(2, 1) * s.side[1]) + fabs(R(2, 2) * s.side[2]));
00228
00229 Vec3f v_delta(x_range, y_range, z_range);
00230 bv.max_ = T + v_delta;
00231 bv.min_ = T - v_delta;
00232 }
00233
00234 template<>
00235 void computeBV<AABB, Sphere>(const Sphere& s, const Transform3f& tf, AABB& bv)
00236 {
00237 const Vec3f& T = tf.getTranslation();
00238
00239 Vec3f v_delta(s.radius);
00240 bv.max_ = T + v_delta;
00241 bv.min_ = T - v_delta;
00242 }
00243
00244 template<>
00245 void computeBV<AABB, Capsule>(const Capsule& s, const Transform3f& tf, AABB& bv)
00246 {
00247 const Matrix3f& R = tf.getRotation();
00248 const Vec3f& T = tf.getTranslation();
00249
00250 FCL_REAL x_range = 0.5 * fabs(R(0, 2) * s.lz) + s.radius;
00251 FCL_REAL y_range = 0.5 * fabs(R(1, 2) * s.lz) + s.radius;
00252 FCL_REAL z_range = 0.5 * fabs(R(2, 2) * s.lz) + s.radius;
00253
00254 Vec3f v_delta(x_range, y_range, z_range);
00255 bv.max_ = T + v_delta;
00256 bv.min_ = T - v_delta;
00257 }
00258
00259 template<>
00260 void computeBV<AABB, Cone>(const Cone& s, const Transform3f& tf, AABB& bv)
00261 {
00262 const Matrix3f& R = tf.getRotation();
00263 const Vec3f& T = tf.getTranslation();
00264
00265 FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz);
00266 FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz);
00267 FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz);
00268
00269 Vec3f v_delta(x_range, y_range, z_range);
00270 bv.max_ = T + v_delta;
00271 bv.min_ = T - v_delta;
00272 }
00273
00274 template<>
00275 void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3f& tf, AABB& bv)
00276 {
00277 const Matrix3f& R = tf.getRotation();
00278 const Vec3f& T = tf.getTranslation();
00279
00280 FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz);
00281 FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz);
00282 FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz);
00283
00284 Vec3f v_delta(x_range, y_range, z_range);
00285 bv.max_ = T + v_delta;
00286 bv.min_ = T - v_delta;
00287 }
00288
00289 template<>
00290 void computeBV<AABB, Convex>(const Convex& s, const Transform3f& tf, AABB& bv)
00291 {
00292 const Matrix3f& R = tf.getRotation();
00293 const Vec3f& T = tf.getTranslation();
00294
00295 AABB bv_;
00296 for(int i = 0; i < s.num_points; ++i)
00297 {
00298 Vec3f new_p = R * s.points[i] + T;
00299 bv_ += new_p;
00300 }
00301
00302 bv = bv_;
00303 }
00304
00305 template<>
00306 void computeBV<AABB, Triangle2>(const Triangle2& s, const Transform3f& tf, AABB& bv)
00307 {
00308 bv = AABB(tf.transform(s.a), tf.transform(s.b), tf.transform(s.c));
00309 }
00310
00311 template<>
00312 void computeBV<AABB, Plane>(const Plane& s, const Transform3f& tf, AABB& bv)
00313 {
00314 const Matrix3f& R = tf.getRotation();
00315
00316 Vec3f n = R * s.n;
00317
00318 AABB bv_;
00319 if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00320 {
00321
00322 if(n[0] < 0) bv_.min_[0] = -s.d;
00323 else if(n[0] > 0) bv_.max_[0] = s.d;
00324 }
00325 else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00326 {
00327
00328 if(n[1] < 0) bv_.min_[1] = -s.d;
00329 else if(n[1] > 0) bv_.max_[1] = s.d;
00330 }
00331 else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
00332 {
00333
00334 if(n[2] < 0) bv_.min_[2] = -s.d;
00335 else if(n[2] > 0) bv_.max_[2] = s.d;
00336 }
00337
00338 bv = bv_;
00339 }
00340
00341
00342 template<>
00343 void computeBV<OBB, Box>(const Box& s, const Transform3f& tf, OBB& bv)
00344 {
00345 const Matrix3f& R = tf.getRotation();
00346 const Vec3f& T = tf.getTranslation();
00347
00348 bv.To = T;
00349 bv.axis[0] = R.getColumn(0);
00350 bv.axis[1] = R.getColumn(1);
00351 bv.axis[2] = R.getColumn(2);
00352 bv.extent = s.side * (FCL_REAL)0.5;
00353 }
00354
00355 template<>
00356 void computeBV<OBB, Sphere>(const Sphere& s, const Transform3f& tf, OBB& bv)
00357 {
00358 const Vec3f& T = tf.getTranslation();
00359
00360 bv.To = T;
00361 bv.axis[0].setValue(1, 0, 0);
00362 bv.axis[1].setValue(0, 1, 0);
00363 bv.axis[2].setValue(0, 0, 1);
00364 bv.extent.setValue(s.radius);
00365 }
00366
00367 template<>
00368 void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv)
00369 {
00370 const Matrix3f& R = tf.getRotation();
00371 const Vec3f& T = tf.getTranslation();
00372
00373 bv.To = T;
00374 bv.axis[0] = R.getColumn(0);
00375 bv.axis[1] = R.getColumn(1);
00376 bv.axis[2] = R.getColumn(2);
00377 bv.extent.setValue(s.radius, s.radius, s.lz / 2 + s.radius);
00378 }
00379
00380 template<>
00381 void computeBV<OBB, Cone>(const Cone& s, const Transform3f& tf, OBB& bv)
00382 {
00383 const Matrix3f& R = tf.getRotation();
00384 const Vec3f& T = tf.getTranslation();
00385
00386 bv.To = T;
00387 bv.axis[0] = R.getColumn(0);
00388 bv.axis[1] = R.getColumn(1);
00389 bv.axis[2] = R.getColumn(2);
00390 bv.extent.setValue(s.radius, s.radius, s.lz / 2);
00391 }
00392
00393 template<>
00394 void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3f& tf, OBB& bv)
00395 {
00396 const Matrix3f& R = tf.getRotation();
00397 const Vec3f& T = tf.getTranslation();
00398
00399 bv.To = T;
00400 bv.axis[0] = R.getColumn(0);
00401 bv.axis[1] = R.getColumn(1);
00402 bv.axis[2] = R.getColumn(2);
00403 bv.extent.setValue(s.radius, s.radius, s.lz / 2);
00404 }
00405
00406 template<>
00407 void computeBV<OBB, Convex>(const Convex& s, const Transform3f& tf, OBB& bv)
00408 {
00409 const Matrix3f& R = tf.getRotation();
00410 const Vec3f& T = tf.getTranslation();
00411
00412 fit(s.points, s.num_points, bv);
00413
00414 bv.axis[0] = R * bv.axis[0];
00415 bv.axis[1] = R * bv.axis[1];
00416 bv.axis[2] = R * bv.axis[2];
00417
00418 bv.To = R * bv.To + T;
00419 }
00420
00421 template<>
00422 void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv)
00423 {
00424 const Matrix3f& R = tf.getRotation();
00425 const Vec3f& T = tf.getTranslation();
00426
00427
00428 generateCoordinateSystem(s.n, bv.axis[1], bv.axis[2]);
00429 bv.axis[0] = s.n;
00430
00431 bv.extent.setValue(0, std::numeric_limits<FCL_REAL>::max(), std::numeric_limits<FCL_REAL>::max());
00432
00433 Vec3f p = s.n * s.d;
00434 bv.To = R * p + T;
00435 }
00436
00437 template<>
00438 void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv)
00439 {
00440 const Matrix3f& R = tf.getRotation();
00441 const Vec3f& T = tf.getTranslation();
00442
00443 generateCoordinateSystem(s.n, bv.axis[1], bv.axis[2]);
00444 bv.axis[0] = s.n;
00445
00446 bv.l[0] = std::numeric_limits<FCL_REAL>::max();
00447 bv.l[1] = std::numeric_limits<FCL_REAL>::max();
00448
00449 bv.r = std::numeric_limits<FCL_REAL>::max();
00450 }
00451
00452 template<>
00453 void computeBV<OBBRSS, Plane>(const Plane& s, const Transform3f& tf, OBBRSS& bv)
00454 {
00455 }
00456
00457 template<>
00458 void computeBV<kIOS, Plane>(const Plane& s, const Transform3f& tf, kIOS& bv)
00459 {
00460 }
00461
00462 template<>
00463 void computeBV<KDOP<16>, Plane>(const Plane& s, const Transform3f& tf, KDOP<16>& bv)
00464 {
00465 }
00466
00467 template<>
00468 void computeBV<KDOP<18>, Plane>(const Plane& s, const Transform3f& tf, KDOP<18>& bv)
00469 {
00470 }
00471
00472 template<>
00473 void computeBV<KDOP<24>, Plane>(const Plane& s, const Transform3f& tf, KDOP<24>& bv)
00474 {
00475 }
00476
00477
00478 void constructBox(const AABB& bv, Box& box, Transform3f& tf)
00479 {
00480 box = Box(bv.max_ - bv.min_);
00481 tf = Transform3f(bv.center());
00482 }
00483
00484 void constructBox(const OBB& bv, Box& box, Transform3f& tf)
00485 {
00486 box = Box(bv.extent * 2);
00487 tf = Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
00488 bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
00489 bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.To);
00490 }
00491
00492 void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf)
00493 {
00494 box = Box(bv.obb.extent * 2);
00495 tf = Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
00496 bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
00497 bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
00498 }
00499
00500 void constructBox(const kIOS& bv, Box& box, Transform3f& tf)
00501 {
00502 box = Box(bv.obb.extent * 2);
00503 tf = Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
00504 bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
00505 bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
00506 }
00507
00508 void constructBox(const RSS& bv, Box& box, Transform3f& tf)
00509 {
00510 box = Box(bv.width(), bv.height(), bv.depth());
00511 tf = Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
00512 bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
00513 bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.Tr);
00514 }
00515
00516 void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf)
00517 {
00518 box = Box(bv.width(), bv.height(), bv.depth());
00519 tf = Transform3f(bv.center());
00520 }
00521
00522 void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf)
00523 {
00524 box = Box(bv.width(), bv.height(), bv.depth());
00525 tf = Transform3f(bv.center());
00526 }
00527
00528 void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf)
00529 {
00530 box = Box(bv.width(), bv.height(), bv.depth());
00531 tf = Transform3f(bv.center());
00532 }
00533
00534
00535
00536 void constructBox(const AABB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
00537 {
00538 box = Box(bv.max_ - bv.min_);
00539 tf = tf_bv * Transform3f(bv.center());
00540 }
00541
00542 void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
00543 {
00544 box = Box(bv.extent * 2);
00545 tf = tf_bv *Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
00546 bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
00547 bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.To);
00548 }
00549
00550 void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
00551 {
00552 box = Box(bv.obb.extent * 2);
00553 tf = tf_bv * Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
00554 bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
00555 bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
00556 }
00557
00558 void constructBox(const kIOS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
00559 {
00560 box = Box(bv.obb.extent * 2);
00561 tf = tf_bv * Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
00562 bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
00563 bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
00564 }
00565
00566 void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
00567 {
00568 box = Box(bv.width(), bv.height(), bv.depth());
00569 tf = tf_bv * Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
00570 bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
00571 bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.Tr);
00572 }
00573
00574 void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
00575 {
00576 box = Box(bv.width(), bv.height(), bv.depth());
00577 tf = tf_bv * Transform3f(bv.center());
00578 }
00579
00580 void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
00581 {
00582 box = Box(bv.width(), bv.height(), bv.depth());
00583 tf = tf_bv * Transform3f(bv.center());
00584 }
00585
00586 void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
00587 {
00588 box = Box(bv.width(), bv.height(), bv.depth());
00589 tf = tf_bv * Transform3f(bv.center());
00590 }
00591
00592
00593
00594 }