GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: test/gjk.cpp Lines: 233 250 93.2 %
Date: 2024-02-09 12:57:42 Branches: 654 1376 47.5 %

Line Branch Exec Source
1
/*
2
 *  Software License Agreement (BSD License)
3
 *
4
 *  Copyright (c) 2018, CNRS-LAAS.
5
 *  All rights reserved.
6
 *
7
 *  Redistribution and use in source and binary forms, with or without
8
 *  modification, are permitted provided that the following conditions
9
 *  are met:
10
 *
11
 *   * Redistributions of source code must retain the above copyright
12
 *     notice, this list of conditions and the following disclaimer.
13
 *   * Redistributions in binary form must reproduce the above
14
 *     copyright notice, this list of conditions and the following
15
 *     disclaimer in the documentation and/or other materials provided
16
 *     with the distribution.
17
 *   * Neither the name of Willow Garage, Inc. nor the names of its
18
 *     contributors may be used to endorse or promote products derived
19
 *     from this software without specific prior written permission.
20
 *
21
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32
 *  POSSIBILITY OF SUCH DAMAGE.
33
 */
34
35
/** \author Florent Lamiraux <florent@laas.fr> */
36
37
#define BOOST_TEST_MODULE FCL_GJK
38
#include <time.h>
39
#include <boost/test/included/unit_test.hpp>
40
41
#include <Eigen/Geometry>
42
#include <hpp/fcl/narrowphase/narrowphase.h>
43
#include <hpp/fcl/shape/geometric_shapes.h>
44
#include <hpp/fcl/internal/tools.h>
45
46
#include "utility.h"
47
48
using hpp::fcl::FCL_REAL;
49
using hpp::fcl::GJKSolver;
50
using hpp::fcl::GJKVariant;
51
using hpp::fcl::Matrix3f;
52
using hpp::fcl::Quaternion3f;
53
using hpp::fcl::Transform3f;
54
using hpp::fcl::TriangleP;
55
using hpp::fcl::Vec3f;
56
57
typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 1> vector_t;
58
typedef Eigen::Matrix<FCL_REAL, 6, 1> vector6_t;
59
typedef Eigen::Matrix<FCL_REAL, 4, 1> vector4_t;
60
typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
61
62
struct Result {
63
  bool collision;
64
  clock_t timeGjk;
65
  clock_t timeGte;
66
};  // struct benchmark
67
68
typedef std::vector<Result> Results_t;
69
70
2
void test_gjk_distance_triangle_triangle(
71
    bool enable_gjk_nesterov_acceleration) {
72
  Eigen::IOFormat numpy(Eigen::FullPrecision, Eigen::DontAlignCols, ", ", ", ",
73



6
                        "np.array ((", "))", "", "");
74
  Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ",
75



6
                        "", "", "(", ")");
76
2
  std::size_t N = 10000;
77
2
  GJKSolver solver;
78
2
  if (enable_gjk_nesterov_acceleration)
79
1
    solver.gjk_variant = GJKVariant::NesterovAcceleration;
80

2
  Transform3f tf1, tf2;
81


2
  Vec3f p1, p2, a1, a2;
82
2
  Matrix3f M;
83
2
  FCL_REAL distance(sqrt(-1));
84
  clock_t start, end;
85
86
2
  std::size_t nCol = 0, nDiff = 0;
87
2
  FCL_REAL eps = 1e-7;
88
4
  Results_t results(N);
