FCL  0.6.0
Flexible Collision Library
gjk-inl.h
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 
38 #ifndef FCL_NARROWPHASE_DETAIL_GJK_INL_H
39 #define FCL_NARROWPHASE_DETAIL_GJK_INL_H
40 
41 #include "fcl/narrowphase/detail/convexity_based_algorithm/gjk.h"
42 
43 namespace fcl
44 {
45 
46 namespace detail
47 {
48 
49 //==============================================================================
50 extern template
51 struct GJK<double>;
52 
53 //==============================================================================
54 template <typename S>
55 GJK<S>::GJK(unsigned int max_iterations_, S tolerance_)
56  : max_iterations(max_iterations_), tolerance(tolerance_)
57 {
58  initialize();
59 }
60 
61 //==============================================================================
62 template <typename S>
63 void GJK<S>::initialize()
64 {
65  ray.setZero();
66  nfree = 0;
67  status = Failed;
68  current = 0;
69  distance = 0.0;
70  simplex = nullptr;
71 }
72 
73 //==============================================================================
74 template <typename S>
75 Vector3<S> GJK<S>::getGuessFromSimplex() const
76 {
77  return ray;
78 }
79 
80 //==============================================================================
81 template <typename S>
82 typename GJK<S>::Status GJK<S>::evaluate(const MinkowskiDiff<S>& shape_, const Vector3<S>& guess)
83 {
84  size_t iterations = 0;
85  S alpha = 0;
86  Vector3<S> lastw[4];
87  size_t clastw = 0;
88 
89  free_v[0] = &store_v[0];
90  free_v[1] = &store_v[1];
91  free_v[2] = &store_v[2];
92  free_v[3] = &store_v[3];
93 
94  nfree = 4;
95  current = 0;
96  status = Valid;
97  shape = shape_;
98  distance = 0.0;
99  simplices[0].rank = 0;
100  ray = guess;
101 
102  appendVertex(simplices[0], (ray.squaredNorm() > 0) ? (-ray).eval() : Vector3<S>::UnitX());
103  simplices[0].p[0] = 1;
104  ray = simplices[0].c[0]->w;
105  lastw[0] = lastw[1] = lastw[2] = lastw[3] = ray; // cache previous support points, the new support point will compare with it to avoid too close support points
106 
107  do
108  {
109  size_t next = 1 - current;
110  Simplex& curr_simplex = simplices[current];
111  Simplex& next_simplex = simplices[next];
112 
113  // check A: when origin is near the existing simplex, stop
114  S rl = ray.norm();
115  if(rl < tolerance) // mean origin is near the face of original simplex, return touch
116  {
117  status = Inside;
118  break;
119  }
120 
121  appendVertex(curr_simplex, -ray); // see below, ray points away from origin
122 
123  // check B: when the new support point is close to previous support points, stop (as the new simplex is degenerated)
124  Vector3<S>& w = curr_simplex.c[curr_simplex.rank - 1]->w;
125  bool found = false;
126  for(size_t i = 0; i < 4; ++i)
127  {
128  if((w - lastw[i]).squaredNorm() < tolerance)
129  {
130  found = true; break;
131  }
132  }
133 
134  if(found)
135  {
136  removeVertex(simplices[current]);
137  break;
138  }
139  else
140  {
141  lastw[clastw = (clastw+1)&3] = w;
142  }
143 
144  // check C: when the new support point is close to the sub-simplex where the ray point lies, stop (as the new simplex again is degenerated)
145  S omega = ray.dot(w) / rl;
146  alpha = std::max(alpha, omega);
147  if((rl - alpha) - tolerance * rl <= 0)
148  {
149  removeVertex(simplices[current]);
150  break;
151  }
152 
153  typename Project<S>::ProjectResult project_res;
154  switch(curr_simplex.rank)
155  {
156  case 2:
157  project_res = Project<S>::projectLineOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w); break;
158  case 3:
159  project_res = Project<S>::projectTriangleOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, curr_simplex.c[2]->w); break;
160  case 4:
161  project_res = Project<S>::projectTetrahedraOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, curr_simplex.c[2]->w, curr_simplex.c[3]->w); break;
162  }
163 
164  if(project_res.sqr_distance >= 0)
165  {
166  next_simplex.rank = 0;
167  ray.setZero();
168  current = next;
169  for(size_t i = 0; i < curr_simplex.rank; ++i)
170  {
171  if(project_res.encode & (1 << i))
172  {
173  next_simplex.c[next_simplex.rank] = curr_simplex.c[i];
174  next_simplex.p[next_simplex.rank++] = project_res.parameterization[i]; // weights[i];
175  ray += curr_simplex.c[i]->w * project_res.parameterization[i]; // weights[i];
176  }
177  else
178  free_v[nfree++] = curr_simplex.c[i];
179  }
180  if(project_res.encode == 15) status = Inside; // the origin is within the 4-simplex, collision
181  }
182  else
183  {
184  removeVertex(simplices[current]);
185  break;
186  }
187 
188  status = ((++iterations) < max_iterations) ? status : Failed;
189 
190  } while(status == Valid);
191 
192  simplex = &simplices[current];
193  switch(status)
194  {
195  case Valid: distance = ray.norm(); break;
196  case Inside: distance = 0; break;
197  default: break;
198  }
199  return status;
200 }
201 
202 //==============================================================================
203 template <typename S>
204 void GJK<S>::getSupport(const Vector3<S>& d, SimplexV& sv) const
205 {
206  sv.d = d.normalized();
207  sv.w = shape.support(sv.d);
208 }
209 
210 //==============================================================================
211 template <typename S>
212 void GJK<S>::getSupport(const Vector3<S>& d, const Vector3<S>& v, SimplexV& sv) const
213 {
214  sv.d = d.normalized();
215  sv.w = shape.support(sv.d, v);
216 }
217 
218 //==============================================================================
219 template <typename S>
221 {
222  free_v[nfree++] = simplex.c[--simplex.rank];
223 }
224 
225 //==============================================================================
226 template <typename S>
227 void GJK<S>::appendVertex(Simplex& simplex, const Vector3<S>& v)
228 {
229  simplex.p[simplex.rank] = 0; // initial weight 0
230  simplex.c[simplex.rank] = free_v[--nfree]; // set the memory
231  getSupport(v, *simplex.c[simplex.rank++]);
232 }
233 
234 //==============================================================================
235 template <typename S>
237 {
238  switch(simplex->rank)
239  {
240  case 1:
241  {
242  for(size_t i = 0; i < 3; ++i)
243  {
244  Vector3<S> axis = Vector3<S>::Zero();
245  axis[i] = 1;
246  appendVertex(*simplex, axis);
247  if(encloseOrigin()) return true;
248  removeVertex(*simplex);
249  appendVertex(*simplex, -axis);
250  if(encloseOrigin()) return true;
251  removeVertex(*simplex);
252  }
253  }
254  break;
255  case 2:
256  {
257  Vector3<S> d = simplex->c[1]->w - simplex->c[0]->w;
258  for(size_t i = 0; i < 3; ++i)
259  {
260  Vector3<S> axis = Vector3<S>::Zero();
261  axis[i] = 1;
262  Vector3<S> p = d.cross(axis);
263  if(p.squaredNorm() > 0)
264  {
265  appendVertex(*simplex, p);
266  if(encloseOrigin()) return true;
267  removeVertex(*simplex);
268  appendVertex(*simplex, -p);
269  if(encloseOrigin()) return true;
270  removeVertex(*simplex);
271  }
272  }
273  }
274  break;
275  case 3:
276  {
277  Vector3<S> n = (simplex->c[1]->w - simplex->c[0]->w).cross(simplex->c[2]->w - simplex->c[0]->w);
278  if(n.squaredNorm() > 0)
279  {
280  appendVertex(*simplex, n);
281  if(encloseOrigin()) return true;
282  removeVertex(*simplex);
283  appendVertex(*simplex, -n);
284  if(encloseOrigin()) return true;
285  removeVertex(*simplex);
286  }
287  }
288  break;
289  case 4:
290  {
291  if(std::abs(triple(simplex->c[0]->w - simplex->c[3]->w, simplex->c[1]->w - simplex->c[3]->w, simplex->c[2]->w - simplex->c[3]->w)) > 0)
292  return true;
293  }
294  break;
295  }
296 
297  return false;
298 }
299 
300 //==============================================================================
301 template <typename S>
303 {
304  return simplex;
305 }
306 
307 //==============================================================================
308 template <typename S>
310  : rank(0)
311 {
312  // Do nothing
313 }
314 
315 } // namespace detail
316 } // namespace fcl
317 
318 #endif
static ProjectResult projectTetrahedraOrigin(const Vector3< S > &a, const Vector3< S > &b, const Vector3< S > &c, const Vector3< S > &d)
Project origin (0) onto tetrahedran a-b-c-d.
Definition: project-inl.h:257
unsigned int encode
the code of the projection type
Definition: project.h:64
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
Minkowski difference class of two shapes.
Definition: minkowski_diff.h:58
bool encloseOrigin()
whether the simplex enclose the origin
Definition: gjk-inl.h:236
S parameterization[4]
Parameterization of the projected point (based on the simplex to be projected, use 2 or 3 or 4 of the...
Definition: project.h:58
S p[4]
weight
Definition: gjk.h:67
Definition: project.h:55
void getSupport(const Vector3< S > &d, SimplexV &sv) const
apply the support function along a direction, the result is return in sv
Definition: gjk-inl.h:204
Vector3< S > getGuessFromSimplex() const
get the guess from current simplex
Definition: gjk-inl.h:75
Definition: gjk.h:62
size_t rank
size of simplex (number of vertices)
Definition: gjk.h:69
void appendVertex(Simplex &simplex, const Vector3< S > &v)
append one vertex to the simplex
Definition: gjk-inl.h:227
static ProjectResult projectTriangleOrigin(const Vector3< S > &a, const Vector3< S > &b, const Vector3< S > &c)
Project origin (0) onto triangle a-b-c.
Definition: project-inl.h:205
Definition: gjk.h:54
Vector3< S > d
support direction
Definition: gjk.h:57
Status evaluate(const MinkowskiDiff< S > &shape_, const Vector3< S > &guess)
GJK algorithm, given the initial value guess.
Definition: gjk-inl.h:82
SimplexV * c[4]
simplex vertex
Definition: gjk.h:65
static ProjectResult projectLineOrigin(const Vector3< S > &a, const Vector3< S > &b)
Project origin (0) onto line a-b.
Definition: project-inl.h:183
S distance(const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const kIOS< S > &b1, const kIOS< S > &b2, Vector3< S > *P, Vector3< S > *Q)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS-inl.h:266
Simplex * getSimplex() const
get the underlying simplex using in GJK, can be used for cache in next iteration
Definition: gjk-inl.h:302
class for GJK algorithm
Definition: gjk.h:52
Vector3< S > w
support vector (i.e., the furthest point on the shape along the support direction) ...
Definition: gjk.h:59
S sqr_distance
square distance from the query point to the projected simplex
Definition: project.h:61
void removeVertex(Simplex &simplex)
discard one vertex from the simplex
Definition: gjk-inl.h:220