37 #define BOOST_TEST_MODULE FCL_GJK
39 #include <boost/test/included/unit_test.hpp>
41 #include <Eigen/Geometry>
57 typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 1>
vector_t;
60 typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, Eigen::Dynamic>
matrix_t;
71 bool enable_gjk_nesterov_acceleration) {
72 Eigen::IOFormat numpy(Eigen::FullPrecision, Eigen::DontAlignCols,
", ",
", ",
73 "np.array ((",
"))",
"",
"");
74 Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols,
"",
", ",
76 std::size_t N = 10000;
78 if (enable_gjk_nesterov_acceleration)
86 std::size_t nCol = 0, nDiff = 0;
89 for (std::size_t i = 0; i < N; ++i) {
90 Vec3f P1_loc(Vec3f::Random()), P2_loc(Vec3f::Random()),
91 P3_loc(Vec3f::Random());
92 Vec3f Q1_loc(Vec3f::Random()), Q2_loc(Vec3f::Random()),
93 Q3_loc(Vec3f::Random());
95 P1_loc =
Vec3f(0.063996093749999997, -0.15320971679687501,
96 -0.42799999999999999);
98 Vec3f(0.069105957031249998, -0.150722900390625, -0.42999999999999999);
99 P3_loc =
Vec3f(0.063996093749999997, -0.15320971679687501,
100 -0.42999999999999999);
102 Vec3f(-25.655000000000001, -1.2858199462890625, 3.7249809570312502);
103 Q2_loc =
Vec3f(-10.926, -1.284259033203125, 3.7281499023437501);
104 Q3_loc =
Vec3f(-10.926, -1.2866180419921875, 3.72335400390625);
106 Quaternion3f(-0.42437287410898855, -0.26862477561450587,
107 -0.46249645019513175, 0.73064726592483387),
108 Vec3f(-12.824601270753471, -1.6840516940066426, 3.8914453043793844));
112 Vec3f(-0.8027043342590332, -0.30276307463645935, -0.4372950792312622);
114 Vec3f(-0.8027043342590332, 0.30276307463645935, -0.4372950792312622);
116 Vec3f(0.8027043342590332, 0.30276307463645935, -0.4372950792312622);
118 Vec3f(-0.224713996052742, -0.7417119741439819, 0.19999997317790985);
120 Vec3f(-0.5247139930725098, -0.7417119741439819, 0.19999997317790985);
122 Vec3f(-0.224713996052742, -0.7417119741439819, 0.09999997168779373);
125 R << 0.9657787025454787, 0.09400415350535746, 0.24173273843919627,
126 -0.06713698817647556, 0.9908494114820345, -0.11709000206805695,
127 -0.25052768814676646, 0.09685382227587608, 0.9632524147814993;
129 T << -0.13491177905469953, -1, 0.6000449621843792;
131 tf1.setTranslation(T);
144 results[i].timeGjk = end - start;
145 results[i].collision = !
res;
153 assert(penetration_depth >= 0);
154 tf2.setTranslation((penetration_depth + 10 - 4) * normal);
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;
178 P3(
tf1.transform(P3_loc)),
Q1(
tf2.transform(Q1_loc)),
179 Q2(
tf2.transform(Q2_loc)),
Q3(
tf2.transform(Q3_loc));
182 Vec3f w1(u1.cross(v1));
185 Vec3f w2(u2.cross(v2));
186 BOOST_CHECK(w1.squaredNorm() >
eps *
eps);
191 a1 =
M.inverse() * (
p1 -
P1);
193 BOOST_CHECK(w2.squaredNorm() >
eps *
eps);
198 a2 =
M.inverse() * (p2 -
Q1);
222 grad_f[0] = -(p2 -
p1).dot(u1);
223 grad_f[1] = -(p2 -
p1).dot(v1);
224 grad_f[2] = (p2 -
p1).dot(u2);
225 grad_f[3] = (p2 -
p1).dot(v2);
229 g[2] = a1[0] + a1[1] - 1;
232 g[5] = a2[0] + a2[1] - 1;
245 BOOST_CHECK_SMALL(a1[2],
eps);
247 BOOST_CHECK_SMALL(a2[2],
eps);
263 matrix_t::Index col = 0;
265 for (vector6_t::Index j = 0; j < 6; ++j) {
266 BOOST_CHECK(g[j] <=
eps);
268 if (fabs(g[j]) <=
eps) {
269 Mkkt.col(col) = grad_g.col(j);
274 Mkkt.conservativeResize(4, col);
277 Eigen::JacobiSVD<matrix_t> svd(Mkkt,
278 Eigen::ComputeThinU | Eigen::ComputeThinV);
280 for (vector_t::Index j = 0; j <
c.size(); ++j) {
281 BOOST_CHECK_MESSAGE(
c[j] >= -
eps,
282 "c[" << j <<
"]{" <<
c[j] <<
"} is below " << -
eps);
286 std::cerr <<
"nCol / nTotal = " << nCol <<
" / " << N << std::endl;
287 std::cerr <<
"nDiff = " << nDiff << std::endl;
289 clock_t totalTimeGjkColl = 0;
290 clock_t totalTimeGjkNoColl = 0;
291 for (std::size_t i = 0; i < N; ++i) {
293 totalTimeGjkColl += results[i].timeGjk;
295 totalTimeGjkNoColl += results[i].timeGjk;
298 std::cerr <<
"Total / average time gjk: "
299 << totalTimeGjkNoColl + totalTimeGjkColl <<
", "
300 <<
FCL_REAL(totalTimeGjkNoColl + totalTimeGjkColl) /
303 std::cerr <<
"-- Collisions -------------------------" << std::endl;
304 std::cerr <<
"Total / average time gjk: " << totalTimeGjkColl <<
", "
307 std::cerr <<
"-- No collisions -------------------------" << std::endl;
308 std::cerr <<
"Total / average time gjk: " << totalTimeGjkNoColl <<
", "
310 FCL_REAL(CLOCKS_PER_SEC * (N - nCol))
320 bool expect_collision,
321 bool use_gjk_nesterov_acceleration) {
325 typedef Eigen::Matrix<FCL_REAL, 4, 1> Vec4f;
327 tf1(
Quaternion3f(Vec4f::Random().normalized()), center_distance * ray);
329 details::MinkowskiDiff shape;
332 BOOST_CHECK_EQUAL(shape.inflation[0],
sphere.radius);
333 BOOST_CHECK_EQUAL(shape.inflation[1],
sphere.radius);
335 details::GJK
gjk(2, 1e-6);
336 if (use_gjk_nesterov_acceleration)
340 if (expect_collision)
346 gjk.getClosestPoints(shape, w0, w1);
348 Vec3f w0_expected(tf0.inverse().transform(tf0.getTranslation() + ray));
349 Vec3f w1_expected(tf0.inverse().transform(
tf1.getTranslation() - ray));
376 bool use_gjk_nesterov_acceleration,
383 tf1.setTranslation(T);
385 details::MinkowskiDiff shape;
386 shape.set(&capsule, &triangle, tf0,
tf1);
388 BOOST_CHECK_EQUAL(shape.inflation[0], capsule.
radius);
389 BOOST_CHECK_EQUAL(shape.inflation[1], 0.);
391 details::GJK
gjk(10, 1e-6);
392 if (use_gjk_nesterov_acceleration)
396 if (expect_collision)
402 Vec3f guess =
gjk.getGuessFromSimplex();
403 details::GJK gjk2(3, 1e-6);
410 gjk.getClosestPoints(shape, w0, w1);
412 details::EPA epa(128, 64, 255, 1e-6);
415 epa.getClosestPoints(shape, w0, w1);