89
20002
  for (std::size_t i = 0; i < N; ++i) {
90


20000
    Vec3f P1_loc(Vec3f::Random()), P2_loc(Vec3f::Random()),
91

20000
        P3_loc(Vec3f::Random());
92


20000
    Vec3f Q1_loc(Vec3f::Random()), Q2_loc(Vec3f::Random()),
93

20000
        Q3_loc(Vec3f::Random());
94
20000
    if (i == 0) {
95
      P1_loc = Vec3f(0.063996093749999997, -0.15320971679687501,
96
2
                     -0.42799999999999999);
97
      P2_loc =
98
2
          Vec3f(0.069105957031249998, -0.150722900390625, -0.42999999999999999);
99
      P3_loc = Vec3f(0.063996093749999997, -0.15320971679687501,
100
2
                     -0.42999999999999999);
101
      Q1_loc =
102
2
          Vec3f(-25.655000000000001, -1.2858199462890625, 3.7249809570312502);
103
2
      Q2_loc = Vec3f(-10.926, -1.284259033203125, 3.7281499023437501);
104
2
      Q3_loc = Vec3f(-10.926, -1.2866180419921875, 3.72335400390625);
105
      Transform3f tf(
106
          Quaternion3f(-0.42437287410898855, -0.26862477561450587,
107
2
                       -0.46249645019513175, 0.73064726592483387),
108

4
          Vec3f(-12.824601270753471, -1.6840516940066426, 3.8914453043793844));
109
2
      tf1 = tf;
110
19998
    } else if (i == 1) {
111
      P1_loc =
112
2
          Vec3f(-0.8027043342590332, -0.30276307463645935, -0.4372950792312622);
113
      P2_loc =
114
2
          Vec3f(-0.8027043342590332, 0.30276307463645935, -0.4372950792312622);
115
      P3_loc =
116
2
          Vec3f(0.8027043342590332, 0.30276307463645935, -0.4372950792312622);
117
      Q1_loc =
118
2
          Vec3f(-0.224713996052742, -0.7417119741439819, 0.19999997317790985);
119
      Q2_loc =
120
2
          Vec3f(-0.5247139930725098, -0.7417119741439819, 0.19999997317790985);
121
      Q3_loc =
122
2
          Vec3f(-0.224713996052742, -0.7417119741439819, 0.09999997168779373);
123
2
      Matrix3f R;
124
2
      Vec3f T;
125

2
      R << 0.9657787025454787, 0.09400415350535746, 0.24173273843919627,
126

2
          -0.06713698817647556, 0.9908494114820345, -0.11709000206805695,
127

2
          -0.25052768814676646, 0.09685382227587608, 0.9632524147814993;
128
129

2
      T << -0.13491177905469953, -1, 0.6000449621843792;
130
2
      tf1.setRotation(R);
131
2
      tf1.setTranslation(T);
132
    } else {
133

19996
      tf1 = Transform3f();
134
    }
135
136
40000
    TriangleP tri1(P1_loc, P2_loc, P3_loc);
137
40000
    TriangleP tri2(Q1_loc, Q2_loc, Q3_loc);
138
20000
    Vec3f normal;
139
140
    bool res;
141
20000
    start = clock();
142
20000
    res = solver.shapeDistance(tri1, tf1, tri2, tf2, distance, p1, p2, normal);
143
20000
    end = clock();
144
20000
    results[i].timeGjk = end - start;
145
20000
    results[i].collision = !res;
146
20000
    assert(res == (distance > 0));
147
20000
    if (!res) {
148

5664
      Vec3f c1, c2, normal2;
149
5664
      ++nCol;
150
      // check that moving triangle 2 by the penetration depth in the
151
      // direction of the normal makes the triangles collision free.
152
5664
      FCL_REAL penetration_depth(-distance);
153
5664
      assert(penetration_depth >= 0);
154

5664
      tf2.setTranslation((penetration_depth + 10 - 4) * normal);
155
      res =
156
5664
          solver.shapeDistance(tri1, tf1, tri2, tf2, distance, c1, c2, normal2);
157
5664
      if (!res) {
158
        std::cerr << "P1 = " << P1_loc.format(tuple) << std::endl;
159
        std::cerr << "P2 = " << P2_loc.format(tuple) << std::endl;
160
        std::cerr << "P3 = " << P3_loc.format(tuple) << std::endl;
161
        std::cerr << "Q1 = " << Q1_loc.format(tuple) << std::endl;
162
        std::cerr << "Q2 = " << Q2_loc.format(tuple) << std::endl;
163
        std::cerr << "Q3 = " << Q3_loc.format(tuple) << std::endl;
164
        std::cerr << "p1 = " << c1.format(tuple) << std::endl;
165
        std::cerr << "p2 = " << c2.format(tuple) << std::endl;
166
        std::cerr << "tf1 = " << tf1.getTranslation().format(tuple) << " + "
167
                  << tf1.getQuatRotation().coeffs().format(tuple) << std::endl;
168
        std::cerr << "tf2 = " << tf2.getTranslation().format(tuple) << " + "
169
                  << tf2.getQuatRotation().coeffs().format(tuple) << std::endl;
170
        std::cerr << "normal = " << normal.format(tuple) << std::endl;
171
        abort();
172
      }
173
5664
      distance = 0;
174
5664
      tf2.setIdentity();
175
    }
176
    // Compute vectors between vertices
177

20000
    Vec3f P1(tf1.transform(P1_loc)), P2(tf1.transform(P2_loc)),
178

20000
        P3(tf1.transform(P3_loc)), Q1(tf2.transform(Q1_loc)),
179

20000
        Q2(tf2.transform(Q2_loc)), Q3(tf2.transform(Q3_loc));
180

20000
    Vec3f u1(P2 - P1);
181

20000
    Vec3f v1(P3 - P1);
182
20000
    Vec3f w1(u1.cross(v1));
183

20000
    Vec3f u2(Q2 - Q1);
184

20000
    Vec3f v2(Q3 - Q1);
185
20000
    Vec3f w2(u2.cross(v2));
186



20000
    BOOST_CHECK(w1.squaredNorm() > eps * eps);
187

20000
    M.col(0) = u1;
188

20000
    M.col(1) = v1;
189

20000
    M.col(2) = w1;
190
    // Compute a1 such that p1 = P1 + a11 u1 + a12 v1 + a13 u1 x v1
191


20000
    a1 = M.inverse() * (p1 - P1);
192













20000
    EIGEN_VECTOR_IS_APPROX(p1, P1 + a1[0] * u1 + a1[1] * v1, eps);
193



20000
    BOOST_CHECK(w2.squaredNorm() > eps * eps);
194
    // Compute a2 such that p2 = Q1 + a21 u2 + a22 v2 + a23 u2 x v2
195

20000
    M.col(0) = u2;
196

20000
    M.col(1) = v2;
197

20000
    M.col(2) = w2;
198


20000
    a2 = M.inverse() * (p2 - Q1);
199













20000
    EIGEN_VECTOR_IS_APPROX(p2, Q1 + a2[0] * u2 + a2[1] * v2, eps);
200
201
    // minimal distance and closest points can be considered as a constrained
202
    // optimisation problem:
203
    //
204
    // min f (a1[0],a1[1], a2[0],a2[1])
205
    //   g1 (a1[0],a1[1], a2[0],a2[1]) <=0
206
    //   ...
207
    //   g6 (a1[0],a1[1], a2[0],a2[1]) <=0
208
    // with
209
    //  f (a1[0],a1[1], a2[0],a2[1]) =
210
    //         1                                                           2
211
    //        --- dist (P1 + a1[0] u1 + a1[1] v1, Q1 + a2[0] u2 + a2[1] v2),
212
    //         2
213
    //  g1 (a1[0],a1[1], a2[0],a2[1]) = -a1[0]
214
    //  g2 (a1[0],a1[1], a2[0],a2[1]) = -a1[1]
215
    //  g3 (a1[0],a1[1], a2[0],a2[1]) =  a1[0] + a1[1] - 1
216
    //  g4 (a1[0],a1[1], a2[0],a2[1]) = -a2[0]
217
    //  g5 (a1[0],a1[1], a2[0],a2[1]) = -a2[1]
218
    //  g6 (a1[0],a1[1], a2[0],a2[1]) =  a2[0] + a2[1] - 1
219
220
    // Compute gradient of f
221
20000
    vector4_t grad_f;
222

20000
    grad_f[0] = -(p2 - p1).dot(u1);
223

20000
    grad_f[1] = -(p2 - p1).dot(v1);
224

20000
    grad_f[2] = (p2 - p1).dot(u2);
225

20000
    grad_f[3] = (p2 - p1).dot(v2);
226
20000
    vector6_t g;
227

20000
    g[0] = -a1[0];
228

20000
    g[1] = -a1[1];
229

20000
    g[2] = a1[0] + a1[1] - 1;
230

20000
    g[3] = -a2[0];
231

20000
    g[4] = -a2[1];
232

20000
    g[5] = a2[0] + a2[1] - 1;
233
40000
    matrix_t grad_g(4, 6);
234
20000
    grad_g.setZero();
235
20000
    grad_g(0, 0) = -1.;
236
20000
    grad_g(1, 1) = -1;
237
20000
    grad_g(0, 2) = 1;
238
20000
    grad_g(1, 2) = 1;
239
20000
    grad_g(2, 3) = -1;
240
20000
    grad_g(3, 4) = -1;
241
20000
    grad_g(2, 5) = 1;
242
20000
    grad_g(3, 5) = 1;
243
    // Check that closest points are on triangles planes
244
    // Projection of [P1p1] on line normal to triangle 1 plane is equal to 0
245



20000
    BOOST_CHECK_SMALL(a1[2], eps);
246
    // Projection of [Q1p2] on line normal to triangle 2 plane is equal to 0
247



20000
    BOOST_CHECK_SMALL(a2[2], eps);
248
249
    /* Check Karush–Kuhn–Tucker conditions
250
                    6
251
                   __
252
                   \
253
         -grad f = /_  c  grad g
254
                   i=1  i       i
255
256
         where c  >= 0, and
257
                i
258
         c  g  = 0 for i between 1 and 6
259
          i  i
260
    */
261
262
40000
    matrix_t Mkkt(4, 6);
263
20000
    matrix_t::Index col = 0;
264
    // Check that constraints are satisfied
265
140000
    for (vector6_t::Index j = 0; j < 6; ++j) {
266



120000
      BOOST_CHECK(g[j] <= eps);
267
      // if constraint is saturated, add gradient in matrix
268

120000
      if (fabs(g[j]) <= eps) {
269

35625
        Mkkt.col(col) = grad_g.col(j);
270
35625
        ++col;
271
      }
272
    }
273
20000
    if (col > 0) {
274
17089
      Mkkt.conservativeResize(4, col);
275
      // Compute KKT coefficients ci by inverting
276
      // Mkkt.c = -grad_f
277
      Eigen::JacobiSVD<matrix_t> svd(Mkkt,
278
34178
                                     Eigen::ComputeThinU | Eigen::ComputeThinV);
279

34178
      vector_t c(svd.solve(-grad_f));
280

52714
      for (vector_t::Index j = 0; j < c.size(); ++j) {
281






35625
        BOOST_CHECK_MESSAGE(c[j] >= -eps,
282
                            "c[" << j << "]{" << c[j] << "} is below " << -eps);
283
      }
284
    }
285
  }
