gjk.cpp 22.3 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
326
  if (ray.squaredNorm() > 0) appendVertex(simplices[0], -ray);
  else                       appendVertex(simplices[0], Vec3f(1, 0, 0));
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
420
421
  }
  return status;
}

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

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

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

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

  return false;
}

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

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

  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
527
528
    FCL_REAL a_dot_ba = a->w.dot(ba); 
    FCL_REAL b_dot_ba = b->w.dot(ba);
jpan's avatar
jpan committed
529
530

    if(a_dot_ba > 0) 
531
      dist = a->w.norm();
jpan's avatar
jpan committed
532
    else if(b_dot_ba < 0)
533
      dist = b->w.norm();
jpan's avatar
jpan committed
534
535
    else
    {
536
      FCL_REAL a_dot_b = a->w.dot(b->w);
537
      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
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
    }

    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;
554
555
556
    face->vertex[0] = a;
    face->vertex[1] = b;
    face->vertex[2] = c;
jpan's avatar
jpan committed
557
    face->n = (b->w - a->w).cross(c->w - a->w);
558
    FCL_REAL l = face->n.norm();
jpan's avatar
jpan committed
559
      
560
    if(l > tolerance)
jpan's avatar
jpan committed
561
562
563
564
565
566
567
568
569
    {
      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;
570
      if(forced || face->d >= -tolerance)
jpan's avatar
jpan committed
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
        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;
591
  FCL_REAL mind = minf->d * minf->d;
jpan's avatar
jpan committed
592
593
  for(SimplexF* f = minf->l[1]; f; f = f->l[1])
  {
594
    FCL_REAL sqd = f->d * f->d;
jpan's avatar
jpan committed
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
    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;

619
620
621
    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
622
    {
623
624
625
      SimplexV* tmp = simplex.vertex[0];
      simplex.vertex[0] = simplex.vertex[1];
      simplex.vertex[1] = tmp;
jpan's avatar
jpan committed
626

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

632
633
634
635
636
637
638
639
    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
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656

    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;
657
      for(; iterations < max_iterations; ++iterations)
jpan's avatar
jpan committed
658
      {
659
        if(nextsv < max_vertex_num)
jpan's avatar
jpan committed
660
661
662
663
664
665
        {
          SimplexHorizon horizon;
          SimplexV* w = &sv_store[nextsv++];
          bool valid = true;
          best->pass = ++pass;
          gjk.getSupport(best->n, *w);
666
          FCL_REAL wdist = best->n.dot(w->w) - best->d;
667
          if(wdist > tolerance)
jpan's avatar
jpan committed
668
669
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
          {
            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;
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
      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
719
720
721
722
723
724
      return status;
    }
  }

  status = FallBack;
  normal = -guess;
725
  FCL_REAL nl = normal.norm();
jpan's avatar
jpan committed
726
727
728
729
  if(nl > 0) normal /= nl;
  else normal = Vec3f(1, 0, 0);
  depth = 0;
  result.rank = 1;
730
731
  result.vertex[0] = simplex.vertex[0];
  result.coefficient[0] = 1;
jpan's avatar
jpan committed
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
  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.
747
    if(f->n.dot(w->w) - f->d < -tolerance)
jpan's avatar
jpan committed
748
    {
749
      SimplexF* nf = newFace(f->vertex[e1], f->vertex[e], w, false);
jpan's avatar
jpan committed
750
751
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
      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;
}

784
} // details
jpan's avatar
jpan committed
785
786

} // fcl
787
788

} // namespace hpp