GCC Code Coverage Report

Directory: ./
File: test/gjk.cpp
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 259 287 90.2%
Branches: 731 1526 47.9%
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 COAL_GJK
38 #include <time.h>
39 #include <boost/test/included/unit_test.hpp>
40
41 #include <Eigen/Geometry>
42 #include "coal/narrowphase/narrowphase.h"
43 #include "coal/shape/geometric_shapes.h"
44 #include "coal/internal/tools.h"
45 #include "coal/internal/shape_shape_func.h"
46
47 #include "utility.h"
48
49 using coal::GJKSolver;
50 using coal::GJKVariant;
51 using coal::Matrix3s;
52 using coal::Quats;
53 using coal::Scalar;
54 using coal::Transform3s;
55 using coal::TriangleP;
56 using coal::Vec3s;
57
58 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> vector_t;
59 typedef Eigen::Matrix<Scalar, 6, 1> vector6_t;
60 typedef Eigen::Matrix<Scalar, 4, 1> vector4_t;
61 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
62
63 struct Result {
64 bool collision;
65 clock_t timeGjk;
66 clock_t timeGte;
67 }; // struct benchmark
68
69 typedef std::vector<Result> Results_t;
70
71 2 void test_gjk_distance_triangle_triangle(
72 bool enable_gjk_nesterov_acceleration) {
73 Eigen::IOFormat numpy(Eigen::FullPrecision, Eigen::DontAlignCols, ", ", ", ",
74
7/14
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2 times.
✗ Branch 26 not taken.
 4 "np.array ((", "))", "", "");
75 Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ",
76
7/14
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2 times.
✗ Branch 26 not taken.
 4 "", "", "(", ")");
77 2 std::size_t N = 10000;
78
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 GJKSolver solver;
79
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
 2 if (enable_gjk_nesterov_acceleration)
80 1 solver.gjk.gjk_variant = GJKVariant::NesterovAcceleration;
81
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 Transform3s tf1, tf2;
82
4/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
 2 Vec3s p1, p2, a1, a2;
