narrowphase.h
Go to the documentation of this file.
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::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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines

fcl
Author(s): Jia Pan
autogenerated on Tue Jan 15 2013 16:05:30