gjk.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_GJK_H
00038 #define FCL_GJK_H
00039 
00040 #include "fcl/shape/geometric_shapes.h"
00041 #include "fcl/math/transform.h"
00042 
00043 namespace fcl
00044 {
00045 
00046 namespace details
00047 {
00048 
00050 Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir); 
00051 
00053 struct MinkowskiDiff
00054 {
00056   const ShapeBase* shapes[2];
00057 
00059   Matrix3f toshape1;
00060 
00062   Transform3f toshape0;
00063 
00064   MinkowskiDiff() { }
00065 
00067   inline Vec3f support0(const Vec3f& d) const
00068   {
00069     return getSupport(shapes[0], d);
00070   }
00071 
00073   inline Vec3f support1(const Vec3f& d) const
00074   {
00075     return toshape0.transform(getSupport(shapes[1], toshape1 * d));
00076   }
00077 
00078   inline Vec3f support(const Vec3f& d) const
00079   {
00080     return support0(d) - support1(-d);
00081   }
00082 
00083   inline Vec3f support(const Vec3f& d, size_t index) const
00084   {
00085     if(index)
00086       return support1(d);
00087     else
00088       return support0(d);
00089   }
00090 
00091 };
00092 
00097 FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, FCL_REAL* w, size_t& m);
00098 
00102 FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, FCL_REAL* w, size_t& m);
00103 
00107 FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, FCL_REAL* w, size_t& m);
00108 
00110 struct GJK
00111 {
00112   struct SimplexV
00113   {
00115     Vec3f d; 
00117     Vec3f w;
00118   };
00119 
00120   struct Simplex
00121   {
00123     SimplexV* c[4];
00125     FCL_REAL p[4];
00127     size_t rank; 
00128   };
00129 
00130   enum Status {Valid, Inside, Failed};
00131 
00132   MinkowskiDiff shape;
00133   Vec3f ray;
00134   FCL_REAL distance;
00135   Simplex simplices[2];
00136 
00137 
00138   GJK(unsigned int max_iterations_, FCL_REAL tolerance_)  : max_iterations(max_iterations_),
00139                                                             tolerance(tolerance_)
00140   {
00141     initialize(); 
00142   }
00143   
00144   void initialize();
00145 
00147   Status evaluate(const MinkowskiDiff& shape_, const Vec3f& guess);
00148 
00150   void getSupport(const Vec3f& d, SimplexV& sv) const;
00151 
00153   void removeVertex(Simplex& simplex);
00154 
00156   void appendVertex(Simplex& simplex, const Vec3f& v);
00157 
00159   bool encloseOrigin();
00160 
00162   inline Simplex* getSimplex() const
00163   {
00164     return simplex;
00165   }
00166   
00167 private:
00168   SimplexV store_v[4];
00169   SimplexV* free_v[4];
00170   size_t nfree;
00171   size_t current;
00172   Simplex* simplex;
00173   Status status;
00174 
00175   unsigned int max_iterations;
00176   FCL_REAL tolerance;
00177 
00178 };
00179 
00180 
00181 static const size_t EPA_MAX_FACES = 128;
00182 static const size_t EPA_MAX_VERTICES = 64;
00183 static const FCL_REAL EPA_EPS = 0.000001;
00184 static const size_t EPA_MAX_ITERATIONS = 255;
00185 
00187 struct EPA
00188 {
00189 private:
00190   typedef GJK::SimplexV SimplexV;
00191   struct SimplexF
00192   {
00193     Vec3f n;
00194     FCL_REAL d;
00195     SimplexV* c[3]; // a face has three vertices
00196     SimplexF* f[3]; // a face has three adjacent faces
00197     SimplexF* l[2]; // the pre and post faces in the list
00198     size_t e[3];
00199     size_t pass;
00200   };
00201 
00202   struct SimplexList
00203   {
00204     SimplexF* root;
00205     size_t count;
00206     SimplexList() : root(NULL), count(0) {}
00207     void append(SimplexF* face)
00208     {
00209       face->l[0] = NULL;
00210       face->l[1] = root;
00211       if(root) root->l[0] = face;
00212       root = face;
00213       ++count;
00214     }
00215 
00216     void remove(SimplexF* face)
00217     {
00218       if(face->l[1]) face->l[1]->l[0] = face->l[0];
00219       if(face->l[0]) face->l[0]->l[1] = face->l[1];
00220       if(face == root) root = face->l[1];
00221       --count;
00222     }
00223   };
00224 
00225   static inline void bind(SimplexF* fa, size_t ea, SimplexF* fb, size_t eb)
00226   {
00227     fa->e[ea] = eb; fa->f[ea] = fb;
00228     fb->e[eb] = ea; fb->f[eb] = fa;
00229   }
00230 
00231   struct SimplexHorizon
00232   {
00233     SimplexF* cf; // current face in the horizon
00234     SimplexF* ff; // first face in the horizon
00235     size_t nf; // number of faces in the horizon
00236     SimplexHorizon() : cf(NULL), ff(NULL), nf(0) {}
00237   };
00238 
00239 private:
00240   unsigned int max_face_num;
00241   unsigned int max_vertex_num;
00242   unsigned int max_iterations;
00243   FCL_REAL tolerance;
00244 
00245 public:
00246 
00247   enum Status {Valid, Touching, Degenerated, NonConvex, InvalidHull, OutOfFaces, OutOfVertices, AccuracyReached, FallBack, Failed};
00248   
00249   Status status;
00250   GJK::Simplex result;
00251   Vec3f normal;
00252   FCL_REAL depth;
00253   SimplexV* sv_store;
00254   SimplexF* fc_store;
00255   size_t nextsv;
00256   SimplexList hull, stock;
00257 
00258   EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, FCL_REAL tolerance_) : max_face_num(max_face_num_),
00259                                                                                                                      max_vertex_num(max_vertex_num_),
00260                                                                                                                      max_iterations(max_iterations_),
00261                                                                                                                      tolerance(tolerance_)
00262   {
00263     initialize();
00264   }
00265 
00266   ~EPA()
00267   {
00268     delete [] sv_store;
00269     delete [] fc_store;
00270   }
00271 
00272   void initialize();
00273 
00274   bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist);
00275 
00276   SimplexF* newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced);
00277 
00279   SimplexF* findBest();
00280 
00281   Status evaluate(GJK& gjk, const Vec3f& guess);
00282 
00284   bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon);  
00285 };
00286 
00287 
00288 } // details
00289 
00290 
00291 
00292 
00293 }
00294 
00295 
00296 #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