1 /*
2  * Software License Agreement (BSD License)
3  *
4  *  Copyright (c) 2011-2014, Willow Garage, Inc.
5  *  Copyright (c) 2014-2016, Open Source Robotics Foundation
6  *  All rights reserved.
7  *
8  *  Redistribution and use in source and binary forms, with or without
9  *  modification, are permitted provided that the following conditions
10  *  are met:
11  *
12  *   * Redistributions of source code must retain the above copyright
13  *     notice, this list of conditions and the following disclaimer.
14  *   * Redistributions in binary form must reproduce the above
15  *     copyright notice, this list of conditions and the following
16  *     disclaimer in the documentation and/or other materials provided
17  *     with the distribution.
18  *   * Neither the name of Open Source Robotics Foundation nor the names of its
19  *     contributors may be used to endorse or promote products derived
20  *     from this software without specific prior written permission.
21  *
22  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  *  POSSIBILITY OF SUCH DAMAGE.
34  */
35 
36 /** \author Jia Pan */
37 
38 #ifndef FCL_GJK_H
39 #define FCL_GJK_H
40 
41 #include "fcl/shape/geometric_shapes.h"
42 #include "fcl/math/transform.h"
43 
44 namespace fcl
45 {
46 
47 namespace details
48 {
49 
50 /// @brief the support function for shape
51 Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir);
52 
53 /// @brief Minkowski difference class of two shapes
54 struct MinkowskiDiff
55 {
56   /// @brief points to two shapes
57   const ShapeBase* shapes[2];
58 
59   /// @brief rotation from shape0 to shape1
60   Matrix3f toshape1;
61 
62   /// @brief transform from shape1 to shape0
63   Transform3f toshape0;
64 
MinkowskiDiffMinkowskiDiff65   MinkowskiDiff() { }
66 
67   /// @brief support function for shape0
support0MinkowskiDiff68   inline Vec3f support0(const Vec3f& d) const
69   {
70     return getSupport(shapes[0], d);
71   }
72 
73   /// @brief support function for shape1
support1MinkowskiDiff74   inline Vec3f support1(const Vec3f& d) const
75   {
76     return toshape0.transform(getSupport(shapes[1], toshape1 * d));
77   }
78 
79   /// @brief support function for the pair of shapes
supportMinkowskiDiff80   inline Vec3f support(const Vec3f& d) const
81   {
82     return support0(d) - support1(-d);
83   }
84 
85   /// @brief support function for the d-th shape (d = 0 or 1)
supportMinkowskiDiff86   inline Vec3f support(const Vec3f& d, size_t index) const
87   {
88     if(index)
89       return support1(d);
90     else
91       return support0(d);
92   }
93 
94   /// @brief support function for translating shape0, which is translating at velocity v
support0MinkowskiDiff95   inline Vec3f support0(const Vec3f& d, const Vec3f& v) const
96   {
97     if(d.dot(v) <= 0)
98       return getSupport(shapes[0], d);
99     else
100       return getSupport(shapes[0], d) + v;
101   }
102 
103   /// @brief support function for the pair of shapes, where shape0 is translating at velocity v
supportMinkowskiDiff104   inline Vec3f support(const Vec3f& d, const Vec3f& v) const
105   {
106     return support0(d, v) - support1(-d);
107   }
108 
109   /// @brief support function for the d-th shape (d = 0 or 1), where shape0 is translating at velocity v
supportMinkowskiDiff110   inline Vec3f support(const Vec3f& d, const Vec3f& v, size_t index) const
111   {
112     if(index)
113       return support1(d);
114     else
115       return support0(d, v);
116   }
117 };
118 
119 
120 /// @brief class for GJK algorithm
121 struct GJK
122 {
123   struct SimplexV
124   {
125     /// @brief support direction
126     Vec3f d;
127     /// @brieg support vector (i.e., the furthest point on the shape along the support direction)
128     Vec3f w;
129   };
130 
131   struct Simplex
132   {
133     /// @brief simplex vertex
134     SimplexV* c[4];
135     /// @brief weight
136     FCL_REAL p[4];
137     /// @brief size of simplex (number of vertices)
138     size_t rank;
139 
SimplexGJK::Simplex140     Simplex() : rank(0) {}
141   };
142 
143   enum Status {Valid, Inside, Failed};
144 
145   MinkowskiDiff shape;
146   Vec3f ray;
147   FCL_REAL distance;
148   Simplex simplices[2];
149 
150 
GJKGJK151   GJK(unsigned int max_iterations_, FCL_REAL tolerance_)  : max_iterations(max_iterations_),
152                                                             tolerance(tolerance_)
153   {
154     initialize();
155   }
156 
157   void initialize();
158 
159   /// @brief GJK algorithm, given the initial value guess
160   Status evaluate(const MinkowskiDiff& shape_, const Vec3f& guess);
161 
162   /// @brief apply the support function along a direction, the result is return in sv
163   void getSupport(const Vec3f& d, SimplexV& sv) const;
164 
165   /// @brief apply the support function along a direction, the result is return is sv, here shape0 is translating at velocity v
166   void getSupport(const Vec3f& d, const Vec3f& v, SimplexV& sv) const;
167 
168   /// @brief discard one vertex from the simplex
169   void removeVertex(Simplex& simplex);
170 
171   /// @brief append one vertex to the simplex
172   void appendVertex(Simplex& simplex, const Vec3f& v);
173 
174   /// @brief whether the simplex enclose the origin
175   bool encloseOrigin();
176 
177   /// @brief get the underlying simplex using in GJK, can be used for cache in next iteration
getSimplexGJK178   inline Simplex* getSimplex() const
179   {
180     return simplex;
181   }
182 
183   /// @brief get the guess from current simplex
184   Vec3f getGuessFromSimplex() const;
185 
186 private:
187   SimplexV store_v[4];
188   SimplexV* free_v[4];
189   size_t nfree;
190   size_t current;
191   Simplex* simplex;
192   Status status;
193 
194   unsigned int max_iterations;
195   FCL_REAL tolerance;
196 
197 };
198 
199 
200 static const size_t EPA_MAX_FACES = 128;
201 static const size_t EPA_MAX_VERTICES = 64;
202 static const FCL_REAL EPA_EPS = 0.000001;
203 static const size_t EPA_MAX_ITERATIONS = 255;
204 
205 /// @brief class for EPA algorithm
206 struct EPA
207 {
208 private:
209   typedef GJK::SimplexV SimplexV;
210   struct SimplexF
211   {
212     Vec3f n;
213     FCL_REAL d;
214     SimplexV* c[3]; // a face has three vertices
215     SimplexF* f[3]; // a face has three adjacent faces
216     SimplexF* l[2]; // the pre and post faces in the list
217     size_t e[3];
218     size_t pass;
219   };
220 
221   struct SimplexList
222   {
223     SimplexF* root;
224     size_t count;
SimplexListEPA::SimplexList225     SimplexList() : root(NULL), count(0) {}
appendEPA::SimplexList226     void append(SimplexF* face)
227     {
228       face->l[0] = NULL;
229       face->l[1] = root;
230       if(root) root->l[0] = face;
231       root = face;
232       ++count;
233     }
234 
removeEPA::SimplexList235     void remove(SimplexF* face)
236     {
237       if(face->l[1]) face->l[1]->l[0] = face->l[0];
238       if(face->l[0]) face->l[0]->l[1] = face->l[1];
239       if(face == root) root = face->l[1];
240       --count;
241     }
242   };
243 
bindEPA244   static inline void bind(SimplexF* fa, size_t ea, SimplexF* fb, size_t eb)
245   {
246     fa->e[ea] = eb; fa->f[ea] = fb;
247     fb->e[eb] = ea; fb->f[eb] = fa;
248   }
249 
250   struct SimplexHorizon
251   {
252     SimplexF* cf; // current face in the horizon
253     SimplexF* ff; // first face in the horizon
254     size_t nf; // number of faces in the horizon
SimplexHorizonEPA::SimplexHorizon255     SimplexHorizon() : cf(NULL), ff(NULL), nf(0) {}
256   };
257 
258 private:
259   unsigned int max_face_num;
260   unsigned int max_vertex_num;
261   unsigned int max_iterations;
262   FCL_REAL tolerance;
263 
264 public:
265 
266   enum Status {Valid, Touching, Degenerated, NonConvex, InvalidHull, OutOfFaces, OutOfVertices, AccuracyReached, FallBack, Failed};
267 
268   Status status;
269   GJK::Simplex result;
270   Vec3f normal;
271   FCL_REAL depth;
272   SimplexV* sv_store;
273   SimplexF* fc_store;
274   size_t nextsv;
275   SimplexList hull, stock;
276 
EPAEPA277   EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, FCL_REAL tolerance_) : max_face_num(max_face_num_),
278                                                                                                                      max_vertex_num(max_vertex_num_),
279                                                                                                                      max_iterations(max_iterations_),
280                                                                                                                      tolerance(tolerance_)
281   {
282     initialize();
283   }
284 
~EPAEPA285   ~EPA()
286   {
287     delete [] sv_store;
288     delete [] fc_store;
289   }
290 
291   void initialize();
292 
293   bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist);
294 
295   SimplexF* newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced);
296 
297   /// @brief Find the best polytope face to split
298   SimplexF* findBest();
299 
300   Status evaluate(GJK& gjk, const Vec3f& guess);
301 
302   /// @brief the goal is to add a face connecting vertex w and face edge f[e]
303   bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon);
304 };
305 
306 
307 } // details
308 
309 
310 
311 }
312 
313 
314 #endif
315