gjk.h 7.95 KB
Newer Older
jpan's avatar
jpan committed
1
2
3
/*
 * Software License Agreement (BSD License)
 *
4
5
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
jpan's avatar
jpan committed
6
7
8
9
10
11
12
13
14
15
16
17
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
18
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
jpan's avatar
jpan committed
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

/** \author Jia Pan */

Joseph Mirabel's avatar
Joseph Mirabel committed
38
39
#ifndef HPP_FCL_GJK_H
#define HPP_FCL_GJK_H
jpan's avatar
jpan committed
40

41
42
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/math/transform.h>
jpan's avatar
jpan committed
43

44
45
namespace hpp
{
jpan's avatar
jpan committed
46
47
48
namespace fcl
{

49
50
namespace details
{
jpan's avatar
jpan committed
51

52
/// @brief the support function for shape
jpan's avatar
jpan committed
53
54
Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir); 

55
/// @brief Minkowski difference class of two shapes
56
57
///
/// \note The Minkowski difference is expressed in the frame of the first shape.
jpan's avatar
jpan committed
58
59
struct MinkowskiDiff
{
60
  /// @brief points to two shapes
jpan's avatar
jpan committed
61
  const ShapeBase* shapes[2];
62
63

  /// @brief rotation from shape0 to shape1
jpan's avatar
jpan committed
64
  Matrix3f toshape1;
65
66

  /// @brief transform from shape1 to shape0 
67
  Transform3f toshape0;
jpan's avatar
jpan committed
68
69
70

  MinkowskiDiff() { }

71
72
  void set (const ShapeBase* shape0, const ShapeBase* shape1);

73
  /// @brief support function for shape0
jpan's avatar
jpan committed
74
75
76
77
  inline Vec3f support0(const Vec3f& d) const
  {
    return getSupport(shapes[0], d);
  }
78
  
79
  /// @brief support function for shape1
jpan's avatar
jpan committed
80
81
82
83
84
  inline Vec3f support1(const Vec3f& d) const
  {
    return toshape0.transform(getSupport(shapes[1], toshape1 * d));
  }

85
  /// @brief support function for the pair of shapes
jpan's avatar
jpan committed
86
87
88
89
90
  inline Vec3f support(const Vec3f& d) const
  {
    return support0(d) - support1(-d);
  }

91
  /// @brief support function for the d-th shape (d = 0 or 1)
jpan's avatar
jpan committed
92
93
94
95
96
97
98
  inline Vec3f support(const Vec3f& d, size_t index) const
  {
    if(index)
      return support1(d);
    else
      return support0(d);
  }
99
};
jpan's avatar
jpan committed
100

101
/// @brief class for GJK algorithm
102
103
///
/// \note The computations are performed in the frame of the first shape.
jpan's avatar
jpan committed
104
105
106
107
struct GJK
{
  struct SimplexV
  {
108
109
    /// @brief support direction
    Vec3f d; 
110
    /// @brieg support vector (i.e., the furthest point on the shape along the support direction)
111
    Vec3f w;
112
113

    SimplexV () : d(Vec3f::Zero()), w(Vec3f::Zero()) {}
jpan's avatar
jpan committed
114
115
116
117
  };

  struct Simplex
  {
118
    /// @brief simplex vertex
119
    SimplexV* vertex[4];
120
    /// @brief weight 
121
    FCL_REAL coefficient[4];
122
    /// @brief size of simplex (number of vertices)
123
124
125
    size_t rank;

    Simplex() : rank(0) {}
jpan's avatar
jpan committed
126
127
128
129
130
131
  };

  enum Status {Valid, Inside, Failed};

  MinkowskiDiff shape;
  Vec3f ray;
132
  FCL_REAL distance;
jpan's avatar
jpan committed
133
134
135
  Simplex simplices[2];


136
137
  GJK(unsigned int max_iterations_, FCL_REAL tolerance_)  : max_iterations(max_iterations_),
                                                            tolerance(tolerance_)
138
139
140
  {
    initialize(); 
  }
jpan's avatar
jpan committed
141
  
142
  void initialize();
jpan's avatar
jpan committed
143

144
  /// @brief GJK algorithm, given the initial value guess
jpan's avatar
jpan committed
145
146
  Status evaluate(const MinkowskiDiff& shape_, const Vec3f& guess);

147
  /// @brief apply the support function along a direction, the result is return in sv
jpan's avatar
jpan committed
148
149
  void getSupport(const Vec3f& d, SimplexV& sv) const;

150
  /// @brief discard one vertex from the simplex
jpan's avatar
jpan committed
151
152
  void removeVertex(Simplex& simplex);

153
  /// @brief append one vertex to the simplex
jpan's avatar
jpan committed
154
155
  void appendVertex(Simplex& simplex, const Vec3f& v);

156
  /// @brief whether the simplex enclose the origin
jpan's avatar
jpan committed
157
158
  bool encloseOrigin();

159
  /// @brief get the underlying simplex using in GJK, can be used for cache in next iteration
jpan's avatar
jpan committed
160
161
162
163
  inline Simplex* getSimplex() const
  {
    return simplex;
  }
164
165
166
167

