All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
fcl » include » fcl » narrowphase

narrowphase.h

00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
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::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
00207                                               const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00208 
00210 template<> 
00211 bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
00212                                               const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00213 
00215 template<>
00216 bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
00217                                                      const Sphere& s2, const Transform3f& tf2,
00218                                                      FCL_REAL* dist) const;
00219 
00221 template<>
00222 bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
00223                                                      const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, 
00224                                                      FCL_REAL* dist) const;
00225 
00227 template<> 
00228 bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1, 
00229                                                      const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
00230                                                      FCL_REAL* dist) const;
00231 
00233 template<>
00234 bool GJKSolver_libccd::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
00235                                                 const Box& s2, const Transform3f& tf2,
00236                                                 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00237 
00238 
00240 struct GJKSolver_indep
00241 {
00243   template<typename S1, typename S2>
00244   bool shapeIntersect(const S1& s1, const Transform3f& tf1,
00245                       const S2& s2, const Transform3f& tf2,
00246                       Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
00247   {
00248     Vec3f guess(1, 0, 0);
00249     details::MinkowskiDiff shape;
00250     shape.shapes[0] = &s1;
00251     shape.shapes[1] = &s2;
00252     shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
00253     shape.toshape0 = tf1.inverseTimes(tf2);
00254   
00255     details::GJK gjk(gjk_max_iterations, gjk_tolerance);
00256     details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
00257     switch(gjk_status)
00258     {
00259     case details::GJK::Inside:
00260       {
00261         details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
00262         details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
00263         if(epa_status != details::EPA::Failed)
00264         {
00265           Vec3f w0;
00266           for(size_t i = 0; i < epa.result.rank; ++i)
00267           {
00268             w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
00269           }
00270           if(penetration_depth) *penetration_depth = -epa.depth;
00271           if(normal) *normal = -epa.normal;
00272           if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
00273           return true;
00274         }
00275         else return false;
00276       }
00277       break;
00278     default:
00279       ;
00280     }
00281 
00282     return false;
00283   }
00284 
00286   template<typename S>
00287   bool shapeTriangleIntersect(const S& s, const Transform3f& tf,
00288                               const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
00289                               Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
00290   {
00291     Triangle2 tri(P1, P2, P3);
00292     Vec3f guess(1, 0, 0);
00293     details::MinkowskiDiff shape;
00294     shape.shapes[0] = &s;
00295     shape.shapes[1] = &tri;
00296     shape.toshape1 = tf.getRotation();
00297     shape.toshape0 = inverse(tf);
00298   
00299     details::GJK gjk(gjk_max_iterations, gjk_tolerance);
00300     details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
00301     switch(gjk_status)
00302     {
00303     case details::GJK::Inside:
00304       {
00305         details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
00306         details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
00307         if(epa_status != details::EPA::Failed)
00308         {
00309           Vec3f w0;
00310           for(size_t i = 0; i < epa.result.rank; ++i)
00311           {
00312             w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
00313           }
00314           if(penetration_depth) *penetration_depth = -epa.depth;
00315           if(normal) *normal = -epa.normal;
00316           if(contact_points) *contact_points = tf.transform(w0 - epa.normal*(epa.depth *0.5));
00317           return true;
00318         }
00319         else return false;
00320       }
00321       break;
00322     default:
00323       ;
00324     }
00325 
00326     return false;
00327   }
00328 
00330   template<typename S>
00331   bool shapeTriangleIntersect(const S& s, const Transform3f& tf1,
00332                               const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
00333                               Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
00334   {
00335     Triangle2 tri(P1, P2, P3);
00336     Vec3f guess(1, 0, 0);
00337     details::MinkowskiDiff shape;
00338     shape.shapes[0] = &s;
00339     shape.shapes[1] = &tri;
00340     shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
00341     shape.toshape0 = tf1.inverseTimes(tf2);
00342   
00343     details::GJK gjk(gjk_max_iterations, gjk_tolerance);
00344     details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
00345     switch(gjk_status)
00346     {
00347     case details::GJK::Inside:
00348       {
00349         details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
00350         details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
00351         if(epa_status != details::EPA::Failed)
00352         {
00353           Vec3f w0;
00354           for(size_t i = 0; i < epa.result.rank; ++i)
00355           {
00356             w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
00357           }
00358           if(penetration_depth) *penetration_depth = -epa.depth;
00359           if(normal) *normal = -epa.normal;
00360           if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
00361           return true;
00362         }
00363         else return false;
00364       }
00365       break;
00366     default:
00367       ;
00368     }
00369 
00370     return false;
00371   }
00372 
00374   template<typename S1, typename S2>
00375   bool shapeDistance(const S1& s1, const Transform3f& tf1,
00376                      const S2& s2, const Transform3f& tf2,
00377                      FCL_REAL* distance) const
00378   {
00379     Vec3f guess(1, 0, 0);
00380     details::MinkowskiDiff shape;
00381     shape.shapes[0] = &s1;
00382     shape.shapes[1] = &s2;
00383     shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
00384     shape.toshape0 = tf1.inverseTimes(tf2);
00385 
00386     details::GJK gjk(gjk_max_iterations, gjk_tolerance);
00387     details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
00388     if(gjk_status == details::GJK::Valid)
00389     {
00390       Vec3f w0, w1;
00391       for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
00392       {
00393         FCL_REAL p = gjk.getSimplex()->p[i];
00394         w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
00395         w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
00396       }
00397 
00398       *distance = (w0 - w1).length();
00399       return true;
00400     }
00401     else
00402     {
00403       *distance = -1;
00404       return false;
00405     }
00406   }
00407 
00409   template<typename S>
00410   bool shapeTriangleDistance(const S& s, const Transform3f& tf,
00411                              const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
00412                              FCL_REAL* distance) 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 = tf.getRotation();
00420     shape.toshape0 = inverse(tf);
00421 
00422     details::GJK gjk(gjk_max_iterations, gjk_tolerance);
00423     details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
00424     if(gjk_status == details::GJK::Valid)
00425     {
00426       Vec3f w0, w1;
00427       for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
00428       {
00429         FCL_REAL p = gjk.getSimplex()->p[i];
00430         w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
00431         w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
00432       }
00433 
00434       *distance = (w0 - w1).length();
00435       return true;
00436     }
00437     else
00438     {
00439       *distance = -1;
00440       return false;
00441     }
00442   }
00443 
00445   template<typename S>
00446   bool shapeTriangleDistance(const S& s, const Transform3f& tf1,
00447                              const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
00448                              FCL_REAL* distance) const
00449   {
00450     Triangle2 tri(P1, P2, P3);
00451     Vec3f guess(1, 0, 0);
00452     details::MinkowskiDiff shape;
00453     shape.shapes[0] = &s;
00454     shape.shapes[1] = &tri;
00455     shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
00456     shape.toshape0 = tf1.inverseTimes(tf2);
00457 
00458     details::GJK gjk(gjk_max_iterations, gjk_tolerance);
00459     details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
00460     if(gjk_status == details::GJK::Valid)
00461     {
00462       Vec3f w0, w1;
00463       for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
00464       {
00465         FCL_REAL p = gjk.getSimplex()->p[i];
00466         w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
00467         w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
00468       }
00469 
00470       *distance = (w0 - w1).length();
00471       return true;
00472     }
00473     else
00474     {
00475       *distance = -1;
00476       return false;
00477     }
00478   }
00479 
00481   GJKSolver_indep()
00482   {
00483     gjk_max_iterations = 128;
00484     gjk_tolerance = 1e-6;
00485     epa_max_face_num = 128;
00486     epa_max_vertex_num = 64;
00487     epa_max_iterations = 255;
00488     epa_tolerance = 1e-6;
00489   }
00490 
00492   unsigned int epa_max_face_num;
00493 
00495   unsigned int epa_max_vertex_num;
00496 
00498   unsigned int epa_max_iterations;
00499 
00501   FCL_REAL epa_tolerance;
00502 
00504   FCL_REAL gjk_tolerance;
00505 
00507   FCL_REAL gjk_max_iterations;
00508 };
00509 
00511 template<>
00512 bool GJKSolver_indep::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
00513                                                       const Sphere& s2, const Transform3f& tf2,
00514                                                       Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00515 
00517 template<> 
00518 bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
00519                                               const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00520 
00522 template<> 
00523 bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
00524                                              const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00525 
00527 template<>
00528 bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
00529                                                     const Sphere& s2, const Transform3f& tf2,
00530                                                     FCL_REAL* dist) const;
00531 
00533 template<>
00534 bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
00535                                                     const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, 
00536                                                     FCL_REAL* dist) const;
00537 
00539 template<> 
00540 bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1, 
00541                                                     const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
00542                                                     FCL_REAL* dist) const;
00543 
00545 template<>
00546 bool GJKSolver_indep::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
00547                                                 const Box& s2, const Transform3f& tf2,
00548                                                 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
00549 
00550 }
00551 
00552 #endif