83
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Matrix3s M;
84 2 Scalar distance(sqrt(-Scalar(1)));
85 clock_t start, end;
86
87 2 std::size_t nCol = 0, nDiff = 0;
88 2 Scalar eps = Scalar(1e-7);
89
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 Results_t results(N);
90
2/2
✓ Branch 0 taken 20000 times.
✓ Branch 1 taken 2 times.
 20002 for (std::size_t i = 0; i < N; ++i) {
91
4/8
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20000 times.
✗ Branch 11 not taken.
 20000 Vec3s P1_loc(Vec3s::Random()), P2_loc(Vec3s::Random()),
92
2/4
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
 20000 P3_loc(Vec3s::Random());
93
4/8
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20000 times.
✗ Branch 11 not taken.
 20000 Vec3s Q1_loc(Vec3s::Random()), Q2_loc(Vec3s::Random()),
94
2/4
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
 20000 Q3_loc(Vec3s::Random());
95
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 19998 times.
 20000 if (i == 0) {
96 ✗ P1_loc = Vec3s(Scalar(0.063996093749999997), Scalar(00.15320971679687501),
97
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Scalar(-0.42799999999999999));
98 ✗ P2_loc = Vec3s(Scalar(0.069105957031249998), Scalar(-0.150722900390625),
99
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Scalar(-0.42999999999999999));
100 ✗ P3_loc = Vec3s(Scalar(0.063996093749999997), Scalar(-0.15320971679687501),
101
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Scalar(-0.42999999999999999));
102 ✗ Q1_loc = Vec3s(Scalar(-25.655000000000001), Scalar(-1.2858199462890625),
103
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Scalar(3.7249809570312502));
104 ✗ Q2_loc = Vec3s(Scalar(-10.926), Scalar(-1.284259033203125),
105
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Scalar(3.7281499023437501));
106 ✗ Q3_loc = Vec3s(Scalar(-10.926), Scalar(-1.2866180419921875),
107
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Scalar(3.72335400390625));
108 Transform3s tf(
109 ✗ Quats(Scalar(-0.42437287410898855), Scalar(-0.26862477561450587),
110
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Scalar(-0.46249645019513175), Scalar(0.73064726592483387)),
111 ✗ Vec3s(Scalar(-12.824601270753471), Scalar(-1.6840516940066426),
112
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 4 Scalar(3.8914453043793844)));
113
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 tf1 = tf;
114
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 19996 times.
 19998 } else if (i == 1) {
115 ✗ P1_loc = Vec3s(Scalar(-0.8027043342590332), Scalar(-0.30276307463645935),
116
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Scalar(-0.4372950792312622));
117 ✗ P2_loc = Vec3s(Scalar(-0.8027043342590332), Scalar(0.30276307463645935),
118
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Scalar(-0.4372950792312622));
119 ✗ P3_loc = Vec3s(Scalar(0.8027043342590332), Scalar(0.30276307463645935),
120
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Scalar(-0.4372950792312622));
121 ✗ Q1_loc = Vec3s(Scalar(-0.224713996052742), Scalar(-0.7417119741439819),
122
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Scalar(0.19999997317790985));
123 ✗ Q2_loc = Vec3s(Scalar(-0.5247139930725098), Scalar(-0.7417119741439819),
124
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Scalar(0.19999997317790985));
125 ✗ Q3_loc = Vec3s(Scalar(-0.224713996052742), Scalar(-0.7417119741439819),
126
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Scalar(0.09999997168779373));
127
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Matrix3s R;
128
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Vec3s T;
129
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 R << Scalar(0.9657787025454787), Scalar(0.09400415350535746),
130
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 Scalar(0.24173273843919627), Scalar(-0.06713698817647556),
131
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 Scalar(0.9908494114820345), Scalar(-0.11709000206805695),
132
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 Scalar(-0.25052768814676646), Scalar(0.09685382227587608),
133
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Scalar(0.9632524147814993);
134
135
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 T << Scalar(-0.13491177905469953), -1, Scalar(0.6000449621843792);
136
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 tf1.setRotation(R);
137
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 tf1.setTranslation(T);
138 } else {
139
2/4
✓ Branch 1 taken 19996 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19996 times.
✗ Branch 5 not taken.
 19996 tf1 = Transform3s();
140 }
141
142
1/2
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
 20000 TriangleP tri1(P1_loc, P2_loc, P3_loc);
143
1/2
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
 20000 TriangleP tri2(Q1_loc, Q2_loc, Q3_loc);
144
1/2
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
 20000 Vec3s normal;
145 20000 const bool compute_penetration = true;
146
1/2
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
 20000 coal::DistanceRequest request(compute_penetration, compute_penetration);
147
1/2
✓ Branch 2 taken 20000 times.
✗ Branch 3 not taken.
 20000 coal::DistanceResult result;
148
149 20000 start = clock();
150 // The specialized function TriangleP-TriangleP calls GJK to check for
151 // collision and compute the witness points but it does not use EPA to
152 // compute the penetration depth.
153
1/2
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
 20000 distance = coal::ShapeShapeDistance<TriangleP, TriangleP>(
154 &tri1, tf1, &tri2, tf2, &solver, request, result);
155 20000 end = clock();
156
1/2
✓ Branch 2 taken 20000 times.
✗ Branch 3 not taken.
 20000 p1 = result.nearest_points[0];
157
1/2
✓ Branch 2 taken 20000 times.
✗ Branch 3 not taken.
 20000 p2 = result.nearest_points[1];
158
1/2
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
 20000 normal = result.normal;
159 20000 bool res = (distance <= 0);
160 20000 results[i].timeGjk = end - start;
161 20000 results[i].collision = res;
162
2/2
✓ Branch 0 taken 5664 times.
✓ Branch 1 taken 14336 times.
 20000 if (res) {
163
3/6
✓ Branch 1 taken 5664 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5664 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5664 times.
✗ Branch 8 not taken.
 5664 Vec3s c1, c2, normal2;
164 5664 ++nCol;
165 // check that moving triangle 2 by the penetration depth in the
166 // direction of the normal makes the triangles collision free.
167 5664 Scalar penetration_depth(-distance);
168
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 5664 times.
 5664 assert(penetration_depth >= 0);
169
2/4
✓ Branch 1 taken 5664 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5664 times.
✗ Branch 5 not taken.
 5664 tf2.setTranslation((penetration_depth + 10 - 4) * normal);
170
1/2
✓ Branch 1 taken 5664 times.
✗ Branch 2 not taken.
 5664 result.clear();
171
1/2
✓ Branch 1 taken 5664 times.
✗ Branch 2 not taken.
 5664 distance = coal::ShapeShapeDistance<TriangleP, TriangleP>(
172 &tri1, tf1, &tri2, tf2, &solver, request, result);
173
1/2
✓ Branch 2 taken 5664 times.
✗ Branch 3 not taken.
 5664 c1 = result.nearest_points[0];
174
1/2
✓ Branch 2 taken 5664 times.
✗ Branch 3 not taken.
 5664 c2 = result.nearest_points[1];
175
1/2
✓ Branch 1 taken 5664 times.
✗ Branch 2 not taken.
 5664 normal2 = result.normal;
176 5664 res = (distance <= 0);
177
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 5664 times.
 5664 if (res) {
178 ✗ std::cerr << "P1 = " << P1_loc.format(tuple) << std::endl;
179 ✗ std::cerr << "P2 = " << P2_loc.format(tuple) << std::endl;
180 ✗ std::cerr << "P3 = " << P3_loc.format(tuple) << std::endl;
181 ✗ std::cerr << "Q1 = " << Q1_loc.format(tuple) << std::endl;
182 ✗ std::cerr << "Q2 = " << Q2_loc.format(tuple) << std::endl;
183 ✗ std::cerr << "Q3 = " << Q3_loc.format(tuple) << std::endl;
184 ✗ std::cerr << "p1 = " << c1.format(tuple) << std::endl;
185 ✗ std::cerr << "p2 = " << c2.format(tuple) << std::endl;
186 ✗ std::cerr << "tf1 = " << tf1.getTranslation().format(tuple) << " + "
187 ✗ << tf1.getQuatRotation().coeffs().format(tuple) << std::endl;
188 ✗ std::cerr << "tf2 = " << tf2.getTranslation().format(tuple) << " + "
189 ✗ << tf2.getQuatRotation().coeffs().format(tuple) << std::endl;
190 ✗ std::cerr << "normal = " << normal.format(tuple) << std::endl;
191 ✗ abort();
192 }
193 5664 distance = 0;
194
1/2
✓ Branch 1 taken 5664 times.
✗ Branch 2 not taken.
 5664 tf2.setIdentity();
195 }
196 // Compute vectors between vertices
197
2/4
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
 20000 Vec3s P1(tf1.transform(P1_loc)), P2(tf1.transform(P2_loc)),
198
2/4
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
 20000 P3(tf1.transform(P3_loc)), Q1(tf2.transform(Q1_loc)),
199
2/4
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
 20000 Q2(tf2.transform(Q2_loc)), Q3(tf2.transform(Q3_loc));
200
2/4
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
 20000 Vec3s u1(P2 - P1);
201
2/4
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
 20000 Vec3s v1(P3 - P1);
202
1/2
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
 20000 Vec3s w1(u1.cross(v1));
203
2/4
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
 20000 Vec3s u2(Q2 - Q1);
204
2/4
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
 20000 Vec3s v2(Q3 - Q1);
205
1/2
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
 20000 Vec3s w2(u2.cross(v2));
206
7/14
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 20000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 20000 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 20000 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 20000 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 20000 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 20000 times.
 20000 BOOST_CHECK(w1.squaredNorm() > eps * eps);
207
2/4
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
 20000 M.col(0) = u1;
208
2/4
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
 20000 M.col(1) = v1;
209
2/4
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
 20000 M.col(2) = w1;
210 // Compute a1 such that p1 = P1 + a11 u1 + a12 v1 + a13 u1 x v1
211
4/8
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20000 times.
✗ Branch 11 not taken.
 20000 a1 = M.inverse() * (p1 - P1);
212
28/56
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 20000 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 20000 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20000 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20000 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 20000 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 20000 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 20000 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 20000 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 20000 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 20000 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 20000 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 20000 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 20000 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 20000 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 20000 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 20000 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 20000 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 20000 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 20000 times.
✗ Branch 62 not taken.
✓ Branch 64 taken 20000 times.
✗ Branch 65 not taken.
✓ Branch 67 taken 20000 times.
✗ Branch 68 not taken.
✓ Branch 70 taken 20000 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 20000 times.
✗ Branch 74 not taken.
✓ Branch 76 taken 20000 times.
✗ Branch 77 not taken.
✓ Branch 79 taken 20000 times.
✗ Branch 80 not taken.
✓ Branch 82 taken 20000 times.
✗ Branch 83 not taken.
✗ Branch 93 not taken.
✓ Branch 94 taken 20000 times.
 20000 EIGEN_VECTOR_IS_APPROX(p1, P1 + a1[0] * u1 + a1[1] * v1, eps);
213
7/14
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 20000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 20000 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 20000 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 20000 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 20000 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 20000 times.
 20000 BOOST_CHECK(w2.squaredNorm() > eps * eps);
214 // Compute a2 such that p2 = Q1 + a21 u2 + a22 v2 + a23 u2 x v2
215
2/4
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
 20000 M.col(0) = u2;
216
2/4
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
 20000 M.col(1) = v2;
217
2/4
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
 20000 M.col(2) = w2;
218
4/8
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20000 times.
✗ Branch 11 not taken.
 20000 a2 = M.inverse() * (p2 - Q1);
219
28/56
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 20000 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 20000 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20000 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20000 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 20000 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 20000 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 20000 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 20000 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 20000 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 20000 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 20000 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 20000 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 20000 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 20000 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 20000 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 20000 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 20000 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 20000 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 20000 times.
✗ Branch 62 not taken.
✓ Branch 64 taken 20000 times.
✗ Branch 65 not taken.
✓ Branch 67 taken 20000 times.
✗ Branch 68 not taken.
✓ Branch 70 taken 20000 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 20000 times.
✗ Branch 74 not taken.
✓ Branch 76 taken 20000 times.
✗ Branch 77 not taken.
✓ Branch 79 taken 20000 times.
✗ Branch 80 not taken.
✓ Branch 82 taken 20000 times.
✗ Branch 83 not taken.
✗ Branch 93 not taken.
✓ Branch 94 taken 20000 times.
 20000 EIGEN_VECTOR_IS_APPROX(p2, Q1 + a2[0] * u2 + a2[1] * v2, eps);
220
221 // minimal distance and closest points can be considered as a constrained
222 // optimisation problem:
223 //
224 // min f (a1[0],a1[1], a2[0],a2[1])
225 // g1 (a1[0],a1[1], a2[0],a2[1]) <=0
226 // ...
227 // g6 (a1[0],a1[1], a2[0],a2[1]) <=0
228 // with
229 // f (a1[0],a1[1], a2[0],a2[1]) =
230 // 1 2
231 // --- dist (P1 + a1[0] u1 + a1[1] v1, Q1 + a2[0] u2 + a2[1] v2),
232 // 2
233 // g1 (a1[0],a1[1], a2[0],a2[1]) = -a1[0]
234 // g2 (a1[0],a1[1], a2[0],a2[1]) = -a1[1]
235 // g3 (a1[0],a1[1], a2[0],a2[1]) = a1[0] + a1[1] - 1
236 // g4 (a1[0],a1[1], a2[0],a2[1]) = -a2[0]
237 // g5 (a1[0],a1[1], a2[0],a2[1]) = -a2[1]
238 // g6 (a1[0],a1[1], a2[0],a2[1]) = a2[0] + a2[1] - 1
239
240 // Compute gradient of f
241
1/2
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
 20000 vector4_t grad_f;
242
3/6
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20000 times.
✗ Branch 8 not taken.
 20000 grad_f[0] = -(p2 - p1).dot(u1);
243
3/6
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20000 times.
✗ Branch 8 not taken.
 20000 grad_f[1] = -(p2 - p1).dot(v1);
244
3/6
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20000 times.
✗ Branch 8 not taken.
 20000 grad_f[2] = (p2 - p1).dot(u2);
245
3/6
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20000 times.
✗ Branch 8 not taken.
 20000 grad_f[3] = (p2 - p1).dot(v2);
246
1/2
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
 20000 vector6_t g;
247
2/4
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
 20000 g[0] = -a1[0];
248
2/4
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
 20000 g[1] = -a1[1];
249
3/6
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20000 times.
✗ Branch 8 not taken.
 20000 g[2] = a1[0] + a1[1] - 1;
250
2/4
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
 20000 g[3] = -a2[0];
251
2/4
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
 20000 g[4] = -a2[1];
252
3/6
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20000 times.
✗ Branch 8 not taken.
 20000 g[5] = a2[0] + a2[1] - 1;
253
1/2
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
 20000 matrix_t grad_g(4, 6);
254
1/2
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
 20000 grad_g.setZero();
255
1/2
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
 20000 grad_g(0, 0) = -1.;
256
1/2
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
 20000 grad_g(1, 1) = -1;
257
1/2
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
 20000 grad_g(0, 2) = 1;
258
1/2
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
 20000 grad_g(1, 2) = 1;
259
1/2
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
 20000 grad_g(2, 3) = -1;
260
1/2
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
 20000 grad_g(3, 4) = -1;
261
1/2
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
 20000 grad_g(2, 5) = 1;
262
1/2
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
 20000 grad_g(3, 5) = 1;
263 // Check that closest points are on triangles planes
264 // Projection of [P1p1] on line normal to triangle 1 plane is equal to 0
265
6/12
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 20000 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 20000 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 20000 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20000 times.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✓ Branch 21 taken 20000 times.
 20000 BOOST_CHECK_SMALL(a1[2], eps);
266 // Projection of [Q1p2] on line normal to triangle 2 plane is equal to 0
267
6/12
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 20000 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 20000 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 20000 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20000 times.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✓ Branch 21 taken 20000 times.
 20000 BOOST_CHECK_SMALL(a2[2], eps);
268
269 /* Check Karush–Kuhn–Tucker conditions
270 6
271 __
272 \
273 -grad f = /_ c grad g
274 i=1 i i
275
276 where c >= 0, and
277 i
278 c g = 0 for i between 1 and 6
279 i i
280 */
281
282
1/2
✓ Branch 1 taken 20000 times.
✗ Branch 2 not taken.
 20000 matrix_t Mkkt(4, 6);
283 20000 matrix_t::Index col = 0;
284 // Check that constraints are satisfied
285
2/2
✓ Branch 0 taken 120000 times.
✓ Branch 1 taken 20000 times.
 140000 for (vector6_t::Index j = 0; j < 6; ++j) {
286
7/14
✓ Branch 1 taken 120000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 120000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 120000 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 120000 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 120000 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 120000 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 120000 times.
 120000 BOOST_CHECK(g[j] <= eps);
287 // if constraint is saturated, add gradient in matrix
288
3/4
✓ Branch 1 taken 120000 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 35548 times.
✓ Branch 4 taken 84452 times.
 120000 if (fabs(g[j]) <= eps) {
289
3/6
✓ Branch 1 taken 35548 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35548 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35548 times.
✗ Branch 8 not taken.
 35548 Mkkt.col(col) = grad_g.col(j);
290 35548 ++col;
291 }
292 }
293
2/2
✓ Branch 0 taken 17012 times.
✓ Branch 1 taken 2988 times.
 20000 if (col > 0) {
294
1/2
✓ Branch 1 taken 17012 times.
✗ Branch 2 not taken.
 17012 Mkkt.conservativeResize(4, col);
295 // Compute KKT coefficients ci by inverting
296 // Mkkt.c = -grad_f
297 Eigen::JacobiSVD<matrix_t> svd(Mkkt,
298
1/2
✓ Branch 1 taken 17012 times.
✗ Branch 2 not taken.
 17012 Eigen::ComputeThinU | Eigen::ComputeThinV);
299
3/6
✓ Branch 1 taken 17012 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17012 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17012 times.
✗ Branch 8 not taken.
 17012 vector_t c(svd.solve(-grad_f));
300
2/2
✓ Branch 1 taken 35548 times.
✓ Branch 2 taken 17012 times.
 52560 for (vector_t::Index j = 0; j < c.size(); ++j) {
301
13/26
✓ Branch 1 taken 35548 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 35548 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 35548 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 35548 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 35548 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 35548 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 35548 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 35548 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 35548 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 35548 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 35548 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 35548 times.
✗ Branch 38 not taken.
✗ Branch 47 not taken.
✓ Branch 48 taken 35548 times.
 35548 BOOST_CHECK_MESSAGE(c[j] >= -eps,
302 "c[" << j << "]{" << c[j] << "} is below " << -eps);
303 }
304 17012 }
305 20000 }
306
5/10
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
 2 std::cerr << "nCol / nTotal = " << nCol << " / " << N << std::endl;