286


2
  std::cerr << "nCol / nTotal = " << nCol << " / " << N << std::endl;
287

2
  std::cerr << "nDiff = " << nDiff << std::endl;
288
  // statistics
289
2
  clock_t totalTimeGjkColl = 0;
290
2
  clock_t totalTimeGjkNoColl = 0;
291
20002
  for (std::size_t i = 0; i < N; ++i) {
292
20000
    if (results[i].collision) {
293
5664
      totalTimeGjkColl += results[i].timeGjk;
294
    } else {
295
14336
      totalTimeGjkNoColl += results[i].timeGjk;
296
    }
297
  }
298
2
  std::cerr << "Total / average time gjk: "
299

2
            << totalTimeGjkNoColl + totalTimeGjkColl << ", "
300
2
            << FCL_REAL(totalTimeGjkNoColl + totalTimeGjkColl) /
301
2
                   FCL_REAL(CLOCKS_PER_SEC * N)
302

2
            << "s" << std::endl;
303

2
  std::cerr << "-- Collisions -------------------------" << std::endl;
304

2
  std::cerr << "Total / average time gjk: " << totalTimeGjkColl << ", "
305
2
            << FCL_REAL(totalTimeGjkColl) / FCL_REAL(CLOCKS_PER_SEC * nCol)
