gjk.cpp 22.5 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 */

38
39
#include <hpp/fcl/narrowphase/gjk.h>
#include <hpp/fcl/intersect.h>
jpan's avatar
jpan committed
40

41
42
namespace hpp
{
jpan's avatar
jpan committed
43
44
45
namespace fcl
{

46
47
namespace details
{
jpan's avatar
jpan committed
48

49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
struct shape_traits_base
{
  enum { NeedNormalizedDir = true
  };
};

template <typename Shape> struct shape_traits : shape_traits_base {};

template <> struct shape_traits<TriangleP> : shape_traits_base
{
  enum { NeedNormalizedDir = false
  };
};

template <> struct shape_traits<Box> : shape_traits_base
{
  enum { NeedNormalizedDir = false
  };
};

69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support)
{
  FCL_REAL dota = dir.dot(triangle->a);
  FCL_REAL dotb = dir.dot(triangle->b);
  FCL_REAL dotc = dir.dot(triangle->c);
  if(dota > dotb)
  {
    if(dotc > dota)
      support = triangle->c;
    else
      support = triangle->a;
  }
  else
  {
    if(dotc > dotb)
      support = triangle->c;
    else
      support = triangle->b;
  }
}

inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support)
{
  support.noalias() = (dir.array() > 0).select(box->halfSide, -box->halfSide);
}

inline void getShapeSupport(const Sphere* sphere, const Vec3f& dir, Vec3f& support)
{
  support = dir * sphere->radius;
}

inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, Vec3f& support)
{
  support = capsule->radius * dir;
  if (dir[2] > 0) support[2] += capsule->lz / 2;
  else            support[2] -= capsule->lz / 2;
}

void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support)
{
  // TODO (Joseph Mirabel)
  // this assumes that the cone radius is, for -0.5 < z < 0.5:
  // (lz/2 - z) * radius / lz
  //
  // I did not change the code myself. However, I think it should be revised.
  // 1. It can be optimized.
  // 2. I am not sure this is what conePlaneIntersect and coneHalfspaceIntersect
  //    assumes...
  FCL_REAL zdist = dir[0] * dir[0] + dir[1] * dir[1];
  FCL_REAL len = zdist + dir[2] * dir[2];
  zdist = std::sqrt(zdist);
  len = std::sqrt(len);
  FCL_REAL half_h = cone->lz * 0.5;
  FCL_REAL radius = cone->radius;

  FCL_REAL sin_a = radius / std::sqrt(radius * radius + 4 * half_h * half_h);

  if(dir[2] > len * sin_a)
    support = Vec3f(0, 0, half_h);
  else if(zdist > 0)
  {
    FCL_REAL rad = radius / zdist;
    support = Vec3f(rad * dir[0], rad * dir[1], -half_h);
  }
  else
    support = Vec3f(0, 0, -half_h);
}

void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support)
{
  static const FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
  FCL_REAL zdist = std::sqrt(dir[0] * dir[0] + dir[1] * dir[1]);
  FCL_REAL half_h = cylinder->lz * 0.5;
  if(zdist == 0.0)
    support = Vec3f(0, 0, (dir[2]>0)? half_h:-half_h);
  else {
    FCL_REAL d = cylinder->radius / zdist;
    FCL_REAL z (0.);
    if (dir [2] > eps) z = half_h;
    else if (dir [2] < -eps) z = -half_h;
    support << d * dir.head<2>(), z;
  }
  assert (fabs (support [0] * dir [1] - support [1] * dir [0]) < eps);
}

void getShapeSupport(const Convex* convex, const Vec3f& dir, Vec3f& support)
{
  FCL_REAL maxdot = - std::numeric_limits<FCL_REAL>::max();
  Vec3f* curp = convex->points;
  for(int i = 0; i < convex->num_points; ++i, curp+=1)
  {
    FCL_REAL dot = dir.dot(*curp);
    if(dot > maxdot)
    {
      support = *curp;
      maxdot = dot;
    }
  }
}

