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 Halfspace transform(const Halfspace& a, const Transform3f& tf)
00219 {
00225
00226 Vec3f n = tf.getQuatRotation().transform(a.n);
00227 FCL_REAL d = a.d + n.dot(tf.getTranslation());
00228
00229 return Halfspace(n, d);
00230 }
00231
00232
00233 Plane transform(const Plane& a, const Transform3f& tf)
00234 {
00240
00241 Vec3f n = tf.getQuatRotation().transform(a.n);
00242 FCL_REAL d = a.d + n.dot(tf.getTranslation());
00243
00244 return Plane(n, d);
00245 }
00246
00247
00248
00249 template<>
00250 void computeBV<AABB, Box>(const Box& s, const Transform3f& tf, AABB& bv)
00251 {
00252 const Matrix3f& R = tf.getRotation();
00253 const Vec3f& T = tf.getTranslation();
00254
00255 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]));
00256 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]));
00257 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]));
00258
00259 Vec3f v_delta(x_range, y_range, z_range);
00260 bv.max_ = T + v_delta;
00261 bv.min_ = T - v_delta;
00262 }
00263
00264 template<>
00265 void computeBV<AABB, Sphere>(const Sphere& s, const Transform3f& tf, AABB& bv)
00266 {
00267 const Vec3f& T = tf.getTranslation();
00268
00269 Vec3f v_delta(s.radius);
00270 bv.max_ = T + v_delta;
00271 bv.min_ = T - v_delta;
00272 }
00273
00274 template<>
00275 void computeBV<AABB, Capsule>(const Capsule& 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 = 0.5 * fabs(R(0, 2) * s.lz) + s.radius;
00281 FCL_REAL y_range = 0.5 * fabs(R(1, 2) * s.lz) + s.radius;
00282 FCL_REAL z_range = 0.5 * fabs(R(2, 2) * s.lz) + s.radius;
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, Cone>(const Cone& s, const Transform3f& tf, AABB& bv)
00291 {
00292 const Matrix3f& R = tf.getRotation();
00293 const Vec3f& T = tf.getTranslation();
00294
00295 FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz);
00296 FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz);
00297 FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz);
00298
00299 Vec3f v_delta(x_range, y_range, z_range);
00300 bv.max_ = T + v_delta;
00301 bv.min_ = T - v_delta;
00302 }
00303
00304 template<>
00305 void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3f& tf, AABB& bv)
00306 {
00307 const Matrix3f& R = tf.getRotation();
00308 const Vec3f& T = tf.getTranslation();
00309
00310 FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz);
00311 FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz);
00312 FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz);
00313
00314 Vec3f v_delta(x_range, y_range, z_range);
00315 bv.max_ = T + v_delta;
00316 bv.min_ = T - v_delta;
00317 }
00318
00319 template<>
00320 void computeBV<AABB, Convex>(const Convex& s, const Transform3f& tf, AABB& bv)
00321 {
00322 const Matrix3f& R = tf.getRotation();
00323 const Vec3f& T = tf.getTranslation();
00324
00325 AABB bv_;
00326 for(int i = 0; i < s.num_points; ++i)
00327 {
00328 Vec3f new_p = R * s.points[i] + T;
00329 bv_ += new_p;
00330 }
00331
00332 bv = bv_;
00333 }
00334
00335 template<>
00336 void computeBV<AABB, Triangle2>(const Triangle2& s, const Transform3f& tf, AABB& bv)
00337 {
00338 bv = AABB(tf.transform(s.a), tf.transform(s.b), tf.transform(s.c));
00339 }
00340
00341
00342 template<>
00343 void computeBV<AABB, Halfspace>(const Halfspace& s, const Transform3f& tf, AABB& bv)
00344 {
00345 Halfspace new_s = transform(s, tf);
00346 const Vec3f& n = new_s.n;
00347 const FCL_REAL& d = new_s.d;
00348
00349 AABB bv_;
00350 bv_.min_ = Vec3f(-std::numeric_limits<FCL_REAL>::max());
00351 bv_.max_ = Vec3f(std::numeric_limits<FCL_REAL>::max());
00352 if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00353 {
00354
00355 if(n[0] < 0) bv_.min_[0] = -d;
00356 else if(n[0] > 0) bv_.max_[0] = d;
00357 }
00358 else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00359 {
00360
00361 if(n[1] < 0) bv_.min_[1] = -d;
00362 else if(n[1] > 0) bv_.max_[1] = d;
00363 }
00364 else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
00365 {
00366
00367 if(n[2] < 0) bv_.min_[2] = -d;
00368 else if(n[2] > 0) bv_.max_[2] = d;
00369 }
00370
00371 bv = bv_;
00372 }
00373
00374 template<>
00375 void computeBV<AABB, Plane>(const Plane& s, const Transform3f& tf, AABB& bv)
00376 {
00377 Plane new_s = transform(s, tf);
00378 const Vec3f& n = new_s.n;
00379 const FCL_REAL& d = new_s.d;
00380
00381 AABB bv_;
00382 bv_.min_ = Vec3f(-std::numeric_limits<FCL_REAL>::max());
00383 bv_.max_ = Vec3f(std::numeric_limits<FCL_REAL>::max());
00384 if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00385 {
00386
00387 if(n[0] < 0) { bv_.min_[0] = bv_.max_[0] = -d; }
00388 else if(n[0] > 0) { bv_.min_[0] = bv_.max_[0] = d; }
00389 }
00390 else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00391 {
00392
00393 if(n[1] < 0) { bv_.min_[1] = bv_.max_[1] = -d; }
00394 else if(n[1] > 0) { bv_.min_[1] = bv_.max_[1] = d; }
00395 }
00396 else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
00397 {
00398
00399 if(n[2] < 0) { bv_.min_[2] = bv_.max_[2] = -d; }
00400 else if(n[2] > 0) { bv_.min_[2] = bv_.max_[2] = d; }
00401 }
00402
00403 bv = bv_;
00404 }
00405
00406
00407 template<>
00408 void computeBV<OBB, Box>(const Box& s, const Transform3f& tf, OBB& bv)
00409 {
00410 const Matrix3f& R = tf.getRotation();
00411 const Vec3f& T = tf.getTranslation();
00412
00413 bv.To = T;
00414 bv.axis[0] = R.getColumn(0);
00415 bv.axis[1] = R.getColumn(1);
00416 bv.axis[2] = R.getColumn(2);
00417 bv.extent = s.side * (FCL_REAL)0.5;
00418 }
00419
00420 template<>
00421 void computeBV<OBB, Sphere>(const Sphere& s, const Transform3f& tf, OBB& bv)
00422 {
00423 const Vec3f& T = tf.getTranslation();
00424
00425 bv.To = T;
00426 bv.axis[0].setValue(1, 0, 0);
00427 bv.axis[1].setValue(0, 1, 0);
00428 bv.axis[2].setValue(0, 0, 1);
00429 bv.extent.setValue(s.radius);
00430 }
00431
00432 template<>
00433 void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv)
00434 {
00435 const Matrix3f& R = tf.getRotation();
00436 const Vec3f& T = tf.getTranslation();
00437
00438 bv.To = T;
00439 bv.axis[0] = R.getColumn(0);
00440 bv.axis[1] = R.getColumn(1);
00441 bv.axis[2] = R.getColumn(2);
00442 bv.extent.setValue(s.radius, s.radius, s.lz / 2 + s.radius);
00443 }
00444
00445 template<>
00446 void computeBV<OBB, Cone>(const Cone& s, const Transform3f& tf, OBB& bv)
00447 {
00448 const Matrix3f& R = tf.getRotation();
00449 const Vec3f& T = tf.getTranslation();
00450
00451 bv.To = T;
00452 bv.axis[0] = R.getColumn(0);
00453 bv.axis[1] = R.getColumn(1);
00454 bv.axis[2] = R.getColumn(2);
00455 bv.extent.setValue(s.radius, s.radius, s.lz / 2);
00456 }
00457
00458 template<>
00459 void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3f& tf, OBB& bv)
00460 {
00461 const Matrix3f& R = tf.getRotation();
00462 const Vec3f& T = tf.getTranslation();
00463
00464 bv.To = T;
00465 bv.axis[0] = R.getColumn(0);
00466 bv.axis[1] = R.getColumn(1);
00467 bv.axis[2] = R.getColumn(2);
00468 bv.extent.setValue(s.radius, s.radius, s.lz / 2);
00469 }
00470
00471 template<>
00472 void computeBV<OBB, Convex>(const Convex& s, const Transform3f& tf, OBB& bv)
00473 {
00474 const Matrix3f& R = tf.getRotation();
00475 const Vec3f& T = tf.getTranslation();
00476
00477 fit(s.points, s.num_points, bv);
00478
00479 bv.axis[0] = R * bv.axis[0];
00480 bv.axis[1] = R * bv.axis[1];
00481 bv.axis[2] = R * bv.axis[2];
00482
00483 bv.To = R * bv.To + T;
00484 }
00485
00486 template<>
00487 void computeBV<OBB, Halfspace>(const Halfspace& s, const Transform3f& tf, OBB& bv)
00488 {
00490 bv.axis[0] = Vec3f(1, 0, 0);
00491 bv.axis[1] = Vec3f(0, 1, 0);
00492 bv.axis[2] = Vec3f(0, 0, 1);
00493 bv.To = Vec3f(0, 0, 0);
00494 bv.extent.setValue(std::numeric_limits<FCL_REAL>::max());
00495 }
00496
00497 template<>
00498 void computeBV<RSS, Halfspace>(const Halfspace& s, const Transform3f& tf, RSS& bv)
00499 {
00501 bv.axis[0] = Vec3f(1, 0, 0);
00502 bv.axis[1] = Vec3f(0, 1, 0);
00503 bv.axis[2] = Vec3f(0, 0, 1);
00504 bv.Tr = Vec3f(0, 0, 0);
00505 bv.l[0] = bv.l[1] = bv.r = std::numeric_limits<FCL_REAL>::max();
00506 }
00507
00508 template<>
00509 void computeBV<OBBRSS, Halfspace>(const Halfspace& s, const Transform3f& tf, OBBRSS& bv)
00510 {
00511 computeBV<OBB, Halfspace>(s, tf, bv.obb);
00512 computeBV<RSS, Halfspace>(s, tf, bv.rss);
00513 }
00514
00515 template<>
00516 void computeBV<kIOS, Halfspace>(const Halfspace& s, const Transform3f& tf, kIOS& bv)
00517 {
00518 bv.num_spheres = 1;
00519 computeBV<OBB, Halfspace>(s, tf, bv.obb);
00520 bv.spheres[0].o = Vec3f();
00521 bv.spheres[0].r = std::numeric_limits<FCL_REAL>::max();
00522 }
00523
00524 template<>
00525 void computeBV<KDOP<16>, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<16>& bv)
00526 {
00527 Halfspace new_s = transform(s, tf);
00528 const Vec3f& n = new_s.n;
00529 const FCL_REAL& d = new_s.d;
00530
00531 const std::size_t D = 8;
00532 for(std::size_t i = 0; i < D; ++i)
00533 bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
00534 for(std::size_t i = D; i < 2 * D; ++i)
00535 bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
00536
00537 if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00538 {
00539 if(n[0] > 0) bv.dist(D) = d;
00540 else bv.dist(0) = -d;
00541 }
00542 else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00543 {
00544 if(n[1] > 0) bv.dist(D + 1) = d;
00545 else bv.dist(1) = -d;
00546 }
00547 else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
00548 {
00549 if(n[2] > 0) bv.dist(D + 2) = d;
00550 else bv.dist(2) = -d;
00551 }
00552 else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
00553 {
00554 if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2;
00555 else bv.dist(3) = n[0] * d * 2;
00556 }
00557 else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
00558 {
00559 if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2;
00560 else bv.dist(4) = n[0] * d * 2;
00561 }
00562 else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
00563 {
00564 if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2;
00565 else bv.dist(5) = n[1] * d * 2;
00566 }
00567 else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
00568 {
00569 if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2;
00570 else bv.dist(6) = n[0] * d * 2;
00571 }
00572 else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
00573 {
00574 if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2;
00575 else bv.dist(7) = n[0] * d * 2;
00576 }
00577 }
00578
00579 template<>
00580 void computeBV<KDOP<18>, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<18>& bv)
00581 {
00582 Halfspace new_s = transform(s, tf);
00583 const Vec3f& n = new_s.n;
00584 const FCL_REAL& d = new_s.d;
00585
00586 const std::size_t D = 9;
00587
00588 for(std::size_t i = 0; i < D; ++i)
00589 bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
00590 for(std::size_t i = D; i < 2 * D; ++i)
00591 bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
00592
00593 if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00594 {
00595 if(n[0] > 0) bv.dist(D) = d;
00596 else bv.dist(0) = -d;
00597 }
00598 else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00599 {
00600 if(n[1] > 0) bv.dist(D + 1) = d;
00601 else bv.dist(1) = -d;
00602 }
00603 else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
00604 {
00605 if(n[2] > 0) bv.dist(D + 2) = d;
00606 else bv.dist(2) = -d;
00607 }
00608 else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
00609 {
00610 if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2;
00611 else bv.dist(3) = n[0] * d * 2;
00612 }
00613 else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
00614 {
00615 if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2;
00616 else bv.dist(4) = n[0] * d * 2;
00617 }
00618 else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
00619 {
00620 if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2;
00621 else bv.dist(5) = n[1] * d * 2;
00622 }
00623 else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
00624 {
00625 if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2;
00626 else bv.dist(6) = n[0] * d * 2;
00627 }
00628 else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
00629 {
00630 if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2;
00631 else bv.dist(7) = n[0] * d * 2;
00632 }
00633 else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
00634 {
00635 if(n[1] > 0) bv.dist(D + 8) = n[1] * d * 2;
00636 else bv.dist(8) = n[1] * d * 2;
00637 }
00638 }
00639
00640 template<>
00641 void computeBV<KDOP<24>, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<24>& bv)
00642 {
00643 Halfspace new_s = transform(s, tf);
00644 const Vec3f& n = new_s.n;
00645 const FCL_REAL& d = new_s.d;
00646
00647 const std::size_t D = 12;
00648
00649 for(std::size_t i = 0; i < D; ++i)
00650 bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
00651 for(std::size_t i = D; i < 2 * D; ++i)
00652 bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
00653
00654 if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00655 {
00656 if(n[0] > 0) bv.dist(D) = d;
00657 else bv.dist(0) = -d;
00658 }
00659 else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00660 {
00661 if(n[1] > 0) bv.dist(D + 1) = d;
00662 else bv.dist(1) = -d;
00663 }
00664 else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
00665 {
00666 if(n[2] > 0) bv.dist(D + 2) = d;
00667 else bv.dist(2) = -d;
00668 }
00669 else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
00670 {
00671 if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2;
00672 else bv.dist(3) = n[0] * d * 2;
00673 }
00674 else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
00675 {
00676 if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2;
00677 else bv.dist(4) = n[0] * d * 2;
00678 }
00679 else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
00680 {
00681 if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2;
00682 else bv.dist(5) = n[1] * d * 2;
00683 }
00684 else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
00685 {
00686 if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2;
00687 else bv.dist(6) = n[0] * d * 2;
00688 }
00689 else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
00690 {
00691 if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2;
00692 else bv.dist(7) = n[0] * d * 2;
00693 }
00694 else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
00695 {
00696 if(n[1] > 0) bv.dist(D + 8) = n[1] * d * 2;
00697 else bv.dist(8) = n[1] * d * 2;
00698 }
00699 else if(n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
00700 {
00701 if(n[0] > 0) bv.dist(D + 9) = n[0] * d * 3;
00702 else bv.dist(9) = n[0] * d * 3;
00703 }
00704 else if(n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
00705 {
00706 if(n[0] > 0) bv.dist(D + 10) = n[0] * d * 3;
00707 else bv.dist(10) = n[0] * d * 3;
00708 }
00709 else if(n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
00710 {
00711 if(n[1] > 0) bv.dist(D + 11) = n[1] * d * 3;
00712 else bv.dist(11) = n[1] * d * 3;
00713 }
00714 }
00715
00716
00717
00718 template<>
00719 void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv)
00720 {
00721 Vec3f n = tf.getQuatRotation().transform(s.n);
00722 generateCoordinateSystem(n, bv.axis[1], bv.axis[2]);
00723 bv.axis[0] = n;
00724
00725 bv.extent.setValue(0, std::numeric_limits<FCL_REAL>::max(), std::numeric_limits<FCL_REAL>::max());
00726
00727 Vec3f p = s.n * s.d;
00728 bv.To = tf.transform(p);
00729 }
00730
00731 template<>
00732 void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv)
00733 {
00734 Vec3f n = tf.getQuatRotation().transform(s.n);
00735
00736 generateCoordinateSystem(n, bv.axis[1], bv.axis[2]);
00737 bv.axis[0] = n;
00738
00739 bv.l[0] = std::numeric_limits<FCL_REAL>::max();
00740 bv.l[1] = std::numeric_limits<FCL_REAL>::max();
00741
00742 bv.r = 0;
00743
00744 Vec3f p = s.n * s.d;
00745 bv.Tr = tf.transform(p);
00746 }
00747
00748 template<>
00749 void computeBV<OBBRSS, Plane>(const Plane& s, const Transform3f& tf, OBBRSS& bv)
00750 {
00751 computeBV<OBB, Plane>(s, tf, bv.obb);
00752 computeBV<RSS, Plane>(s, tf, bv.rss);
00753 }
00754
00755 template<>
00756 void computeBV<kIOS, Plane>(const Plane& s, const Transform3f& tf, kIOS& bv)
00757 {
00758 bv.num_spheres = 1;
00759 computeBV<OBB, Plane>(s, tf, bv.obb);
00760 bv.spheres[0].o = Vec3f();
00761 bv.spheres[0].r = std::numeric_limits<FCL_REAL>::max();
00762 }
00763
00764 template<>
00765 void computeBV<KDOP<16>, Plane>(const Plane& s, const Transform3f& tf, KDOP<16>& bv)
00766 {
00767 Plane new_s = transform(s, tf);
00768 const Vec3f& n = new_s.n;
00769 const FCL_REAL& d = new_s.d;
00770
00771 const std::size_t D = 8;
00772
00773 for(std::size_t i = 0; i < D; ++i)
00774 bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
00775 for(std::size_t i = D; i < 2 * D; ++i)
00776 bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
00777
00778 if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00779 {
00780 if(n[0] > 0) bv.dist(0) = bv.dist(D) = d;
00781 else bv.dist(0) = bv.dist(D) = -d;
00782 }
00783 else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00784 {
00785 if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d;
00786 else bv.dist(1) = bv.dist(D + 1) = -d;
00787 }
00788 else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
00789 {
00790 if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d;
00791 else bv.dist(2) = bv.dist(D + 2) = -d;
00792 }
00793 else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
00794 {
00795 bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
00796 }
00797 else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
00798 {
00799 bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
00800 }
00801 else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
00802 {
00803 bv.dist(6) = bv.dist(D + 5) = n[1] * d * 2;
00804 }
00805 else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
00806 {
00807 bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
00808 }
00809 else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
00810 {
00811 bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
00812 }
00813 }
00814
00815 template<>
00816 void computeBV<KDOP<18>, Plane>(const Plane& s, const Transform3f& tf, KDOP<18>& bv)
00817 {
00818 Plane new_s = transform(s, tf);
00819 const Vec3f& n = new_s.n;
00820 const FCL_REAL& d = new_s.d;
00821
00822 const std::size_t D = 9;
00823
00824 for(std::size_t i = 0; i < D; ++i)
00825 bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
00826 for(std::size_t i = D; i < 2 * D; ++i)
00827 bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
00828
00829 if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00830 {
00831 if(n[0] > 0) bv.dist(0) = bv.dist(D) = d;
00832 else bv.dist(0) = bv.dist(D) = -d;
00833 }
00834 else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00835 {
00836 if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d;
00837 else bv.dist(1) = bv.dist(D + 1) = -d;
00838 }
00839 else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
00840 {
00841 if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d;
00842 else bv.dist(2) = bv.dist(D + 2) = -d;
00843 }
00844 else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
00845 {
00846 bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
00847 }
00848 else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
00849 {
00850 bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
00851 }
00852 else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
00853 {
00854 bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2;
00855 }
00856 else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
00857 {
00858 bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
00859 }
00860 else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
00861 {
00862 bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
00863 }
00864 else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
00865 {
00866 bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2;
00867 }
00868 }
00869
00870 template<>
00871 void computeBV<KDOP<24>, Plane>(const Plane& s, const Transform3f& tf, KDOP<24>& bv)
00872 {
00873 Plane new_s = transform(s, tf);
00874 const Vec3f& n = new_s.n;
00875 const FCL_REAL& d = new_s.d;
00876
00877 const std::size_t D = 12;
00878
00879 for(std::size_t i = 0; i < D; ++i)
00880 bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
00881 for(std::size_t i = D; i < 2 * D; ++i)
00882 bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
00883
00884 if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00885 {
00886 if(n[0] > 0) bv.dist(0) = bv.dist(D) = d;
00887 else bv.dist(0) = bv.dist(D) = -d;
00888 }
00889 else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00890 {
00891 if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d;
00892 else bv.dist(1) = bv.dist(D + 1) = -d;
00893 }
00894 else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
00895 {
00896 if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d;
00897 else bv.dist(2) = bv.dist(D + 2) = -d;
00898 }
00899 else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
00900 {
00901 bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
00902 }
00903 else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
00904 {
00905 bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
00906 }
00907 else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
00908 {
00909 bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2;
00910 }
00911 else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
00912 {
00913 bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
00914 }
00915 else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
00916 {
00917 bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
00918 }
00919 else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
00920 {
00921 bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2;
00922 }
00923 else if(n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
00924 {
00925 bv.dist(9) = bv.dist(D + 9) = n[0] * d * 3;
00926 }
00927 else if(n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
00928 {
00929 bv.dist(10) = bv.dist(D + 10) = n[0] * d * 3;
00930 }
00931 else if(n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
00932 {
00933 bv.dist(11) = bv.dist(D + 11) = n[1] * d * 3;
00934 }
00935 }
00936
00937
00938 void constructBox(const AABB& bv, Box& box, Transform3f& tf)
00939 {
00940 box = Box(bv.max_ - bv.min_);
00941 tf = Transform3f(bv.center());
00942 }
00943
00944 void constructBox(const OBB& bv, Box& box, Transform3f& tf)
00945 {
00946 box = Box(bv.extent * 2);
00947 tf = Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
00948 bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
00949 bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.To);
00950 }
00951
00952 void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf)
00953 {
00954 box = Box(bv.obb.extent * 2);
00955 tf = Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
00956 bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
00957 bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
00958 }
00959
00960 void constructBox(const kIOS& bv, Box& box, Transform3f& tf)
00961 {
00962 box = Box(bv.obb.extent * 2);
00963 tf = Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
00964 bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
00965 bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
00966 }
00967
00968 void constructBox(const RSS& bv, Box& box, Transform3f& tf)
00969 {
00970 box = Box(bv.width(), bv.height(), bv.depth());
00971 tf = Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
00972 bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
00973 bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.Tr);
00974 }
00975
00976 void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf)
00977 {
00978 box = Box(bv.width(), bv.height(), bv.depth());
00979 tf = Transform3f(bv.center());
00980 }
00981
00982 void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf)
00983 {
00984 box = Box(bv.width(), bv.height(), bv.depth());
00985 tf = Transform3f(bv.center());
00986 }
00987
00988 void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf)
00989 {
00990 box = Box(bv.width(), bv.height(), bv.depth());
00991 tf = Transform3f(bv.center());
00992 }
00993
00994
00995
00996 void constructBox(const AABB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
00997 {
00998 box = Box(bv.max_ - bv.min_);
00999 tf = tf_bv * Transform3f(bv.center());
01000 }
01001
01002 void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
01003 {
01004 box = Box(bv.extent * 2);
01005 tf = tf_bv *Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
01006 bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
01007 bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.To);
01008 }
01009
01010 void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
01011 {
01012 box = Box(bv.obb.extent * 2);
01013 tf = tf_bv * Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
01014 bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
01015 bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
01016 }
01017
01018 void constructBox(const kIOS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
01019 {
01020 box = Box(bv.obb.extent * 2);
01021 tf = tf_bv * Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
01022 bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
01023 bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
01024 }
01025
01026 void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
01027 {
01028 box = Box(bv.width(), bv.height(), bv.depth());
01029 tf = tf_bv * Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
01030 bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
01031 bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.Tr);
01032 }
01033
01034 void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
01035 {
01036 box = Box(bv.width(), bv.height(), bv.depth());
01037 tf = tf_bv * Transform3f(bv.center());
01038 }
01039
01040 void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
01041 {
01042 box = Box(bv.width(), bv.height(), bv.depth());
01043 tf = tf_bv * Transform3f(bv.center());
01044 }
01045
01046 void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
01047 {
01048 box = Box(bv.width(), bv.height(), bv.depth());
01049 tf = tf_bv * Transform3f(bv.center());
01050 }
01051
01052
01053
01054 }