306

2
            << "s" << std::endl;
307

2
  std::cerr << "-- No collisions -------------------------" << std::endl;
308

2
  std::cerr << "Total / average time gjk: " << totalTimeGjkNoColl << ", "
309
2
            << FCL_REAL(totalTimeGjkNoColl) /
310
2
                   FCL_REAL(CLOCKS_PER_SEC * (N - nCol))
311

2
            << "s" << std::endl;
312
2
}
313
314
















4
BOOST_AUTO_TEST_CASE(distance_triangle_triangle_1) {
315
2
  test_gjk_distance_triangle_triangle(false);
316
2
  test_gjk_distance_triangle_triangle(true);
317
2
}
318
319
16
void test_gjk_unit_sphere(FCL_REAL center_distance, Vec3f ray,
320
                          bool expect_collision,
321
                          bool use_gjk_nesterov_acceleration) {
322
  using namespace hpp::fcl;
323
32
  Sphere sphere(1.);
324
325
  typedef Eigen::Matrix<FCL_REAL, 4, 1> Vec4f;
326


16
  Transform3f tf0(Quaternion3f(Vec4f::Random().normalized()), Vec3f::Zero()),
327


16
      tf1(Quaternion3f(Vec4f::Random().normalized()), center_distance * ray);
328
329
32
  details::MinkowskiDiff shape;
330
16
  shape.set(&sphere, &sphere, tf0, tf1);
331
332



16
  BOOST_CHECK_EQUAL(shape.inflation[0], sphere.radius);
333



16
  BOOST_CHECK_EQUAL(shape.inflation[1], sphere.radius);
334
335
16
  details::GJK gjk(2, 1e-6);
336
16
  if (use_gjk_nesterov_acceleration)
337
8
    gjk.gjk_variant = GJKVariant::NesterovAcceleration;
338


16
  details::GJK::Status status = gjk.evaluate(shape, Vec3f(1, 0, 0));
339
340
16
  if (expect_collision)
341


8
    BOOST_CHECK_EQUAL(status, details::GJK::Inside);
342
  else
343


8
    BOOST_CHECK_EQUAL(status, details::GJK::Valid);
344
345

16
  Vec3f w0, w1;
346
16
  gjk.getClosestPoints(shape, w0, w1);
347
348

16
  Vec3f w0_expected(tf0.inverse().transform(tf0.getTranslation() + ray));
349

16
  Vec3f w1_expected(tf0.inverse().transform(tf1.getTranslation() - ray));
350
351







16
  EIGEN_VECTOR_IS_APPROX(w0, w0_expected, 1e-10);
352







16
  EIGEN_VECTOR_IS_APPROX(w1, w1_expected, 1e-10);
353
16
}
354
355
















