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
00037 #ifndef FCL_NARROWPHASE_H
00038 #define FCL_NARROWPHASE_H
00039
00040 #include "fcl/narrowphase/gjk.h"
00041 #include "fcl/narrowphase/gjk_libccd.h"
00042
00043
00044
00045 namespace fcl
00046 {
00047
00049 struct GJKSolver_libccd
00050 {
00052 template<typename S1, typename S2>
00053 bool shapeIntersect(const S1& s1, const Transform3f& tf1,
00054 const S2& s2, const Transform3f& tf2,
00055 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
00056 {
00057 void* o1 = details::GJKInitializer<S1>::createGJKObject(s1, tf1);
00058 void* o2 = details::GJKInitializer<S2>::createGJKObject(s2, tf2);
00059
00060 bool res = details::GJKCollide(o1, details::GJKInitializer<S1>::getSupportFunction(), details::GJKInitializer<S1>::getCenterFunction(),
00061 o2, details::GJKInitializer<S2>::getSupportFunction(), details::GJKInitializer<S2>::getCenterFunction(),
00062 max_collision_iterations, collision_tolerance,
00063 contact_points, penetration_depth, normal);
00064
00065 details::GJKInitializer<S1>::deleteGJKObject(o1);
00066 details::GJKInitializer<S2>::deleteGJKObject(o2);
00067
00068 return res;
00069 }
00070
00072 template<typename S>
00073 bool shapeTriangleIntersect(const S& s, const Transform3f& tf,
00074 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
00075 {
00076 void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
00077 void* o2 = details::triCreateGJKObject(P1, P2, P3);
00078
00079 bool res = details::GJKCollide(o1, details::GJKInitializer<S>::getSupportFunction(), details::GJKInitializer<S>::getCenterFunction(),
00080 o2, details::triGetSupportFunction(), details::triGetCenterFunction(),
00081 max_collision_iterations, collision_tolerance,
00082 contact_points, penetration_depth, normal);
00083
00084 details::GJKInitializer<S>::deleteGJKObject(o1);
00085 details::triDeleteGJKObject(o2);
00086
00087 return res;
00088 }
00089
00091 template<typename S>
00092 bool shapeTriangleIntersect(const S& s, const Transform3f& tf1,
00093 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
00094 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
00095 {
00096 void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf1);
00097 void* o2 = details::triCreateGJKObject(P1, P2, P3, tf2);
00098
00099 bool res = details::GJKCollide(o1, details::GJKInitializer<S>::getSupportFunction(), details::GJKInitializer<S>::getCenterFunction(),
00100 o2, details::triGetSupportFunction(), details::triGetCenterFunction(),
00101 max_collision_iterations, collision_tolerance,
00102 contact_points, penetration_depth, normal);
00103
00104 details::GJKInitializer<S>::deleteGJKObject(o1);
00105 details::triDeleteGJKObject(o2);
00106
00107 return res;
00108 }
00109
00110
00112 template<typename S1, typename S2>
00113 bool shapeDistance(const S1& s1, const Transform3f& tf1,
00114 const S2& s2, const Transform3f& tf2,
00115 FCL_REAL* dist) const
00116 {
00117 void* o1 = details::GJKInitializer<S1>::createGJKObject(s1, tf1);
00118 void* o2 = details::GJKInitializer<S2>::createGJKObject(s2, tf2);
00119
00120 bool res = details::GJKDistance(o1, details::GJKInitializer<S1>::getSupportFunction(),
00121 o2, details::GJKInitializer<S2>::getSupportFunction(),
00122 max_distance_iterations, distance_tolerance,
00123 dist);
00124
00125 details::GJKInitializer<S1>::deleteGJKObject(o1);
00126 details::GJKInitializer<S2>::deleteGJKObject(o2);
00127
00128 return res;
00129 }
00130
00131
00133 template<typename S>
00134 bool shapeTriangleDistance(const S& s, const Transform3f& tf,
00135 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
00136 FCL_REAL* dist) const
00137 {
00138 void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
00139 void* o2 = details::triCreateGJKObject(P1, P2, P3);
00140
00141 bool res = details::GJKDistance(o1, details::GJKInitializer<S>::getSupportFunction(),
00142 o2, details::triGetSupportFunction(),
00143 max_distance_iterations, distance_tolerance,
00144 dist);
00145
00146 details::GJKInitializer<S>::deleteGJKObject(o1);
00147 details::triDeleteGJKObject(o2);
00148
00149 return res;
00150 }
00151
00153 template<typename S>
00154 bool shapeTriangleDistance(const S& s, const Transform3f& tf1,
00155 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
00156 FCL_REAL* dist) const
00157 {
00158 void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf1);
00159 void* o2 = details::triCreateGJKObject(P1, P2, P3, tf2);
00160
00161 bool res = details::GJKDistance(o1, details::GJKInitializer<S>::getSupportFunction(),
00162 o2, details::triGetSupportFunction(),
00163 max_distance_iterations, distance_tolerance,
00164 dist);
00165
00166 details::GJKInitializer<S>::deleteGJKObject(o1);
00167 details::triDeleteGJKObject(o2);
00168
00169 return res;
00170 }
00171
00172
00174 GJKSolver_libccd()
00175 {
00176 max_collision_iterations = 500;
00177 max_distance_iterations = 1000;
00178 collision_tolerance = 1e-6;
00179 distance_tolerance = 1e-6;
00180 }
00181
00183 unsigned int max_collision_iterations;
00184
00186 unsigned int max_distance_iterations;
00187
00189 FCL_REAL collision_tolerance;
00190
00192 FCL_REAL distance_tolerance;
00193
00194
00195 };
00196
00197
00199 template<>
00200 bool GJKSolver_libccd::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
00201 const Sphere& s2, const Transform3f& tf2,
00202 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00203
00205 template<>
00206 bool GJKSolver_libccd::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
00207 const Box& s2, const Transform3f& tf2,
00208 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00209
00210 template<>
00211 bool GJKSolver_libccd::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const Transform3f& tf1,
00212 const Halfspace& s2, const Transform3f& tf2,
00213 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00214
00215 template<>
00216 bool GJKSolver_libccd::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
00217 const Halfspace& s2, const Transform3f& tf2,
00218 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00219
00220 template<>
00221 bool GJKSolver_libccd::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, const Transform3f& tf1,
00222 const Halfspace& s2, const Transform3f& tf2,
00223 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00224
00225 template<>
00226 bool GJKSolver_libccd::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, const Transform3f& tf1,
00227 const Halfspace& s2, const Transform3f& tf2,
00228 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00229
00230 template<>
00231 bool GJKSolver_libccd::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Transform3f& tf1,
00232 const Halfspace& s2, const Transform3f& tf2,
00233 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00234
00235 template<>
00236 bool GJKSolver_libccd::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
00237 const Halfspace& s2, const Transform3f& tf2,
00238 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00239
00240 template<>
00241 bool GJKSolver_libccd::shapeIntersect<Plane, Halfspace>(const Plane& s1, const Transform3f& tf1,
00242 const Halfspace& s2, const Transform3f& tf2,
00243 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00244
00245 template<>
00246 bool GJKSolver_libccd::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Transform3f& tf1,
00247 const Plane& s2, const Transform3f& tf2,
00248 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00249
00250 template<>
00251 bool GJKSolver_libccd::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
00252 const Plane& s2, const Transform3f& tf2,
00253 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00254
00255 template<>
00256 bool GJKSolver_libccd::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Transform3f& tf1,
00257 const Plane& s2, const Transform3f& tf2,
00258 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00259
00260 template<>
00261 bool GJKSolver_libccd::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const Transform3f& tf1,
00262 const Plane& s2, const Transform3f& tf2,
00263 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00264
00265 template<>
00266 bool GJKSolver_libccd::shapeIntersect<Cone, Plane>(const Cone& s1, const Transform3f& tf1,
00267 const Plane& s2, const Transform3f& tf2,
00268 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00269
00270 template<>
00271 bool GJKSolver_libccd::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
00272 const Plane& s2, const Transform3f& tf2,
00273 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00274
00275 template<>
00276 bool GJKSolver_libccd::shapeIntersect<Plane, Plane>(const Plane& s1, const Transform3f& tf1,
00277 const Plane& s2, const Transform3f& tf2,
00278 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00279
00281 template<>
00282 bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
00283 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00284
00286 template<>
00287 bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
00288 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00289
00290
00291 template<>
00292 bool GJKSolver_libccd::shapeTriangleIntersect(const Halfspace& s, const Transform3f& tf1,
00293 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00294
00295 template<>
00296 bool GJKSolver_libccd::shapeTriangleIntersect(const Plane& s, const Transform3f& tf1,
00297 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00298
00300 template<>
00301 bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
00302 const Sphere& s2, const Transform3f& tf2,
00303 FCL_REAL* dist) const;
00304
00306 template<>
00307 bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
00308 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
00309 FCL_REAL* dist) const;
00310
00312 template<>
00313 bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
00314 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
00315 FCL_REAL* dist) const;
00316
00317
00319 struct GJKSolver_indep
00320 {
00322 template<typename S1, typename S2>
00323 bool shapeIntersect(const S1& s1, const Transform3f& tf1,
00324 const S2& s2, const Transform3f& tf2,
00325 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
00326 {
00327 Vec3f guess(1, 0, 0);
00328 details::MinkowskiDiff shape;
00329 shape.shapes[0] = &s1;
00330 shape.shapes[1] = &s2;
00331 shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
00332 shape.toshape0 = tf1.inverseTimes(tf2);
00333
00334 details::GJK gjk(gjk_max_iterations, gjk_tolerance);
00335 details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
00336 switch(gjk_status)
00337 {
00338 case details::GJK::Inside:
00339 {
00340 details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
00341 details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
00342 if(epa_status != details::EPA::Failed)
00343 {
00344 Vec3f w0;
00345 for(size_t i = 0; i < epa.result.rank; ++i)
00346 {
00347 w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
00348 }
00349 if(penetration_depth) *penetration_depth = -epa.depth;
00350 if(normal) *normal = -epa.normal;
00351 if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
00352 return true;
00353 }
00354 else return false;
00355 }
00356 break;
00357 default:
00358 ;
00359 }
00360
00361 return false;
00362 }
00363
00365 template<typename S>
00366 bool shapeTriangleIntersect(const S& s, const Transform3f& tf,
00367 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
00368 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
00369 {
00370 Triangle2 tri(P1, P2, P3);
00371 Vec3f guess(1, 0, 0);
00372 details::MinkowskiDiff shape;
00373 shape.shapes[0] = &s;
00374 shape.shapes[1] = &tri;
00375 shape.toshape1 = tf.getRotation();
00376 shape.toshape0 = inverse(tf);
00377
00378 details::GJK gjk(gjk_max_iterations, gjk_tolerance);
00379 details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
00380 switch(gjk_status)
00381 {
00382 case details::GJK::Inside:
00383 {
00384 details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
00385 details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
00386 if(epa_status != details::EPA::Failed)
00387 {
00388 Vec3f w0;
00389 for(size_t i = 0; i < epa.result.rank; ++i)
00390 {
00391 w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
00392 }
00393 if(penetration_depth) *penetration_depth = -epa.depth;
00394 if(normal) *normal = -epa.normal;
00395 if(contact_points) *contact_points = tf.transform(w0 - epa.normal*(epa.depth *0.5));
00396 return true;
00397 }
00398 else return false;
00399 }
00400 break;
00401 default:
00402 ;
00403 }
00404
00405 return false;
00406 }
00407
00409 template<typename S>
00410 bool shapeTriangleIntersect(const S& s, const Transform3f& tf1,
00411 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
00412 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
00413 {
00414 Triangle2 tri(P1, P2, P3);
00415 Vec3f guess(1, 0, 0);
00416 details::MinkowskiDiff shape;
00417 shape.shapes[0] = &s;
00418 shape.shapes[1] = &tri;
00419 shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
00420 shape.toshape0 = tf1.inverseTimes(tf2);
00421
00422 details::GJK gjk(gjk_max_iterations, gjk_tolerance);
00423 details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
00424 switch(gjk_status)
00425 {
00426 case details::GJK::Inside:
00427 {
00428 details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
00429 details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
00430 if(epa_status != details::EPA::Failed)
00431 {
00432 Vec3f w0;
00433 for(size_t i = 0; i < epa.result.rank; ++i)
00434 {
00435 w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
00436 }
00437 if(penetration_depth) *penetration_depth = -epa.depth;
00438 if(normal) *normal = -epa.normal;
00439 if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
00440 return true;
00441 }
00442 else return false;
00443 }
00444 break;
00445 default:
00446 ;
00447 }
00448
00449 return false;
00450 }
00451
00453 template<typename S1, typename S2>
00454 bool shapeDistance(const S1& s1, const Transform3f& tf1,
00455 const S2& s2, const Transform3f& tf2,
00456 FCL_REAL* distance) const
00457 {
00458 Vec3f guess(1, 0, 0);
00459 details::MinkowskiDiff shape;
00460 shape.shapes[0] = &s1;
00461 shape.shapes[1] = &s2;
00462 shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
00463 shape.toshape0 = tf1.inverseTimes(tf2);
00464
00465 details::GJK gjk(gjk_max_iterations, gjk_tolerance);
00466 details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
00467 if(gjk_status == details::GJK::Valid)
00468 {
00469 Vec3f w0, w1;
00470 for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
00471 {
00472 FCL_REAL p = gjk.getSimplex()->p[i];
00473 w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
00474 w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
00475 }
00476
00477 *distance = (w0 - w1).length();
00478 return true;
00479 }
00480 else
00481 {
00482 *distance = -1;
00483 return false;
00484 }
00485 }
00486
00488 template<typename S>
00489 bool shapeTriangleDistance(const S& s, const Transform3f& tf,
00490 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
00491 FCL_REAL* distance) const
00492 {
00493 Triangle2 tri(P1, P2, P3);
00494 Vec3f guess(1, 0, 0);
00495 details::MinkowskiDiff shape;
00496 shape.shapes[0] = &s;
00497 shape.shapes[1] = &tri;
00498 shape.toshape1 = tf.getRotation();
00499 shape.toshape0 = inverse(tf);
00500
00501 details::GJK gjk(gjk_max_iterations, gjk_tolerance);
00502 details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
00503 if(gjk_status == details::GJK::Valid)
00504 {
00505 Vec3f w0, w1;
00506 for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
00507 {
00508 FCL_REAL p = gjk.getSimplex()->p[i];
00509 w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
00510 w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
00511 }
00512
00513 *distance = (w0 - w1).length();
00514 return true;
00515 }
00516 else
00517 {
00518 *distance = -1;
00519 return false;
00520 }
00521 }
00522
00524 template<typename S>
00525 bool shapeTriangleDistance(const S& s, const Transform3f& tf1,
00526 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
00527 FCL_REAL* distance) const
00528 {
00529 Triangle2 tri(P1, P2, P3);
00530 Vec3f guess(1, 0, 0);
00531 details::MinkowskiDiff shape;
00532 shape.shapes[0] = &s;
00533 shape.shapes[1] = &tri;
00534 shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
00535 shape.toshape0 = tf1.inverseTimes(tf2);
00536
00537 details::GJK gjk(gjk_max_iterations, gjk_tolerance);
00538 details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
00539 if(gjk_status == details::GJK::Valid)
00540 {
00541 Vec3f w0, w1;
00542 for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
00543 {
00544 FCL_REAL p = gjk.getSimplex()->p[i];
00545 w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
00546 w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
00547 }
00548
00549 *distance = (w0 - w1).length();
00550 return true;
00551 }
00552 else
00553 {
00554 *distance = -1;
00555 return false;
00556 }
00557 }
00558
00560 GJKSolver_indep()
00561 {
00562 gjk_max_iterations = 128;
00563 gjk_tolerance = 1e-6;
00564 epa_max_face_num = 128;
00565 epa_max_vertex_num = 64;
00566 epa_max_iterations = 255;
00567 epa_tolerance = 1e-6;
00568 }
00569
00571 unsigned int epa_max_face_num;
00572
00574 unsigned int epa_max_vertex_num;
00575
00577 unsigned int epa_max_iterations;
00578
00580 FCL_REAL epa_tolerance;
00581
00583 FCL_REAL gjk_tolerance;
00584
00586 FCL_REAL gjk_max_iterations;
00587 };
00588
00590 template<>
00591 bool GJKSolver_indep::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
00592 const Sphere& s2, const Transform3f& tf2,
00593 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00594
00596 template<>
00597 bool GJKSolver_indep::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
00598 const Box& s2, const Transform3f& tf2,
00599 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00600
00601 template<>
00602 bool GJKSolver_indep::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const Transform3f& tf1,
00603 const Halfspace& s2, const Transform3f& tf2,
00604 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00605
00606 template<>
00607 bool GJKSolver_indep::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
00608 const Halfspace& s2, const Transform3f& tf2,
00609 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00610
00611 template<>
00612 bool GJKSolver_indep::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, const Transform3f& tf1,
00613 const Halfspace& s2, const Transform3f& tf2,
00614 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00615
00616 template<>
00617 bool GJKSolver_indep::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, const Transform3f& tf1,
00618 const Halfspace& s2, const Transform3f& tf2,
00619 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00620
00621 template<>
00622 bool GJKSolver_indep::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Transform3f& tf1,
00623 const Halfspace& s2, const Transform3f& tf2,
00624 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00625
00626 template<>
00627 bool GJKSolver_indep::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
00628 const Halfspace& s2, const Transform3f& tf2,
00629 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00630
00631 template<>
00632 bool GJKSolver_indep::shapeIntersect<Plane, Halfspace>(const Plane& s1, const Transform3f& tf1,
00633 const Halfspace& s2, const Transform3f& tf2,
00634 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00635
00636 template<>
00637 bool GJKSolver_indep::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Transform3f& tf1,
00638 const Plane& s2, const Transform3f& tf2,
00639 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00640
00641 template<>
00642 bool GJKSolver_indep::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
00643 const Plane& s2, const Transform3f& tf2,
00644 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00645
00646 template<>
00647 bool GJKSolver_indep::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Transform3f& tf1,
00648 const Plane& s2, const Transform3f& tf2,
00649 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00650
00651 template<>
00652 bool GJKSolver_indep::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const Transform3f& tf1,
00653 const Plane& s2, const Transform3f& tf2,
00654 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00655
00656 template<>
00657 bool GJKSolver_indep::shapeIntersect<Cone, Plane>(const Cone& s1, const Transform3f& tf1,
00658 const Plane& s2, const Transform3f& tf2,
00659 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00660
00661 template<>
00662 bool GJKSolver_indep::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
00663 const Plane& s2, const Transform3f& tf2,
00664 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00665
00666 template<>
00667 bool GJKSolver_indep::shapeIntersect<Plane, Plane>(const Plane& s1, const Transform3f& tf1,
00668 const Plane& s2, const Transform3f& tf2,
00669 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00670
00672 template<>
00673 bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
00674 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00675
00677 template<>
00678 bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
00679 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00680
00681 template<>
00682 bool GJKSolver_indep::shapeTriangleIntersect(const Halfspace& s, const Transform3f& tf1,
00683 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00684
00685 template<>
00686 bool GJKSolver_indep::shapeTriangleIntersect(const Plane& s, const Transform3f& tf1,
00687 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00688
00689
00691 template<>
00692 bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
00693 const Sphere& s2, const Transform3f& tf2,
00694 FCL_REAL* dist) const;
00695
00697 template<>
00698 bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
00699 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
00700 FCL_REAL* dist) const;
00701
00703 template<>
00704 bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
00705 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
00706 FCL_REAL* dist) const;
00707
00708
00709 }
00710
00711 #endif