307
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 std::cerr << "nDiff = " << nDiff << std::endl;
308 // statistics
309 2 clock_t totalTimeGjkColl = 0;
310 2 clock_t totalTimeGjkNoColl = 0;
311
2/2
✓ Branch 0 taken 20000 times.
✓ Branch 1 taken 2 times.
 20002 for (std::size_t i = 0; i < N; ++i) {
312
2/2
✓ Branch 1 taken 5664 times.
✓ Branch 2 taken 14336 times.
 20000 if (results[i].collision) {
313 5664 totalTimeGjkColl += results[i].timeGjk;
314 } else {
315 14336 totalTimeGjkNoColl += results[i].timeGjk;
316 }
317 }
318
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 std::cerr << "Total / average time gjk: "
319
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 << totalTimeGjkNoColl + totalTimeGjkColl << ", "
320 2 << Scalar(totalTimeGjkNoColl + totalTimeGjkColl) /
321
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Scalar(CLOCKS_PER_SEC * N)
322
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 << "s" << std::endl;
323
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 std::cerr << "-- Collisions -------------------------" << std::endl;
324
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 std::cerr << "Total / average time gjk: " << totalTimeGjkColl << ", "
325
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 << Scalar(totalTimeGjkColl) / Scalar(CLOCKS_PER_SEC * nCol) << "s"
326
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 << std::endl;
327
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 std::cerr << "-- No collisions -------------------------" << std::endl;
328
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 std::cerr << "Total / average time gjk: " << totalTimeGjkNoColl << ", "
329
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 << Scalar(totalTimeGjkNoColl) / Scalar(CLOCKS_PER_SEC * (N - nCol))
330
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 << "s" << std::endl;
331 2 }
332
333
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(distance_triangle_triangle) {
334 2 test_gjk_distance_triangle_triangle(false);
335 2 }
336
337
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(distance_triangle_triangle_nesterov) {
338 2 test_gjk_distance_triangle_triangle(true);
339 2 }
340
341 80 void test_gjk_unit_sphere(Scalar center_distance, Vec3s ray,
342 Scalar swept_sphere_radius,
343 bool use_gjk_nesterov_acceleration) {
344 using namespace coal;
345 80 const Scalar r = 1.0;
346
1/2
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
 80 Sphere sphere(r);
347
1/2
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
 80 sphere.setSweptSphereRadius(swept_sphere_radius);
348
349 typedef Eigen::Matrix<Scalar, 4, 1> Vec4f;
350
5/10
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 80 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 80 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 80 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 80 times.
✗ Branch 14 not taken.
 80 Transform3s tf0(Quats(Vec4f::Random().normalized()), Vec3s::Zero());
351
5/10
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 80 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 80 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 80 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 80 times.
✗ Branch 14 not taken.
 80 Transform3s tf1(Quats(Vec4f::Random().normalized()), center_distance * ray);
352
353 80 bool expect_collision = center_distance <= 2 * (r + swept_sphere_radius);
354
355
1/2
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
 80 details::MinkowskiDiff shape;
356
1/2
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
 80 shape.set<details::SupportOptions::NoSweptSphere>(&sphere, &sphere, tf0, tf1);
357
358
6/12
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 80 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 80 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 80 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 80 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 80 times.
 80 BOOST_CHECK_EQUAL(shape.swept_sphere_radius[0],
359 sphere.radius + sphere.getSweptSphereRadius());
360
6/12
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 80 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 80 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 80 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 80 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 80 times.
 80 BOOST_CHECK_EQUAL(shape.swept_sphere_radius[1],
361 sphere.radius + sphere.getSweptSphereRadius());
362
363
1/2
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
 80 details::GJK gjk(2, Scalar(1e-6));
364
2/2
✓ Branch 0 taken 40 times.
✓ Branch 1 taken 40 times.
 80 if (use_gjk_nesterov_acceleration)
365 40 gjk.gjk_variant = GJKVariant::NesterovAcceleration;
366
4/8
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 80 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 80 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 80 times.
✗ Branch 11 not taken.
 80 details::GJK::Status status = gjk.evaluate(shape, Vec3ps(1, 0, 0));
367
368
2/2
✓ Branch 0 taken 68 times.
✓ Branch 1 taken 12 times.
 80 if (expect_collision) {
369
8/16
✓ Branch 1 taken 68 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 68 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 68 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 68 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 68 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 68 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 68 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 68 times.
 68 BOOST_CHECK((status == details::GJK::Collision) ||
370 (status == details::GJK::CollisionWithPenetrationInformation));
371 // For sphere-sphere, if the distance between the centers is above GJK's
372 // tolerance, the `Collision` status should never be returned.
373
8/16
✓ Branch 1 taken 68 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 68 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 68 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 68 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 68 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 68 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 68 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 68 times.
 68 BOOST_CHECK(status == details::GJK::CollisionWithPenetrationInformation &&
374 center_distance > gjk.getTolerance());
375 } else {
376
5/10
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 12 times.
 12 BOOST_CHECK_EQUAL(status, details::GJK::NoCollision);
377 }
378
379
3/6
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 80 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 80 times.
✗ Branch 8 not taken.
 80 Vec3ps w0_, w1_, normal_;
380
1/2
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
 80 gjk.getWitnessPointsAndNormal(shape, w0_, w1_, normal_);
381
2/4
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 80 times.
✗ Branch 5 not taken.
 80 Vec3s w0 = w0_.cast<Scalar>();
382
2/4
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 80 times.
✗ Branch 5 not taken.
 80 Vec3s w1 = w1_.cast<Scalar>();
383
2/4
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 80 times.
✗ Branch 5 not taken.
 80 Vec3s normal = normal_.cast<Scalar>();
384
385
4/8
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 80 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 80 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 80 times.
✗ Branch 12 not taken.
 80 Vec3s w0_expected(tf0.inverse().transform(tf0.getTranslation() + ray) +
386
2/4
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 80 times.
✗ Branch 5 not taken.
 160 swept_sphere_radius * normal);
387
4/8
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 80 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 80 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 80 times.
✗ Branch 12 not taken.
 80 Vec3s w1_expected(tf0.inverse().transform(tf1.getTranslation() - ray) -
388
2/4
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 80 times.
✗ Branch 5 not taken.
 160 swept_sphere_radius * normal);
389
390
16/32
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 80 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 80 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 80 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 80 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 80 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 80 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 80 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 80 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 80 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 80 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 80 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 80 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 80 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 80 times.
✗ Branch 47 not taken.
✗ Branch 57 not taken.
✓ Branch 58 taken 80 times.
 80 EIGEN_VECTOR_IS_APPROX(w0, w0_expected, Scalar(1e-10));
391
16/32
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 80 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 80 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 80 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 80 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 80 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 80 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 80 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 80 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 80 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 80 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 80 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 80 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 80 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 80 times.
✗ Branch 47 not taken.
✗ Branch 57 not taken.
✓ Branch 58 taken 80 times.
 80 EIGEN_VECTOR_IS_APPROX(w1, w1_expected, Scalar(1e-10));
392 80 }
393
394
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(sphere_sphere) {
395 2 std::array<bool, 2> use_nesterov_acceleration = {false, true};
396 2 std::array<Scalar, 5> swept_sphere_radius = {0., Scalar(0.1), 1, 10, 100};
397
2/2
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 1 times.
 6 for (bool nesterov_acceleration : use_nesterov_acceleration) {
398
2/2
✓ Branch 2 taken 10 times.
✓ Branch 3 taken 2 times.
 24 for (Scalar ssr : swept_sphere_radius) {
399
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 20 test_gjk_unit_sphere(3, Vec3s(1, 0, 0), ssr, nesterov_acceleration);
400
401
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 20 test_gjk_unit_sphere(Scalar(2.01), Vec3s(1, 0, 0), ssr,
402 nesterov_acceleration);
403
404
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 20 test_gjk_unit_sphere(2, Vec3s(1, 0, 0), ssr, nesterov_acceleration);
405
406
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 20 test_gjk_unit_sphere(1, Vec3s(1, 0, 0), ssr, nesterov_acceleration);
407
408 // Random rotation
409
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
 20 test_gjk_unit_sphere(3, Vec3s::Random().normalized(), ssr,
410 nesterov_acceleration);
411
412
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
 20 test_gjk_unit_sphere(Scalar(2.01), Vec3s::Random().normalized(), ssr,
413 nesterov_acceleration);
414
415
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
 20 test_gjk_unit_sphere(2, Vec3s::Random().normalized(), ssr,
416 nesterov_acceleration);
417
418
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
 20 test_gjk_unit_sphere(1, Vec3s::Random().normalized(), ssr,
419 nesterov_acceleration);
420 }
421 }
422 2 }
423
424 6 void test_gjk_triangle_capsule(Vec3s T, bool expect_collision,
425 bool use_gjk_nesterov_acceleration,
426 Vec3s w0_expected, Vec3s w1_expected) {
427 using namespace coal;
428
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 Capsule capsule(1., 2.); // Radius 1 and length 2
429
4/8
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
 6 TriangleP triangle(Vec3s(0., 0., 0.), Vec3s(1., 0., 0.), Vec3s(1., 1., 0.));
430
431
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 Transform3s tf0, tf1;
432
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 tf1.setTranslation(T);
433
434
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 details::MinkowskiDiff shape;
435 // No need to take into account swept-sphere radius in supports computation
436 // when using GJK/EPA; after they have converged, these algos will correctly
437 // handle the swept-sphere radius of the shapes.
438
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 shape.set<details::SupportOptions::NoSweptSphere>(&capsule, &triangle, tf0,
439 tf1);
440
441
6/12
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6 times.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✓ Branch 21 taken 6 times.
 6 BOOST_CHECK_EQUAL(shape.swept_sphere_radius[0], capsule.radius);
442
6/12
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6 times.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✓ Branch 21 taken 6 times.
 6 BOOST_CHECK_EQUAL(shape.swept_sphere_radius[1], 0.);
443
444
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 details::GJK gjk(10, SolverScalar(1e-6));
445
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
 6 if (use_gjk_nesterov_acceleration)
446 3 gjk.gjk_variant = GJKVariant::NesterovAcceleration;
447
4/8
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
 6 details::GJK::Status status = gjk.evaluate(shape, Vec3ps(1, 0, 0));
448
449
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 2 times.
 6 if (expect_collision) {
450
9/16
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 2 times.
✓ Branch 14 taken 2 times.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 4 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 4 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 4 times.
 4 BOOST_CHECK((status == details::GJK::Collision) ||
451 (status == details::GJK::CollisionWithPenetrationInformation));
452 } else {
453
5/10
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 2 times.
 2 BOOST_CHECK_EQUAL(status, details::GJK::NoCollision);
454
455 // Check that guess works as expected
456
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Vec3ps guess = gjk.getGuessFromSimplex();
457
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 details::GJK gjk2(3, SolverScalar(1e-6));
458
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 details::GJK::Status status2 = gjk2.evaluate(shape, guess);
459
5/10
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 2 times.
 2 BOOST_CHECK_EQUAL(status2, details::GJK::NoCollision);
460 }
461
462
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 6 Vec3ps w0_, w1_, normal_;
463
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 2 times.
 6 if (status == details::GJK::NoCollision ||
464
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 2 times.
 4 status == details::GJK::CollisionWithPenetrationInformation) {
465
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 gjk.getWitnessPointsAndNormal(shape, w0_, w1_, normal_);
466 } else {
467
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 details::EPA epa(64, SolverScalar(1e-6));
468
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 details::EPA::Status epa_status = epa.evaluate(gjk, Vec3ps(1, 0, 0));
469
5/10
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 2 times.
 2 BOOST_CHECK_EQUAL(epa_status, details::EPA::AccuracyReached);
470
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 epa.getWitnessPointsAndNormal(shape, w0_, w1_, normal_);
471 2 }
472
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 Vec3s w0 = w0_.cast<Scalar>();
473
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 Vec3s w1 = w1_.cast<Scalar>();
474
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 Vec3s normal = normal_.cast<Scalar>();
475 COAL_UNUSED_VARIABLE(normal);
476
477
16/32
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 6 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 6 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 6 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 6 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 6 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 6 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 6 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 6 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 6 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 6 times.
✗ Branch 47 not taken.
✗ Branch 57 not taken.
✓ Branch 58 taken 6 times.
 6 EIGEN_VECTOR_IS_APPROX(w0, w0_expected, Scalar(1e-10));
478
18/36
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 6 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 6 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 6 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 6 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 6 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 6 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 6 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 6 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 6 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 6 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 6 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 6 times.
✗ Branch 53 not taken.
✗ Branch 63 not taken.
✓ Branch 64 taken 6 times.
 6 EIGEN_VECTOR_IS_APPROX(w1 - T, w1_expected, Scalar(1e-10));
479 6 }
480
481
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(triangle_capsule) {
482 // GJK -> no collision
483
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 test_gjk_triangle_capsule(Vec3s(Scalar(1.01), 0, 0), false, false,
484
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Vec3s(1, 0, 0), Vec3s(0., 0, 0));
485 // GJK + Nesterov acceleration -> no collision
486
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 test_gjk_triangle_capsule(Vec3s(Scalar(1.01), 0, 0), false, true,
487
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Vec3s(1, 0, 0), Vec3s(0., 0, 0));
488
489 // GJK -> collision
490
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 test_gjk_triangle_capsule(Vec3s(Scalar(0.5), 0, 0), true, false,
491
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Vec3s(1, 0, 0), Vec3s(0., 0, 0));
492 // GJK + Nesterov acceleration -> collision
493
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 test_gjk_triangle_capsule(Vec3s(0.5, 0, 0), true, true, Vec3s(1., 0, 0),
494
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Vec3s(0., 0, 0));
495
496 // GJK + EPA -> collision
497
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 test_gjk_triangle_capsule(Vec3s(Scalar(-0.5), Scalar(-0.01), 0), true, false,
498
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Vec3s(0, 1, 0), Vec3s(Scalar(0.5), 0, 0));
499 // GJK + Nesterov accleration + EPA -> collision
500
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 test_gjk_triangle_capsule(Vec3s(Scalar(-0.5), Scalar(-0.01), 0), true, true,
501
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Vec3s(0, 1, 0), Vec3s(Scalar(0.5), 0, 0));
502 2 }
503