jpan's avatar
jpan committed
169
170
Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
{
171
  Vec3f support;
172
  switch(shape->getNodeType())
jpan's avatar
jpan committed
173
  {
174
  case GEOM_TRIANGLE:
175
    getShapeSupport (static_cast<const TriangleP*>(shape), dir, support);
176
    break;
jpan's avatar
jpan committed
177
  case GEOM_BOX:
178
    getShapeSupport (static_cast<const Box*>(shape), dir, support);
jpan's avatar
jpan committed
179
180
    break;
  case GEOM_SPHERE:
181
    getShapeSupport (static_cast<const Sphere*>(shape), dir, support);
jpan's avatar
jpan committed
182
183
    break;
  case GEOM_CAPSULE:
184
    getShapeSupport (static_cast<const Capsule*>(shape), dir, support);
jpan's avatar
jpan committed
185
186
    break;
  case GEOM_CONE:
187
    getShapeSupport (static_cast<const Cone*>(shape), dir, support);
jpan's avatar
jpan committed
188
189
    break;
  case GEOM_CYLINDER:
190
    getShapeSupport (static_cast<const Cylinder*>(shape), dir, support);
jpan's avatar
jpan committed
191
192
    break;
  case GEOM_CONVEX:
193
    getShapeSupport (static_cast<const Convex*>(shape), dir, support);
jpan's avatar
jpan committed
194
195
    break;
  case GEOM_PLANE:
196
  case GEOM_HALFSPACE:
197
198
  default:
    ; // nothing
jpan's avatar
jpan committed
199
200
  }

201
202
203
204
205
206
207
208
209
210
211
212
213
214
  return support;
}

template <typename Shape0, typename Shape1>
void getSupportTpl (const Shape0* s0, const Shape1* s1,
    const Matrix3f& oM1, const Vec3f& ot1,
    const Vec3f& dir, Vec3f& support)
{
  getShapeSupport (s0, dir, support);
  Vec3f support1;
  getShapeSupport (s1, - oM1.transpose() * dir, support1);
  support.noalias() -= oM1 * support1 + ot1;
}

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
242
243
244
245
246
247
248
249
250
251
252
253
254
255
template <typename Shape0, typename Shape1>
void getSupportFuncTpl (const MinkowskiDiff& md,
    const Vec3f& dir, bool dirIsNormalized, Vec3f& support)
{
  enum { NeedNormalizedDir =
    bool ( (bool)shape_traits<Shape0>::NeedNormalizedDir
        || (bool)shape_traits<Shape1>::NeedNormalizedDir)
  };
  getSupportTpl<Shape0, Shape1> (
      static_cast <const Shape0*>(md.shapes[0]),
      static_cast <const Shape1*>(md.shapes[1]),
      md.toshape0.getRotation(),
      md.toshape0.getTranslation(),
      (NeedNormalizedDir && !dirIsNormalized) ? dir.normalized() : dir,
      support);
}

template <typename Shape0>
MinkowskiDiff::GetSupportFunction makeGetSupportFunction1 (const ShapeBase* s1)
{
  switch(s1->getNodeType())
  {
  case GEOM_TRIANGLE:
    return getSupportFuncTpl<Shape0, TriangleP>;
  case GEOM_BOX:
    return getSupportFuncTpl<Shape0, Box>;
  case GEOM_SPHERE:
    return getSupportFuncTpl<Shape0, Sphere>;
  case GEOM_CAPSULE:
    return getSupportFuncTpl<Shape0, Capsule>;
  case GEOM_CONE:
    return getSupportFuncTpl<Shape0, Cone>;
  case GEOM_CYLINDER:
    return getSupportFuncTpl<Shape0, Cylinder>;
  case GEOM_CONVEX:
    return getSupportFuncTpl<Shape0, Convex>;
  default:
    throw std::logic_error ("Unsupported geometric shape");
  }
}

