GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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 |
} |
Generated by: GCOVR (Version 4.2) |