  /// @brief get the guess from current simplex
  Vec3f getGuessFromSimplex() const;

jpan's avatar
jpan committed
168
169
170
171
172
173
174
private:
  SimplexV store_v[4];
  SimplexV* free_v[4];
  size_t nfree;
  size_t current;
  Simplex* simplex;
  Status status;
175
176

  unsigned int max_iterations;
177
178
  FCL_REAL tolerance;

jpan's avatar
jpan committed
179
180
181
182
183
};


static const size_t EPA_MAX_FACES = 128;
static const size_t EPA_MAX_VERTICES = 64;
184
static const FCL_REAL EPA_EPS = 0.000001;
jpan's avatar
jpan committed
185
186
static const size_t EPA_MAX_ITERATIONS = 255;

187
/// @brief class for EPA algorithm
jpan's avatar
jpan committed
188
189
struct EPA
{
190
private:
jpan's avatar
jpan committed
191
192
193
194
  typedef GJK::SimplexV SimplexV;
  struct SimplexF
  {
    Vec3f n;
195
    FCL_REAL d;
196
    SimplexV* vertex[3]; // a face has three vertices
jpan's avatar
jpan committed
197
198
199
200
    SimplexF* f[3]; // a face has three adjacent faces
    SimplexF* l[2]; // the pre and post faces in the list
    size_t e[3];
    size_t pass;
201
202

    SimplexF () : n(Vec3f::Zero()) {};
jpan's avatar
jpan committed
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
  };

  struct SimplexList
  {
    SimplexF* root;
    size_t count;
    SimplexList() : root(NULL), count(0) {}
    void append(SimplexF* face)
    {
      face->l[0] = NULL;
      face->l[1] = root;
      if(root) root->l[0] = face;
      root = face;
      ++count;
    }

    void remove(SimplexF* face)
    {
      if(face->l[1]) face->l[1]->l[0] = face->l[0];
      if(face->l[0]) face->l[0]->l[1] = face->l[1];
      if(face == root) root = face->l[1];
      --count;
    }
  };

  static inline void bind(SimplexF* fa, size_t ea, SimplexF* fb, size_t eb)
  {
    fa->e[ea] = eb; fa->f[ea] = fb;
    fb->e[eb] = ea; fb->f[eb] = fa;
  }

  struct SimplexHorizon
  {
    SimplexF* cf; // current face in the horizon
    SimplexF* ff; // first face in the horizon
    size_t nf; // number of faces in the horizon
    SimplexHorizon() : cf(NULL), ff(NULL), nf(0) {}
  };

242
243
244
245
private:
  unsigned int max_face_num;
  unsigned int max_vertex_num;
  unsigned int max_iterations;
246
  FCL_REAL tolerance;
247

248
249
public:

jpan's avatar
jpan committed
250
251
252
253
254
  enum Status {Valid, Touching, Degenerated, NonConvex, InvalidHull, OutOfFaces, OutOfVertices, AccuracyReached, FallBack, Failed};
  
  Status status;
  GJK::Simplex result;
  Vec3f normal;
255
  FCL_REAL depth;
256
257
  SimplexV* sv_store;
  SimplexF* fc_store;
jpan's avatar
jpan committed
258
259
260
  size_t nextsv;
  SimplexList hull, stock;

261
262
263
264
  EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, FCL_REAL tolerance_) : max_face_num(max_face_num_),
                                                                                                                     max_vertex_num(max_vertex_num_),
                                                                                                                     max_iterations(max_iterations_),
                                                                                                                     tolerance(tolerance_)
jpan's avatar
jpan committed
265
266
267
268
  {
    initialize();
  }

269
270
271
272
273
274
  ~EPA()
  {
    delete [] sv_store;
    delete [] fc_store;
  }

275
  void initialize();
jpan's avatar
jpan committed
276

277
  bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist);
jpan's avatar
jpan committed
278

279
  SimplexF* newFace(SimplexV* a, SimplexV* b, SimplexV* vertex, bool forced);
jpan's avatar
jpan committed
280

281
  /// @brief Find the best polytope face to split
jpan's avatar
jpan committed
282
283
284
285
  SimplexF* findBest();

  Status evaluate(GJK& gjk, const Vec3f& guess);

286
  /// @brief the goal is to add a face connecting vertex w and face edge f[e] 
jpan's avatar
jpan committed
287
288
289
290
  bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon);  
};


291
292
} // details

jpan's avatar
jpan committed
293
294
295
296
297


}


298
299
} // namespace hpp

jpan's avatar
jpan committed
300
#endif