256
257
258
259
void MinkowskiDiff::set (const ShapeBase* shape0, const ShapeBase* shape1)
{
  shapes[0] = shape0;
  shapes[1] = shape1;
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286

  switch(shape0->getNodeType())
  {
  case GEOM_TRIANGLE:
    getSupportFunc = makeGetSupportFunction1<TriangleP> (shape1);
    break;
  case GEOM_BOX:
    getSupportFunc = makeGetSupportFunction1<Box> (shape1);
    break;
  case GEOM_SPHERE:
    getSupportFunc = makeGetSupportFunction1<Sphere> (shape1);
    break;
  case GEOM_CAPSULE:
    getSupportFunc = makeGetSupportFunction1<Capsule> (shape1);
    break;
  case GEOM_CONE:
    getSupportFunc = makeGetSupportFunction1<Cone> (shape1);
    break;
  case GEOM_CYLINDER:
    getSupportFunc = makeGetSupportFunction1<Cylinder> (shape1);
    break;
  case GEOM_CONVEX:
    getSupportFunc = makeGetSupportFunction1<Convex> (shape1);
    break;
  default:
    throw std::logic_error ("Unsupported geometric shape");
  }
jpan's avatar
jpan committed
287
288
}

289
290
void GJK::initialize()
{
291
  ray = Vec3f::Zero();
292
293
294
295
  nfree = 0;
  status = Failed;
  current = 0;
  distance = 0.0;
296
297
298
299
300
301
  simplex = NULL;
}

Vec3f GJK::getGuessFromSimplex() const
{
  return ray;
302
}
jpan's avatar
jpan committed
303
304
305
306

GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
{
  size_t iterations = 0;
307
  FCL_REAL alpha = 0;
jpan's avatar
jpan committed
308
309
  Vec3f lastw[4];
  size_t clastw = 0;
Florent Lamiraux's avatar
Florent Lamiraux committed
310
  Project::ProjectResult project_res;
jpan's avatar
jpan committed
311
312
313
314
315
316
317
318
319
320
321
322
323
    
  free_v[0] = &store_v[0];
  free_v[1] = &store_v[1];
  free_v[2] = &store_v[2];
  free_v[3] = &store_v[3];
    
  nfree = 4;
  current = 0;
  status = Valid;
  shape = shape_;
  distance = 0.0;
  simplices[0].rank = 0;
  ray = guess;
324

325
  if (ray.squaredNorm() > 0) appendVertex(simplices[0], -ray);
326
  else                       appendVertex(simplices[0], Vec3f(1, 0, 0), true);
327
328
  simplices[0].coefficient[0] = 1;
  ray = simplices[0].vertex[0]->w;
329
  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
jpan's avatar
jpan committed
330
331
332
333
334
335

  do
  {
    size_t next = 1 - current;
    Simplex& curr_simplex = simplices[current];
    Simplex& next_simplex = simplices[next];
336
337

    // check A: when origin is near the existing simplex, stop
338
    FCL_REAL rl = ray.norm();
339
    if(rl < tolerance) // mean origin is near the face of original simplex, return touch
jpan's avatar
jpan committed
340
341
342
343
344
345
    {
      status = Inside;
      break;
    }

    appendVertex(curr_simplex, -ray); // see below, ray points away from origin
346
347

    // check B: when the new support point is close to previous support points, stop (as the new simplex is degenerated)
348
    const Vec3f& w = curr_simplex.vertex[curr_simplex.rank - 1]->w;
349
    lastw[clastw = (clastw+1)&3] = w;
jpan's avatar
jpan committed
350

351
    // 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)
352
    FCL_REAL omega = ray.dot(w) / rl;
jpan's avatar
jpan committed
353
    alpha = std::max(alpha, omega);
354
    if((rl - alpha) - tolerance * rl <= 0)
jpan's avatar
jpan committed
355
356
357
358
359
360
361
362
    {
      removeVertex(simplices[current]);
      break;
    }

    switch(curr_simplex.rank)
    {
    case 2:
363
364
365
      project_res = Project::projectLineOrigin(curr_simplex.vertex[0]->w,
                                               curr_simplex.vertex[1]->w);
      break;
jpan's avatar
jpan committed
366
    case 3:
367
368
369
      project_res = Project::projectTriangleOrigin(curr_simplex.vertex[0]->w,
                                                   curr_simplex.vertex[1]->w,
                                                   curr_simplex.vertex[2]->w); break;
jpan's avatar
jpan committed
370
    case 4:
371
372
373
374
375
      project_res = Project::projectTetrahedraOrigin(curr_simplex.vertex[0]->w,
                                                     curr_simplex.vertex[1]->w,
                                                     curr_simplex.vertex[2]->w,
                                                     curr_simplex.vertex[3]->w);
      break;
jpan's avatar
jpan committed
376
    }
377
    if(project_res.sqr_distance >= 0)
jpan's avatar
jpan committed
378
379
    {
      next_simplex.rank = 0;
380
      ray = Vec3f(0,0,0);
jpan's avatar
jpan committed
381
382
383
      current = next;
      for(size_t i = 0; i < curr_simplex.rank; ++i)
      {
384
        if(project_res.encode & (1 << i))
jpan's avatar
jpan committed
385
        {
386
387
388
389
390
391
          next_simplex.vertex[next_simplex.rank] = curr_simplex.vertex[i];
          // weights[i]
          next_simplex.coefficient[next_simplex.rank++] =
            project_res.parameterization[i];
          // weights[i]
          ray += curr_simplex.vertex[i]->w * project_res.parameterization[i];
jpan's avatar
jpan committed
392
393
        }
        else
394
          free_v[nfree++] = curr_simplex.vertex[i];
jpan's avatar
jpan committed
395
      }
396
      if(project_res.encode == 15) status = Inside; // the origin is within the 4-simplex, collision
jpan's avatar
jpan committed
397
398
399
400
401
402
403
    }
    else
    {
      removeVertex(simplices[current]);
      break;
    }

404
    status = ((++iterations) < max_iterations) ? status : Failed;
jpan's avatar
jpan committed
405
406
407
408
409
410
      
  } while(status == Valid);

  simplex = &simplices[current];
  switch(status)
  {
411
  case Valid: distance = ray.norm(); break;
jpan's avatar
jpan committed
412
  case Inside: distance = 0; break;
Florent Lamiraux's avatar
Florent Lamiraux committed
413
414
415
  default:
    distance = sqrt (project_res.sqr_distance);
    break;
jpan's avatar
jpan committed
416
417
418
419
  }
  return status;
}

