37 #define BOOST_TEST_MODULE FCL_NESTEROV_GJK
38 #include <boost/test/included/unit_test.hpp>
40 #include <Eigen/Geometry>
88 mink_diff1.
set(&ellipsoid, &ellipsoid);
92 mink_diff2.
set(&ellipsoid, &
box);
96 mink_diff3.
set(&cvx, &cvx);
102 unsigned int max_iterations = 128;
104 GJK gjk(max_iterations, tolerance);
105 GJK gjk_nesterov(max_iterations, tolerance);
114 std::vector<Transform3f> transforms;
121 init_support_guess.setZero();
123 for (
size_t i = 0; i < n; ++i) {
124 mink_diff.
set(&shape0, &shape1, identity, transforms[i]);
127 GJK::Status res_gjk_1 =
128 gjk.evaluate(mink_diff, init_guess, init_support_guess);
130 GJK::Status res_gjk_2 =
131 gjk.evaluate(mink_diff, init_guess, init_support_guess);
132 BOOST_CHECK(res_gjk_1 == res_gjk_2);
135 GJK::Status res_nesterov_gjk_1 =
136 gjk_nesterov.
evaluate(mink_diff, init_guess, init_support_guess);
137 Vec3f ray_nesterov = gjk_nesterov.
ray;
138 GJK::Status res_nesterov_gjk_2 =
139 gjk_nesterov.
evaluate(mink_diff, init_guess, init_support_guess);
140 BOOST_CHECK(res_nesterov_gjk_1 == res_nesterov_gjk_2);
145 BOOST_CHECK(res_nesterov_gjk_1 == res_gjk_1);
146 BOOST_CHECK_SMALL(fabs(ray_gjk.norm() - ray_nesterov.norm()), 1e-4);
150 BOOST_CHECK(
gjk.getIterations() < max_iterations);
178 Box box0 =
Box(0.1, 0.2, 0.3);
179 Box box1 =
Box(1.1, 1.2, 1.3);
223 Box box0 =
Box(0.1, 0.2, 0.3);
224 Box box1 =
Box(1.1, 1.2, 1.3);
231 Box box0 =
Box(0.1, 0.2, 0.3);
232 Box box1 =
Box(1.1, 1.2, 1.3);