4
BOOST_AUTO_TEST_CASE(sphere_sphere) {
356

2
  test_gjk_unit_sphere(3., Vec3f(1, 0, 0), false, false);
357

2
  test_gjk_unit_sphere(3., Vec3f(1, 0, 0), false, true);
358

2
  test_gjk_unit_sphere(2.01, Vec3f(1, 0, 0), false, false);
359

2
  test_gjk_unit_sphere(2.01, Vec3f(1, 0, 0), false, true);
360

2
  test_gjk_unit_sphere(2., Vec3f(1, 0, 0), true, false);
361

2
  test_gjk_unit_sphere(2., Vec3f(1, 0, 0), true, true);
362

2
  test_gjk_unit_sphere(1., Vec3f(1, 0, 0), true, false);
363

2
  test_gjk_unit_sphere(1., Vec3f(1, 0, 0), true, true);
364
365

2
  test_gjk_unit_sphere(3., Vec3f::Random().normalized(), false, false);
366

2
  test_gjk_unit_sphere(3., Vec3f::Random().normalized(), false, true);
367

2
  test_gjk_unit_sphere(2.01, Vec3f::Random().normalized(), false, false);
368

2
  test_gjk_unit_sphere(2.01, Vec3f::Random().normalized(), false, true);
369

2
  test_gjk_unit_sphere(2., Vec3f::Random().normalized(), true, false);
370

2
  test_gjk_unit_sphere(2., Vec3f::Random().normalized(), true, true);
371

2
  test_gjk_unit_sphere(1., Vec3f::Random().normalized(), true, false);
372

2
  test_gjk_unit_sphere(1., Vec3f::Random().normalized(), true, true);
373
2
}
374
375
6
void test_gjk_triangle_capsule(Vec3f T, bool expect_collision,
376
                               bool use_gjk_nesterov_acceleration,
377
                               Vec3f w0_expected, Vec3f w1_expected) {
378
  using namespace hpp::fcl;
379
12
  Capsule capsule(1., 2.);  // Radius 1 and length 2
380


12
  TriangleP triangle(Vec3f(0., 0., 0.), Vec3f(1., 0., 0.), Vec3f(1., 1., 0.));
381
382

6
  Transform3f tf0, tf1;
383
6
  tf1.setTranslation(T);
384
385
12
  details::MinkowskiDiff shape;
386
6
  shape.set(&capsule, &triangle, tf0, tf1);
387
388



6
  BOOST_CHECK_EQUAL(shape.inflation[0], capsule.radius);
389



6
  BOOST_CHECK_EQUAL(shape.inflation[1], 0.);
390
391
6
  details::GJK gjk(10, 1e-6);
392
6
  if (use_gjk_nesterov_acceleration)
393
3
    gjk.gjk_variant = GJKVariant::NesterovAcceleration;
394


6
  details::GJK::Status status = gjk.evaluate(shape, Vec3f(1, 0, 0));
395
396
6
  if (expect_collision)
397


4
    BOOST_CHECK_EQUAL(status, details::GJK::Inside);
398
  else {
399


2
    BOOST_CHECK_EQUAL(status, details::GJK::Valid);
400
401
    // Check that guess works as expected
402
2
    Vec3f guess = gjk.getGuessFromSimplex();
403
2
    details::GJK gjk2(3, 1e-6);
404

2
    details::GJK::Status status2 = gjk2.evaluate(shape, guess);
405


2
    BOOST_CHECK_EQUAL(status2, details::GJK::Valid);
406
  }
407
408

6
  Vec3f w0, w1;
409


6
  if (status == details::GJK::Valid || gjk.hasPenetrationInformation(shape)) {
410
4
    gjk.getClosestPoints(shape, w0, w1);
411
  } else {
412
4
    details::EPA epa(128, 64, 255, 1e-6);
413

2
    details::EPA::Status epa_status = epa.evaluate(gjk, Vec3f(1, 0, 0));
414


2
    BOOST_CHECK_EQUAL(epa_status, details::EPA::AccuracyReached);
415
2
    epa.getClosestPoints(shape, w0, w1);
416
  }
417
418







6
  EIGEN_VECTOR_IS_APPROX(w0, w0_expected, 1e-10);
419








6
  EIGEN_VECTOR_IS_APPROX(w1 - T, w1_expected, 1e-10);
420
6
}
421
422
