420
void GJK::getSupport(const Vec3f& d, bool dIsNormalized, SimplexV& sv) const
jpan's avatar
jpan committed
421
{
422
423
424
  if (dIsNormalized) sv.d = d;
  else sv.d.noalias() = d.normalized();
  shape.support(sv.d, true, sv.w);
jpan's avatar
jpan committed
425
426
427
428
}

void GJK::removeVertex(Simplex& simplex)
{
429
  free_v[nfree++] = simplex.vertex[--simplex.rank];
jpan's avatar
jpan committed
430
431
}

432
void GJK::appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized)
jpan's avatar
jpan committed
433
{
434
435
  simplex.coefficient[simplex.rank] = 0; // initial weight 0
  simplex.vertex[simplex.rank] = free_v[--nfree]; // set the memory
436
  getSupport (v, isNormalized, *simplex.vertex[simplex.rank++]);
jpan's avatar
jpan committed
437
438
439
440
441
442
443
444
445
446
}

bool GJK::encloseOrigin()
{
  switch(simplex->rank)
  {
  case 1:
    {
      for(size_t i = 0; i < 3; ++i)
      {
447
        Vec3f axis(Vec3f::Zero());
jpan's avatar
jpan committed
448
        axis[i] = 1;
449
        appendVertex(*simplex, axis, true);
jpan's avatar
jpan committed
450
451
        if(encloseOrigin()) return true;
        removeVertex(*simplex);
452
        appendVertex(*simplex, -axis, true);
jpan's avatar
jpan committed
453
454
455
456
457
458
459
        if(encloseOrigin()) return true;
        removeVertex(*simplex);
      }
    }
    break;
  case 2:
    {
460
      Vec3f d = simplex->vertex[1]->w - simplex->vertex[0]->w;
jpan's avatar
jpan committed
461
462
      for(size_t i = 0; i < 3; ++i)
      {
463
        Vec3f axis(0,0,0);
jpan's avatar
jpan committed
464
465
        axis[i] = 1;
        Vec3f p = d.cross(axis);
466
        if(p.squaredNorm() > 0)
jpan's avatar
jpan committed
467
468
469
470
471
472
473
474
475
476
477
478
479
        {
          appendVertex(*simplex, p);
          if(encloseOrigin()) return true;
          removeVertex(*simplex);
          appendVertex(*simplex, -p);
          if(encloseOrigin()) return true;
          removeVertex(*simplex);
        }
      }
    }
    break;
  case 3:
    {
480
481
      Vec3f n = (simplex->vertex[1]->w - simplex->vertex[0]->w).cross
        (simplex->vertex[2]->w - simplex->vertex[0]->w);
482
      if(n.squaredNorm() > 0)
jpan's avatar
jpan committed
483
484
485
486
487
488
489
490
491
492
493
494
      {
        appendVertex(*simplex, n);
        if(encloseOrigin()) return true;
        removeVertex(*simplex);
        appendVertex(*simplex, -n);
        if(encloseOrigin()) return true;
        removeVertex(*simplex);
      }
    }
    break;
  case 4:
    {
495
496
497
      if(std::abs(triple(simplex->vertex[0]->w - simplex->vertex[3]->w,
                         simplex->vertex[1]->w - simplex->vertex[3]->w,
                         simplex->vertex[2]->w - simplex->vertex[3]->w)) > 0)
jpan's avatar
jpan committed
498
499
500
501
502
503
504
505
        return true;
    }
    break;
  }

  return false;
}

