38 #ifndef FCL_NARROWPHASE_DETAIL_EPA_INL_H
39 #define FCL_NARROWPHASE_DETAIL_EPA_INL_H
56 : root(nullptr), count(0)
67 if(root) root->
l[0] = face;
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];
86 fa->
e[ea] = eb; fa->
f[ea] = fb;
87 fb->
e[eb] = ea; fb->
f[eb] = fa;
93 unsigned int max_face_num_,
94 unsigned int max_vertex_num_,
95 unsigned int max_iterations_, S tolerance_)
105 template <
typename S>
113 template <
typename S>
116 sv_store =
new SimplexV[max_vertex_num];
117 fc_store =
new SimplexF[max_face_num];
122 for(
size_t i = 0; i < max_face_num; ++i)
123 stock.append(&fc_store[max_face_num-i-1]);
127 template <
typename S>
132 S a_dot_nab = a->w.dot(n_ab);
138 S a_dot_ba = a->w.dot(ba);
139 S b_dot_ba = b->w.dot(ba);
143 else if(b_dot_ba < 0)
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));
158 template <
typename S>
174 face->
n.noalias() = (b->
w - a->
w).cross(c->
w - a->
w);
175 S l = face->
n.norm();
179 if(!(getEdgeDist(face, a, b, face->
d) ||
180 getEdgeDist(face, b, c, face->
d) ||
181 getEdgeDist(face, c, a, face->
d)))
183 face->
d = a->
w.dot(face->
n) / l;
193 status = Degenerated;
200 status = stock.root ? OutOfVertices : OutOfFaces;
206 template <
typename S>
210 S mind = minf->
d * minf->
d;
224 template <
typename S>
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)
243 simplex.
c[0] = simplex.
c[1];
246 S tmpv = simplex.
p[0];
247 simplex.
p[0] = simplex.
p[1];
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) };
261 size_t iterations = 0;
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);
272 for(; iterations < max_iterations; ++iterations)
274 if(nextsv < max_vertex_num)
281 S wdist = best->
n.dot(w->w) - best->
d;
284 for(
size_t j = 0; (j < 3) && valid; ++j)
286 valid &= expand(pass, w, best->
f[j], best->
e[j], horizon);
290 if(valid && horizon.
nf >= 3)
293 bind(horizon.
ff, 2, horizon.
cf, 1);
301 status = InvalidHull;
break;
306 status = AccuracyReached;
break;
311 status = OutOfVertices;
break;
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();
326 S sum = result.p[0] + result.p[1] + result.p[2];
336 S nl = normal.norm();
337 if(nl > 0) normal /= nl;
341 result.c[0] = simplex.
c[0];
348 template <
typename S>
351 static const size_t nexti[] = {1, 2, 0};
352 static const size_t previ[] = {2, 0, 1};
356 const size_t e1 = nexti[e];
361 SimplexF* nf = newFace(f->
c[e1], f->
c[e], w,
false);
371 bind(nf, 2, horizon.
cf, 1);
382 const size_t e2 = previ[e];
384 if(expand(pass, w, f->
f[e1], f->
e[e1], horizon) && expand(pass, w, f->
f[e2], f->
e[e2], horizon))