4
BOOST_AUTO_TEST_CASE(triangle_capsule) {
423
  // GJK -> no collision
424

2
  test_gjk_triangle_capsule(Vec3f(1.01, 0, 0), false, false, Vec3f(1., 0, 0),
425
2
                            Vec3f(0., 0, 0));
426
  // GJK + Nesterov acceleration -> no collision
427

2
  test_gjk_triangle_capsule(Vec3f(1.01, 0, 0), false, true, Vec3f(1., 0, 0),
428
2
                            Vec3f(0., 0, 0));
429
430
  // GJK -> collision
431

2
  test_gjk_triangle_capsule(Vec3f(0.5, 0, 0), true, false, Vec3f(1., 0, 0),
432
2
                            Vec3f(0., 0, 0));
433
  // GJK + Nesterov acceleration -> collision
434

2
  test_gjk_triangle_capsule(Vec3f(0.5, 0, 0), true, true, Vec3f(1., 0, 0),
435
2
                            Vec3f(0., 0, 0));
436
437
  // GJK + EPA -> collision
438

2
  test_gjk_triangle_capsule(Vec3f(-0.5, -0.01, 0), true, false, Vec3f(0, 1, 0),
439
2
                            Vec3f(0.5, 0, 0));
440
  // GJK + Nesterov accleration + EPA -> collision
441

2
  test_gjk_triangle_capsule(Vec3f(-0.5, -0.01, 0), true, true, Vec3f(0, 1, 0),
442
2
                            Vec3f(0.5, 0, 0));
443
2
}