506
507
void EPA::initialize()
{
508
509
  sv_store = new SimplexV[max_vertex_num];
  fc_store = new SimplexF[max_face_num];
510
511
512
513
  status = Failed;
  normal = Vec3f(0, 0, 0);
  depth = 0;
  nextsv = 0;
514
515
  for(size_t i = 0; i < max_face_num; ++i)
    stock.append(&fc_store[max_face_num-i-1]);
516
517
}

518
bool EPA::getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist)
jpan's avatar
jpan committed
519
520
521
{
  Vec3f ba = b->w - a->w;
  Vec3f n_ab = ba.cross(face->n);
522
  FCL_REAL a_dot_nab = a->w.dot(n_ab);
jpan's avatar
jpan committed
523
524
525
526
527

  if(a_dot_nab < 0) // the origin is on the outside part of ab
  {
    // following is similar to projectOrigin for two points
    // however, as we dont need to compute the parameterization, dont need to compute 0 or 1
528
529
    FCL_REAL a_dot_ba = a->w.dot(ba); 
    FCL_REAL b_dot_ba = b->w.dot(ba);
jpan's avatar
jpan committed
530
531

    if(a_dot_ba > 0) 
532
      dist = a->w.norm();
jpan's avatar
jpan committed
533
    else if(b_dot_ba < 0)
534
      dist = b->w.norm();
jpan's avatar
jpan committed
535
536
    else
    {
537
      FCL_REAL a_dot_b = a->w.dot(b->w);
538
      dist = std::sqrt(std::max(a->w.squaredNorm() * b->w.squaredNorm() - a_dot_b * a_dot_b, (FCL_REAL)0));
jpan's avatar
jpan committed
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
    }

    return true;
  }

  return false;
}

