FCL  0.6.0
Flexible Collision Library
epa-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_EPA_INL_H
39 #define FCL_NARROWPHASE_DETAIL_EPA_INL_H
40 
41 #include "fcl/narrowphase/detail/convexity_based_algorithm/epa.h"
42 
43 namespace fcl
44 {
45 
46 namespace detail
47 {
48 
49 //==============================================================================
50 extern template
51 struct EPA<double>;
52 
53 //==============================================================================
54 template <typename S>
55 EPA<S>::SimplexList::SimplexList()
56  : root(nullptr), count(0)
57 {
58  // Do nothing
59 }
60 
61 //==============================================================================
62 template <typename S>
63 void EPA<S>::SimplexList::append(typename EPA<S>::SimplexF* face)
64 {
65  face->l[0] = nullptr;
66  face->l[1] = root;
67  if(root) root->l[0] = face;
68  root = face;
69  ++count;
70 }
71 
72 //==============================================================================
73 template <typename S>
74 void EPA<S>::SimplexList::remove(typename EPA<S>::SimplexF* face)
75 {
76  if(face->l[1]) face->l[1]->l[0] = face->l[0];
77  if(face->l[0]) face->l[0]->l[1] = face->l[1];
78  if(face == root) root = face->l[1];
79  --count;
80 }
81 
82 //==============================================================================
83 template <typename S>
84 void EPA<S>::bind(SimplexF* fa, size_t ea, SimplexF* fb, size_t eb)
85 {
86  fa->e[ea] = eb; fa->f[ea] = fb;
87  fb->e[eb] = ea; fb->f[eb] = fa;
88 }
89 
90 //==============================================================================
91 template <typename S>
92 EPA<S>::EPA(
93  unsigned int max_face_num_,
94  unsigned int max_vertex_num_,
95  unsigned int max_iterations_, S tolerance_)
96  : max_face_num(max_face_num_),
97  max_vertex_num(max_vertex_num_),
98  max_iterations(max_iterations_),
99  tolerance(tolerance_)
100 {
101  initialize();
102 }
103 
104 //==============================================================================
105 template <typename S>
106 EPA<S>::~EPA()
107 {
108  delete [] sv_store;
109  delete [] fc_store;
110 }
111 
112 //==============================================================================
113 template <typename S>
114 void EPA<S>::initialize()
115 {
116  sv_store = new SimplexV[max_vertex_num];
117  fc_store = new SimplexF[max_face_num];
118  status = Failed;
119  normal = Vector3<S>(0, 0, 0);
120  depth = 0;
121  nextsv = 0;
122  for(size_t i = 0; i < max_face_num; ++i)
123  stock.append(&fc_store[max_face_num-i-1]);
124 }
125 
126 //==============================================================================
127 template <typename S>
128 bool EPA<S>::getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, S& dist)
129 {
130  Vector3<S> ba = b->w - a->w;
131  Vector3<S> n_ab = ba.cross(face->n);
132  S a_dot_nab = a->w.dot(n_ab);
133 
134  if(a_dot_nab < 0) // the origin is on the outside part of ab
135  {
136  // following is similar to projectOrigin for two points
137  // however, as we dont need to compute the parameterization, dont need to compute 0 or 1
138  S a_dot_ba = a->w.dot(ba);
139  S b_dot_ba = b->w.dot(ba);
140 
141  if(a_dot_ba > 0)
142  dist = a->w.norm();
143  else if(b_dot_ba < 0)
144  dist = b->w.norm();
145  else
146  {
147  S a_dot_b = a->w.dot(b->w);
148  dist = std::sqrt(std::max(a->w.squaredNorm() * b->w.squaredNorm() - a_dot_b * a_dot_b, (S)0));
149  }
150 
151  return true;
152  }
153 
154  return false;
155 }
156 
157 //==============================================================================
158 template <typename S>
159 typename EPA<S>::SimplexF* EPA<S>::newFace(
160  typename GJK<S>::SimplexV* a,
161  typename GJK<S>::SimplexV* b,
162  typename GJK<S>::SimplexV* c,
163  bool forced)
164 {
165  if(stock.root)
166  {
167  SimplexF* face = stock.root;
168  stock.remove(face);
169  hull.append(face);
170  face->pass = 0;
171  face->c[0] = a;
172  face->c[1] = b;
173  face->c[2] = c;
174  face->n.noalias() = (b->w - a->w).cross(c->w - a->w);
175  S l = face->n.norm();
176 
177  if(l > tolerance)
178  {
179  if(!(getEdgeDist(face, a, b, face->d) ||
180  getEdgeDist(face, b, c, face->d) ||
181  getEdgeDist(face, c, a, face->d)))
182  {
183  face->d = a->w.dot(face->n) / l;
184  }
185 
186  face->n /= l;
187  if(forced || face->d >= -tolerance)
188  return face;
189  else
190  status = NonConvex;
191  }
192  else
193  status = Degenerated;
194 
195  hull.remove(face);
196  stock.append(face);
197  return nullptr;
198  }
199 
200  status = stock.root ? OutOfVertices : OutOfFaces;
201  return nullptr;
202 }
203 
204 //==============================================================================
206 template <typename S>
208 {
209  SimplexF* minf = hull.root;
210  S mind = minf->d * minf->d;
211  for(SimplexF* f = minf->l[1]; f; f = f->l[1])
212  {
213  S sqd = f->d * f->d;
214  if(sqd < mind)
215  {
216  minf = f;
217  mind = sqd;
218  }
219  }
220  return minf;
221 }
222 
223 //==============================================================================
224 template <typename S>
225 typename EPA<S>::Status EPA<S>::evaluate(GJK<S>& gjk, const Vector3<S>& guess)
226 {
227  typename GJK<S>::Simplex& simplex = *gjk.getSimplex();
228  if((simplex.rank > 1) && gjk.encloseOrigin())
229  {
230  while(hull.root)
231  {
232  SimplexF* f = hull.root;
233  hull.remove(f);
234  stock.append(f);
235  }
236 
237  status = Valid;
238  nextsv = 0;
239 
240  if((simplex.c[0]->w - simplex.c[3]->w).dot((simplex.c[1]->w - simplex.c[3]->w).cross(simplex.c[2]->w - simplex.c[3]->w)) < 0)
241  {
242  SimplexV* tmp = simplex.c[0];
243  simplex.c[0] = simplex.c[1];
244  simplex.c[1] = tmp;
245 
246  S tmpv = simplex.p[0];
247  simplex.p[0] = simplex.p[1];
248  simplex.p[1] = tmpv;
249  }
250 
251  SimplexF* tetrahedron[] = {newFace(simplex.c[0], simplex.c[1], simplex.c[2], true),
252  newFace(simplex.c[1], simplex.c[0], simplex.c[3], true),
253  newFace(simplex.c[2], simplex.c[1], simplex.c[3], true),
254  newFace(simplex.c[0], simplex.c[2], simplex.c[3], true) };
255 
256  if(hull.count == 4)
257  {
258  SimplexF* best = findBest(); // find the best face (the face with the minimum distance to origin) to split
259  SimplexF outer = *best;
260  size_t pass = 0;
261  size_t iterations = 0;
262 
263  // set the face connectivity
264  bind(tetrahedron[0], 0, tetrahedron[1], 0);
265  bind(tetrahedron[0], 1, tetrahedron[2], 0);
266  bind(tetrahedron[0], 2, tetrahedron[3], 0);
267  bind(tetrahedron[1], 1, tetrahedron[3], 2);
268  bind(tetrahedron[1], 2, tetrahedron[2], 1);
269  bind(tetrahedron[2], 2, tetrahedron[3], 1);
270 
271  status = Valid;
272  for(; iterations < max_iterations; ++iterations)
273  {
274  if(nextsv < max_vertex_num)
275  {
276  SimplexHorizon horizon;
277  SimplexV* w = &sv_store[nextsv++];
278  bool valid = true;
279  best->pass = ++pass;
280  gjk.getSupport(best->n, *w);
281  S wdist = best->n.dot(w->w) - best->d;
282  if(wdist > tolerance)
283  {
284  for(size_t j = 0; (j < 3) && valid; ++j)
285  {
286  valid &= expand(pass, w, best->f[j], best->e[j], horizon);
287  }
288 
289 
290  if(valid && horizon.nf >= 3)
291  {
292  // need to add the edge connectivity between first and last faces
293  bind(horizon.ff, 2, horizon.cf, 1);
294  hull.remove(best);
295  stock.append(best);
296  best = findBest();
297  outer = *best;
298  }
299  else
300  {
301  status = InvalidHull; break;
302  }
303  }
304  else
305  {
306  status = AccuracyReached; break;
307  }
308  }
309  else
310  {
311  status = OutOfVertices; break;
312  }
313  }
314 
315  Vector3<S> projection = outer.n * outer.d;
316  normal = outer.n;
317  depth = outer.d;
318  result.rank = 3;
319  result.c[0] = outer.c[0];
320  result.c[1] = outer.c[1];
321  result.c[2] = outer.c[2];
322  result.p[0] = ((outer.c[1]->w - projection).cross(outer.c[2]->w - projection)).norm();
323  result.p[1] = ((outer.c[2]->w - projection).cross(outer.c[0]->w - projection)).norm();
324  result.p[2] = ((outer.c[0]->w - projection).cross(outer.c[1]->w - projection)).norm();
325 
326  S sum = result.p[0] + result.p[1] + result.p[2];
327  result.p[0] /= sum;
328  result.p[1] /= sum;
329  result.p[2] /= sum;
330  return status;
331  }
332  }
333 
334  status = FallBack;
335  normal = -guess;
336  S nl = normal.norm();
337  if(nl > 0) normal /= nl;
338  else normal = Vector3<S>(1, 0, 0);
339  depth = 0;
340  result.rank = 1;
341  result.c[0] = simplex.c[0];
342  result.p[0] = 1;
343  return status;
344 }
345 
346 //==============================================================================
348 template <typename S>
349 bool EPA<S>::expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon)
350 {
351  static const size_t nexti[] = {1, 2, 0};
352  static const size_t previ[] = {2, 0, 1};
353 
354  if(f->pass != pass)
355  {
356  const size_t e1 = nexti[e];
357 
358  // case 1: the new face is not degenerated, i.e., the new face is not coplanar with the old face f.
359  if(f->n.dot(w->w) - f->d < -tolerance)
360  {
361  SimplexF* nf = newFace(f->c[e1], f->c[e], w, false);
362  if(nf)
363  {
364  // add face-face connectivity
365  bind(nf, 0, f, e);
366 
367  // if there is last face in the horizon, then need to add another connectivity, i.e. the edge connecting the current new add edge and the last new add edge.
368  // This does not finish all the connectivities because the final face need to connect with the first face, this will be handled in the evaluate function.
369  // Notice the face is anti-clockwise, so the edges are 0 (bottom), 1 (right), 2 (left)
370  if(horizon.cf)
371  bind(nf, 2, horizon.cf, 1);
372  else
373  horizon.ff = nf;
374 
375  horizon.cf = nf;
376  ++horizon.nf;
377  return true;
378  }
379  }
380  else // case 2: the new face is coplanar with the old face f. We need to add two faces and delete the old face
381  {
382  const size_t e2 = previ[e];
383  f->pass = pass;
384  if(expand(pass, w, f->f[e1], f->e[e1], horizon) && expand(pass, w, f->f[e2], f->e[e2], horizon))
385  {
386  hull.remove(f);
387  stock.append(f);
388  return true;
389  }
390  }
391  }
392 
393  return false;
394 }
395 
396 } // namespace detail
397 } // namespace fcl
398 
399 #endif
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
bool encloseOrigin()
whether the simplex enclose the origin
Definition: gjk-inl.h:236
bool expand(size_t pass, SimplexV *w, SimplexF *f, size_t e, SimplexHorizon &horizon)
the goal is to add a face connecting vertex w and face edge f[e]
Definition: epa-inl.h:349
class for EPA algorithm
Definition: epa.h:57
S p[4]
weight
Definition: gjk.h:67
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
Definition: gjk.h:62
size_t rank
size of simplex (number of vertices)
Definition: gjk.h:69
SimplexF * findBest()
Find the best polytope face to split.
Definition: epa-inl.h:207
SimplexV * c[4]
simplex vertex
Definition: gjk.h:65
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