EPA::SimplexF* EPA::newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced)
{
  if(stock.root)
  {
    SimplexF* face = stock.root;
    stock.remove(face);
    hull.append(face);
    face->pass = 0;
555
556
557
    face->vertex[0] = a;
    face->vertex[1] = b;
    face->vertex[2] = c;
jpan's avatar
jpan committed
558
    face->n = (b->w - a->w).cross(c->w - a->w);
559
    FCL_REAL l = face->n.norm();
jpan's avatar
jpan committed
560
      
561
    if(l > tolerance)
jpan's avatar
jpan committed
562
563
564
565
566
567
568
569
570
    {
      if(!(getEdgeDist(face, a, b, face->d) ||
           getEdgeDist(face, b, c, face->d) ||
           getEdgeDist(face, c, a, face->d)))
      {
        face->d = a->w.dot(face->n) / l;
      }

      face->n /= l;
571
      if(forced || face->d >= -tolerance)
jpan's avatar
jpan committed
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
        return face;
      else
        status = NonConvex;
    }
    else
      status = Degenerated;

    hull.remove(face);
    stock.append(face);
    return NULL;
  }
    
  status = stock.root ? OutOfVertices : OutOfFaces;
  return NULL;
}

/** \brief Find the best polytope face to split */
EPA::SimplexF* EPA::findBest()
{
  SimplexF* minf = hull.root;
592
  FCL_REAL mind = minf->d * minf->d;
jpan's avatar
jpan committed
593
594
  for(SimplexF* f = minf->l[1]; f; f = f->l[1])
  {
595
    FCL_REAL sqd = f->d * f->d;
jpan's avatar
jpan committed
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
    if(sqd < mind)
    {
      minf = f;
      mind = sqd;
    }
  }
  return minf;
}

EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
{
  GJK::Simplex& simplex = *gjk.getSimplex();
  if((simplex.rank > 1) && gjk.encloseOrigin())
  {
    while(hull.root)
    {
      SimplexF* f = hull.root;
      hull.remove(f);
      stock.append(f);
    }

    status = Valid;
    nextsv = 0;

620
621
622
    if((simplex.vertex[0]->w - simplex.vertex[3]->w).dot
       ((simplex.vertex[1]->w - simplex.vertex[3]->w).cross
        (simplex.vertex[2]->w - simplex.vertex[3]->w)) < 0)
jpan's avatar
jpan committed
623
    {
624
625
626
      SimplexV* tmp = simplex.vertex[0];
      simplex.vertex[0] = simplex.vertex[1];
      simplex.vertex[1] = tmp;
jpan's avatar
jpan committed
627

628
629
630
      FCL_REAL tmpv = simplex.coefficient[0];
      simplex.coefficient[0] = simplex.coefficient[1];
      simplex.coefficient[1] = tmpv;
jpan's avatar
jpan committed
631
632
    }

633
634
635
636
637
638
639
640
    SimplexF* tetrahedron[] = {newFace(simplex.vertex[0], simplex.vertex[1],
                                       simplex.vertex[2], true),
                               newFace(simplex.vertex[1], simplex.vertex[0],
                                       simplex.vertex[3], true),
                               newFace(simplex.vertex[2], simplex.vertex[1],
                                       simplex.vertex[3], true),
                               newFace(simplex.vertex[0], simplex.vertex[2],
                                       simplex.vertex[3], true) };
jpan's avatar
jpan committed
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657

    if(hull.count == 4)
    {
      SimplexF* best = findBest(); // find the best face (the face with the minimum distance to origin) to split
      SimplexF outer = *best;
      size_t pass = 0;
      size_t iterations = 0;
        
      // set the face connectivity
      bind(tetrahedron[0], 0, tetrahedron[1], 0);
      bind(tetrahedron[0], 1, tetrahedron[2], 0);
      bind(tetrahedron[0], 2, tetrahedron[3], 0);
      bind(tetrahedron[1], 1, tetrahedron[3], 2);
      bind(tetrahedron[1], 2, tetrahedron[2], 1);
      bind(tetrahedron[2], 2, tetrahedron[3], 1);

      status = Valid;
658
      for(; iterations < max_iterations; ++iterations)
jpan's avatar
jpan committed
659
      {
660
        if(nextsv < max_vertex_num)
jpan's avatar
jpan committed
661
662
663
664
665
        {
          SimplexHorizon horizon;
          SimplexV* w = &sv_store[nextsv++];
          bool valid = true;
          best->pass = ++pass;
666
667
          // At the moment, SimplexF.n is always normalized. This could be revised in the future...
          gjk.getSupport(best->n, true, *w);
668
          FCL_REAL wdist = best->n.dot(w->w) - best->d;
669
          if(wdist > tolerance)
jpan's avatar
jpan committed
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
          {
            for(size_t j = 0; (j < 3) && valid; ++j)
            {
              valid &= expand(pass, w, best->f[j], best->e[j], horizon);
            }

              
            if(valid && horizon.nf >= 3)
            {
              // need to add the edge connectivity between first and last faces
              bind(horizon.ff, 2, horizon.cf, 1);
              hull.remove(best);
              stock.append(best);
              best = findBest();
              outer = *best;
            }
            else
            {
              status = InvalidHull; break;
            }
          }
          else
          {
            status = AccuracyReached; break;
          }
        }
        else
        {
          status = OutOfVertices; break;
        }
      }

      Vec3f projection = outer.n * outer.d;
      normal = outer.n;
      depth = outer.d;
      result.rank = 3;
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
      result.vertex[0] = outer.vertex[0];
      result.vertex[1] = outer.vertex[1];
      result.vertex[2] = outer.vertex[2];
      result.coefficient[0] = ((outer.vertex[1]->w - projection).cross
                               (outer.vertex[2]->w - projection)).norm();
      result.coefficient[1] = ((outer.vertex[2]->w - projection).cross
                               (outer.vertex[0]->w - projection)).norm();
      result.coefficient[2] = ((outer.vertex[0]->w - projection).cross
                               (outer.vertex[1]->w - projection)).norm();

      FCL_REAL sum = result.coefficient[0] + result.coefficient[1] +
        result.coefficient[2];
      result.coefficient[0] /= sum;
      result.coefficient[1] /= sum;
      result.coefficient[2] /= sum;
jpan's avatar
jpan committed
721
722
723
724
725
726
      return status;
    }
  }

  status = FallBack;
  normal = -guess;
727
  FCL_REAL nl = normal.norm();
jpan's avatar
jpan committed
728
729
730
731
  if(nl > 0) normal /= nl;
  else normal = Vec3f(1, 0, 0);
  depth = 0;
  result.rank = 1;
732
733
  result.vertex[0] = simplex.vertex[0];
  result.coefficient[0] = 1;
jpan's avatar
jpan committed
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
  return status;
}


/** \brief the goal is to add a face connecting vertex w and face edge f[e] */
bool EPA::expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon)
{
  static const size_t nexti[] = {1, 2, 0};
  static const size_t previ[] = {2, 0, 1};

  if(f->pass != pass)
  {
    const size_t e1 = nexti[e];
      
    // case 1: the new face is not degenerated, i.e., the new face is not coplanar with the old face f.
749
    if(f->n.dot(w->w) - f->d < -tolerance)
jpan's avatar
jpan committed
750
    {
751
      SimplexF* nf = newFace(f->vertex[e1], f->vertex[e], w, false);
jpan's avatar
jpan committed
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
      if(nf)
      {
        // add face-face connectivity
        bind(nf, 0, f, e);
          
        // 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. 
        // 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.
        // Notice the face is anti-clockwise, so the edges are 0 (bottom), 1 (right), 2 (left)
        if(horizon.cf)  
          bind(nf, 2, horizon.cf, 1);
        else
          horizon.ff = nf; 
          
        horizon.cf = nf;
        ++horizon.nf;
        return true;
      }
    }
    else // case 2: the new face is coplanar with the old face f. We need to add two faces and delete the old face
    {
      const size_t e2 = previ[e];
      f->pass = pass;
      if(expand(pass, w, f->f[e1], f->e[e1], horizon) && expand(pass, w, f->f[e2], f->e[e2], horizon))
      {
        hull.remove(f);
        stock.append(f);
        return true;
      }
    }
  }

  return false;
}

786
} // details
jpan's avatar
jpan committed
787
788

} // fcl
789
790

} // namespace hpp