38 #define BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES 39 #include <boost/test/included/unit_test.hpp> 58 #define SET_LINE line = __LINE__ 59 #define FCL_CHECK(cond) \ 60 BOOST_CHECK_MESSAGE(cond, "from line " << line << ": " #cond) 61 #define FCL_CHECK_EQUAL(a, b) \ 62 BOOST_CHECK_MESSAGE((a) == (b), "from line " << line << ": " #a "[" << (a) \ 63 << "] != " #b "[" << (b) \ 65 #define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p)) 70 return os <<
"a_shape";
74 return os <<
"Box(" << 2 * b.
halfSide.transpose() <<
')';
79 template <
typename S1,
typename S2>
83 const Vec3f& contact_or_normal,
84 const Vec3f& expected_contact_or_normal,
85 bool check_opposite_normal,
FCL_REAL tol) {
86 std::cout <<
"Disagreement between " << comparison_type <<
" and expected_" 95 << comparison_type <<
": " << contact_or_normal.transpose()
97 <<
"expected_" << comparison_type <<
": " 98 << expected_contact_or_normal.transpose();
100 if (check_opposite_normal)
101 std::cout <<
" or " << -expected_contact_or_normal.transpose();
103 std::cout << std::endl
105 << (contact_or_normal - expected_contact_or_normal).norm()
107 <<
"tolerance: " << tol << std::endl;
110 template <
typename S1,
typename S2>
115 std::cout <<
"Disagreement between " << comparison_type <<
" and expected_" 122 <<
"depth: " << depth << std::endl
123 <<
"expected_depth: " << expected_depth << std::endl
124 <<
"difference: " << std::fabs(depth - expected_depth) << std::endl
125 <<
"tolerance: " << tol << std::endl;
128 template <
typename S1,
typename S2>
133 Vec3f* expected_normal,
bool check_opposite_normal,
135 if (expected_point) {
136 bool contact_equal =
isEqual(contact, *expected_point, tol);
140 *expected_point,
false, tol);
143 if (expected_depth) {
144 bool depth_equal = std::fabs(depth - *expected_depth) < tol;
151 if (expected_normal) {
152 bool normal_equal =
isEqual(normal, *expected_normal, tol);
154 if (!normal_equal && check_opposite_normal)
155 normal_equal =
isEqual(normal, -(*expected_normal), tol);
160 check_opposite_normal, tol);
164 template <
typename S1,
typename S2>
167 Vec3f* expected_point = NULL,
169 Vec3f* expected_normal = NULL,
170 bool check_opposite_normal =
false,
178 bool check_failed =
false;
182 collision = (
collide(&s1, tf1, &s2, tf2, request, result) > 0);
184 check_failed = check_failed || (collision != expect_collision);
188 collision = (
collide(&s1, tf1, &s2, tf2, request, result) > 0);
190 check_failed = check_failed || (collision != expect_collision);
193 BOOST_TEST_MESSAGE(
"Failure occured between " << s1 <<
" and " << s2
194 <<
" at transformations\n" 199 if (expect_collision) {
205 expected_normal, check_opposite_normal, tol);
239 Box s2(1.6, 0.6, 0.025);
243 -0.67596178682051911, 0.0668715876735793),
244 Vec3f(0.041218354748013122, 1.2022554710435607, 0.77338855025700015));
247 Quaternion3f(0.70738826916719977, 0, 0, 0.70682518110536596),
248 Vec3f(-0.29936284351096382, 0.80023864435868775, 0.71750000000000003));
253 bool res = solver.
shapeDistance(s1, tf1, s2, tf2, distance, p1, p2, normal);
254 BOOST_CHECK((res && distance > 0) || (!res && distance <= 0));
258 bool p2_in_cylinder((fabs(p2Loc[2]) <= s1.
halfLength) &&
259 (p2Loc[0] * p2Loc[0] + p2Loc[1] * p2Loc[1] <= s1.
radius));
261 bool p1_in_box = (p1Loc.array().abs() <= s2.
halfSide.array()).all();
262 std::cout <<
"p2 in cylinder = (" << p2Loc.transpose() <<
")" << std::endl;
263 std::cout <<
"p1 in box = (" << p1Loc.transpose() <<
")" << std::endl;
265 BOOST_CHECK((res && !p2_in_cylinder && !p1_in_box) ||
266 (!res && p2_in_cylinder && p1_in_box));
268 res = solver.
shapeDistance(s2, tf2, s1, tf1, distance, p2, p1, normal);
269 BOOST_CHECK((res && distance > 0) || (!res && distance <= 0));
274 p2_in_cylinder = (fabs(p2Loc[2]) <= s1.
halfLength) &&
275 (p2Loc[0] * p2Loc[0] + p2Loc[1] * p2Loc[1] <= s1.
radius);
277 p1_in_box = (p1Loc.array().abs() <= s2.
halfSide.array()).all();
279 std::cout <<
"p2 in cylinder = (" << p2Loc.transpose() <<
")" << std::endl;
280 std::cout <<
"p1 in box = (" << p1.transpose() <<
")" << std::endl;
282 BOOST_CHECK((res && !p2_in_cylinder && !p1_in_box) ||
283 (!res && p2_in_cylinder && p1_in_box));
287 Vec3f(-0.66734052046473924, 0.22219183277457269, 0.76825248755616293));
289 0.70415587451837913, -0.35175580165512249));
290 res = solver.
shapeDistance(s1, tf1, s2, tf2, distance, p1, p2, normal);
318 tf2 = Transform3f(
Vec3f(30, 0, 0));
324 tf2 = Transform3f(
Vec3f(30.01, 0, 0));
329 tf2 = transform * Transform3f(
Vec3f(30.01, 0, 0));
335 tf2 = Transform3f(
Vec3f(29.9, 0, 0));
341 tf2 = transform * Transform3f(
Vec3f(29.9, 0, 0));
361 tf2 = Transform3f(
Vec3f(-29.9, 0, 0));
367 tf2 = transform * Transform3f(
Vec3f(-29.9, 0, 0));
373 tf2 = Transform3f(
Vec3f(-30.0, 0, 0));
379 tf2 = Transform3f(
Vec3f(-30.01, 0, 0));
385 tf2 = transform * Transform3f(
Vec3f(-30.01, 0, 0));
391 return c1[2] < c2[2];
395 Box s1(100, 100, 100);
399 std::vector<Vec3f> vertices(8);
400 vertices[0] << 1, 1, 1;
401 vertices[1] << 1, 1, -1;
402 vertices[2] << 1, -1, 1;
403 vertices[3] << 1, -1, -1;
404 vertices[4] << -1, 1, 1;
405 vertices[5] << -1, 1, -1;
406 vertices[6] << -1, -1, 1;
407 vertices[7] << -1, -1, -1;
409 for (std::size_t i = 0; i < 8; ++i) {
410 vertices[i].array() *= s2.
halfSide.array();
417 Vec3f point(0., 0., 0.);
424 solver1.
shapeIntersect(s1, tf1, s2, tf2, distance,
true, &point, &normal);
428 for (std::size_t i = 0; i < 8; ++i) vertices[i] = tf2.
transform(vertices[i]);
436 FCL_CHECK(vertices[0][2] <= point[2] && point[2] < 0);
497 for (
int i = 0; i < numTests; ++i) {
545 tf2 = Transform3f(
Vec3f(22.4, 0, 0));
552 tf2 = transform * Transform3f(
Vec3f(22.4, 0, 0));
561 rotSphere << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;
565 rotBox << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;
623 tf2 = Transform3f(
Vec3f(25, 0, 0));
629 tf2 = transform * Transform3f(
Vec3f(24.999999, 0, 0));
635 tf2 = Transform3f(
Vec3f(25.1, 0, 0));
641 tf2 = transform * Transform3f(
Vec3f(25.1, 0, 0));
697 tf2 = Transform3f(
Vec3f(10.01, 0, 0));
702 tf2 = transform * Transform3f(
Vec3f(10.01, 0, 0));
855 transform, distance, c1, c2, normal);
867 transform, distance, c1, c2, normal);
877 transform, distance, c1, c2, normal);
903 transform, distance, c1, c2, normal);
918 transform, distance, c1, c2, normal);
928 if (res) BOOST_CHECK(
isEqual(normal,
Vec3f(1, 0, 0), 1e-9));
931 transform, distance, c1, c2, normal);
959 transform, distance, c1, c2, normal);
963 t[1] << -0.1, -20, 0;
967 transform, distance, c1, c2, normal);
977 transform, distance, c1, c2, normal);
1014 contact << -2.5, 0, 0;
1028 tf1 = Transform3f();
1029 tf2 = Transform3f(
Vec3f(-5, 0, 0));
1030 contact << -7.5, 0, 0;
1037 tf2 = transform * Transform3f(
Vec3f(-5, 0, 0));
1044 tf1 = Transform3f();
1045 tf2 = Transform3f(
Vec3f(-10.1, 0, 0));
1050 tf2 = transform * Transform3f(
Vec3f(-10.1, 0, 0));
1054 tf1 = Transform3f();
1055 tf2 = Transform3f(
Vec3f(10.1, 0, 0));
1056 contact << 0.05, 0, 0;
1063 tf2 = transform * Transform3f(
Vec3f(10.1, 0, 0));
1117 tf1 = Transform3f();
1118 tf2 = Transform3f(
Vec3f(-5, 0, 0));
1119 contact << -5, 0, 0;
1126 tf2 = transform * Transform3f(
Vec3f(-5, 0, 0));
1133 tf1 = Transform3f();
1134 tf2 = Transform3f(
Vec3f(-10.1, 0, 0));
1139 tf2 = transform * Transform3f(
Vec3f(-10.1, 0, 0));
1143 tf1 = Transform3f();
1144 tf2 = Transform3f(
Vec3f(10.1, 0, 0));
1149 tf2 = transform * Transform3f(
Vec3f(10.1, 0, 0));
1170 contact << -1.25, 0, 0;
1186 contact << -0.625, 0, 0;
1200 tf1 = Transform3f();
1201 tf2 = Transform3f(
Vec3f(-1.25, 0, 0));
1202 contact << -1.875, 0, 0;
1209 tf2 = transform * Transform3f(
Vec3f(-1.25, 0, 0));
1216 tf1 = Transform3f();
1217 tf2 = Transform3f(
Vec3f(2.51, 0, 0));
1218 contact << 0.005, 0, 0;
1225 tf2 = transform * Transform3f(
Vec3f(2.51, 0, 0));
1232 tf1 = Transform3f();
1233 tf2 = Transform3f(
Vec3f(-2.51, 0, 0));
1238 tf2 = transform * Transform3f(
Vec3f(-2.51, 0, 0));
1243 tf2 = Transform3f();
1280 contact << 1.25, 0, 0;
1294 tf1 = Transform3f();
1295 tf2 = Transform3f(
Vec3f(-1.25, 0, 0));
1296 contact << -1.25, 0, 0;
1303 tf2 = transform * Transform3f(
Vec3f(-1.25, 0, 0));
1310 tf1 = Transform3f();
1311 tf2 = Transform3f(
Vec3f(2.51, 0, 0));
1316 tf2 = transform * Transform3f(
Vec3f(2.51, 0, 0));
1320 tf1 = Transform3f();
1321 tf2 = Transform3f(
Vec3f(-2.51, 0, 0));
1326 tf2 = transform * Transform3f(
Vec3f(-2.51, 0, 0));
1331 tf2 = Transform3f();
1352 contact << -2.5, 0, 0;
1368 contact << -1.25, 0, 0;
1382 tf1 = Transform3f();
1383 tf2 = Transform3f(
Vec3f(-2.5, 0, 0));
1384 contact << -3.75, 0, 0;
1391 tf2 = transform * Transform3f(
Vec3f(-2.5, 0, 0));
1398 tf1 = Transform3f();
1399 tf2 = Transform3f(
Vec3f(5.1, 0, 0));
1400 contact << 0.05, 0, 0;
1407 tf2 = transform * Transform3f(
Vec3f(5.1, 0, 0));
1414 tf1 = Transform3f();
1415 tf2 = Transform3f(
Vec3f(-5.1, 0, 0));
1420 tf2 = transform * Transform3f(
Vec3f(-5.1, 0, 0));
1426 tf1 = Transform3f();
1427 tf2 = Transform3f();
1428 contact << 0, -2.5, 0;
1442 tf1 = Transform3f();
1443 tf2 = Transform3f(
Vec3f(0, 2.5, 0));
1444 contact << 0, -1.25, 0;
1451 tf2 = transform * Transform3f(
Vec3f(0, 2.5, 0));
1458 tf1 = Transform3f();
1459 tf2 = Transform3f(
Vec3f(0, -2.5, 0));
1460 contact << 0, -3.75, 0;
1467 tf2 = transform * Transform3f(
Vec3f(0, -2.5, 0));
1474 tf1 = Transform3f();
1475 tf2 = Transform3f(
Vec3f(0, 5.1, 0));
1476 contact << 0, 0.05, 0;
1483 tf2 = transform * Transform3f(
Vec3f(0, 5.1, 0));
1490 tf1 = Transform3f();
1491 tf2 = Transform3f(
Vec3f(0, -5.1, 0));
1496 tf2 = transform * Transform3f(
Vec3f(0, -5.1, 0));
1502 tf1 = Transform3f();
1503 tf2 = Transform3f();
1504 contact << 0, 0, -5;
1518 tf1 = Transform3f();
1519 tf2 = Transform3f(
Vec3f(0, 0, 2.5));
1520 contact << 0, 0, -3.75;
1527 tf2 = transform * Transform3f(
Vec3f(0, 0, 2.5));
1534 tf1 = Transform3f();
1535 tf2 = Transform3f(
Vec3f(0, 0, -2.5));
1536 contact << 0, 0, -6.25;
1543 tf2 = transform * Transform3f(
Vec3f(0, 0, -2.5));
1550 tf1 = Transform3f();
1551 tf2 = Transform3f(
Vec3f(0, 0, 10.1));
1552 contact << 0, 0, 0.05;
1559 tf2 = transform * Transform3f(
Vec3f(0, 0, 10.1));
1566 tf1 = Transform3f();
1567 tf2 = Transform3f(
Vec3f(0, 0, -10.1));
1572 tf2 = transform * Transform3f(
Vec3f(0, 0, -10.1));
1609 contact << 2.5, 0, 0;
1623 tf1 = Transform3f();
1624 tf2 = Transform3f(
Vec3f(-2.5, 0, 0));
1625 contact << -2.5, 0, 0;
1632 tf2 = transform * Transform3f(
Vec3f(-2.5, 0, 0));
1639 tf1 = Transform3f();
1640 tf2 = Transform3f(
Vec3f(5.1, 0, 0));
1645 tf2 = transform * Transform3f(
Vec3f(5.1, 0, 0));
1649 tf1 = Transform3f();
1650 tf2 = Transform3f(
Vec3f(-5.1, 0, 0));
1655 tf2 = transform * Transform3f(
Vec3f(-5.1, 0, 0));
1661 tf1 = Transform3f();
1662 tf2 = Transform3f();
1677 tf1 = Transform3f();
1678 tf2 = Transform3f(
Vec3f(0, 2.5, 0));
1679 contact << 0, 2.5, 0;
1686 tf2 = transform * Transform3f(
Vec3f(0, 2.5, 0));
1693 tf1 = Transform3f();
1694 tf2 = Transform3f(
Vec3f(0, -2.5, 0));
1695 contact << 0, -2.5, 0;
1702 tf2 = transform * Transform3f(
Vec3f(0, -2.5, 0));
1709 tf1 = Transform3f();
1710 tf2 = Transform3f(
Vec3f(0, 5.1, 0));
1715 tf2 = transform * Transform3f(
Vec3f(0, 5.1, 0));
1719 tf1 = Transform3f();
1720 tf2 = Transform3f(
Vec3f(0, -5.1, 0));
1725 tf2 = transform * Transform3f(
Vec3f(0, -5.1, 0));
1731 tf1 = Transform3f();
1732 tf2 = Transform3f();
1747 tf1 = Transform3f();
1748 tf2 = Transform3f(
Vec3f(0, 0, 2.5));
1749 contact << 0, 0, 2.5;
1756 tf2 = transform * Transform3f(
Vec3f(0, 0, 2.5));
1763 tf1 = Transform3f();
1764 tf2 = Transform3f(
Vec3f(0, 0, -2.5));
1765 contact << 0, 0, -2.5;
1772 tf2 = transform * Transform3f(
Vec3f(0, 0, -2.5));
1779 tf1 = Transform3f();
1780 tf2 = Transform3f(
Vec3f(0, 0, 10.1));
1785 tf2 = transform * Transform3f(
Vec3f(0, 0, 10.1));
1789 tf1 = Transform3f();
1790 tf2 = Transform3f(
Vec3f(0, 0, -10.1));
1795 tf2 = transform * Transform3f(
Vec3f(0, 0, -10.1));
1816 contact << -2.5, 0, 0;
1832 contact << -1.25, 0, 0;
1846 tf1 = Transform3f();
1847 tf2 = Transform3f(
Vec3f(-2.5, 0, 0));
1848 contact << -3.75, 0, 0;
1855 tf2 = transform * Transform3f(
Vec3f(-2.5, 0, 0));
1862 tf1 = Transform3f();
1863 tf2 = Transform3f(
Vec3f(5.1, 0, 0));
1864 contact << 0.05, 0, 0;
1871 tf2 = transform * Transform3f(
Vec3f(5.1, 0, 0));
1878 tf1 = Transform3f();
1879 tf2 = Transform3f(
Vec3f(-5.1, 0, 0));
1884 tf2 = transform * Transform3f(
Vec3f(-5.1, 0, 0));
1890 tf1 = Transform3f();
1891 tf2 = Transform3f();
1892 contact << 0, -2.5, 0;
1906 tf1 = Transform3f();
1907 tf2 = Transform3f(
Vec3f(0, 2.5, 0));
1908 contact << 0, -1.25, 0;
1915 tf2 = transform * Transform3f(
Vec3f(0, 2.5, 0));
1922 tf1 = Transform3f();
1923 tf2 = Transform3f(
Vec3f(0, -2.5, 0));
1924 contact << 0, -3.75, 0;
1931 tf2 = transform * Transform3f(
Vec3f(0, -2.5, 0));
1938 tf1 = Transform3f();
1939 tf2 = Transform3f(
Vec3f(0, 5.1, 0));
1940 contact << 0, 0.05, 0;
1947 tf2 = transform * Transform3f(
Vec3f(0, 5.1, 0));
1954 tf1 = Transform3f();
1955 tf2 = Transform3f(
Vec3f(0, -5.1, 0));
1960 tf2 = transform * Transform3f(
Vec3f(0, -5.1, 0));
1966 tf1 = Transform3f();
1967 tf2 = Transform3f();
1968 contact << 0, 0, -2.5;
1982 tf1 = Transform3f();
1983 tf2 = Transform3f(
Vec3f(0, 0, 2.5));
1984 contact << 0, 0, -1.25;
1991 tf2 = transform * Transform3f(
Vec3f(0, 0, 2.5));
1998 tf1 = Transform3f();
1999 tf2 = Transform3f(
Vec3f(0, 0, -2.5));
2000 contact << 0, 0, -3.75;
2007 tf2 = transform * Transform3f(
Vec3f(0, 0, -2.5));
2014 tf1 = Transform3f();
2015 tf2 = Transform3f(
Vec3f(0, 0, 5.1));
2016 contact << 0, 0, 0.05;
2023 tf2 = transform * Transform3f(
Vec3f(0, 0, 5.1));
2030 tf1 = Transform3f();
2031 tf2 = Transform3f(
Vec3f(0, 0, -5.1));
2036 tf2 = transform * Transform3f(
Vec3f(0, 0, -5.1));
2073 contact << 2.5, 0, 0;
2087 tf1 = Transform3f();
2088 tf2 = Transform3f(
Vec3f(-2.5, 0, 0));
2089 contact << -2.5, 0, 0;
2096 tf2 = transform * Transform3f(
Vec3f(-2.5, 0, 0));
2103 tf1 = Transform3f();
2104 tf2 = Transform3f(
Vec3f(5.1, 0, 0));
2109 tf2 = transform * Transform3f(
Vec3f(5.1, 0, 0));
2113 tf1 = Transform3f();
2114 tf2 = Transform3f(
Vec3f(-5.1, 0, 0));
2119 tf2 = transform * Transform3f(
Vec3f(-5.1, 0, 0));
2125 tf1 = Transform3f();
2126 tf2 = Transform3f();
2141 tf1 = Transform3f();
2142 tf2 = Transform3f(
Vec3f(0, 2.5, 0));
2143 contact << 0, 2.5, 0;
2150 tf2 = transform * Transform3f(
Vec3f(0, 2.5, 0));
2157 tf1 = Transform3f();
2158 tf2 = Transform3f(
Vec3f(0, -2.5, 0));
2159 contact << 0, -2.5, 0;
2166 tf2 = transform * Transform3f(
Vec3f(0, -2.5, 0));
2173 tf1 = Transform3f();
2174 tf2 = Transform3f(
Vec3f(0, 5.1, 0));
2179 tf2 = transform * Transform3f(
Vec3f(0, 5.1, 0));
2183 tf1 = Transform3f();
2184 tf2 = Transform3f(
Vec3f(0, -5.1, 0));
2189 tf2 = transform * Transform3f(
Vec3f(0, -5.1, 0));
2195 tf1 = Transform3f();
2196 tf2 = Transform3f();
2211 tf1 = Transform3f();
2212 tf2 = Transform3f(
Vec3f(0, 0, 2.5));
2213 contact << 0, 0, 2.5;
2220 tf2 = transform * Transform3f(
Vec3f(0, 0, 2.5));
2227 tf1 = Transform3f();
2228 tf2 = Transform3f(
Vec3f(0, 0, -2.5));
2229 contact << 0, 0, -2.5;
2236 tf2 = transform * Transform3f(
Vec3f(0, 0, -2.5));
2243 tf1 = Transform3f();
2244 tf2 = Transform3f(
Vec3f(0, 0, 10.1));
2249 tf2 = transform * Transform3f(
Vec3f(0, 0, 10.1));
2253 tf1 = Transform3f();
2254 tf2 = Transform3f(
Vec3f(0, 0, -10.1));
2259 tf2 = transform * Transform3f(
Vec3f(0, 0, -10.1));
2280 contact << -2.5, 0, -5;
2296 contact << -1.25, 0, -5;
2310 tf1 = Transform3f();
2311 tf2 = Transform3f(
Vec3f(-2.5, 0, 0));
2312 contact << -3.75, 0, -5;
2319 tf2 = transform * Transform3f(
Vec3f(-2.5, 0, 0));
2326 tf1 = Transform3f();
2327 tf2 = Transform3f(
Vec3f(5.1, 0, 0));
2328 contact << 0.05, 0, -5;
2335 tf2 = transform * Transform3f(
Vec3f(5.1, 0, 0));
2342 tf1 = Transform3f();
2343 tf2 = Transform3f(
Vec3f(-5.1, 0, 0));
2348 tf2 = transform * Transform3f(
Vec3f(-5.1, 0, 0));
2354 tf1 = Transform3f();
2355 tf2 = Transform3f();
2356 contact << 0, -2.5, -5;
2370 tf1 = Transform3f();
2371 tf2 = Transform3f(
Vec3f(0, 2.5, 0));
2372 contact << 0, -1.25, -5;
2379 tf2 = transform * Transform3f(
Vec3f(0, 2.5, 0));
2386 tf1 = Transform3f();
2387 tf2 = Transform3f(
Vec3f(0, -2.5, 0));
2388 contact << 0, -3.75, -5;
2395 tf2 = transform * Transform3f(
Vec3f(0, -2.5, 0));
2402 tf1 = Transform3f();
2403 tf2 = Transform3f(
Vec3f(0, 5.1, 0));
2404 contact << 0, 0.05, -5;
2411 tf2 = transform * Transform3f(
Vec3f(0, 5.1, 0));
2418 tf1 = Transform3f();
2419 tf2 = Transform3f(
Vec3f(0, -5.1, 0));
2424 tf2 = transform * Transform3f(
Vec3f(0, -5.1, 0));
2430 tf1 = Transform3f();
2431 tf2 = Transform3f();
2432 contact << 0, 0, -2.5;
2446 tf1 = Transform3f();
2447 tf2 = Transform3f(
Vec3f(0, 0, 2.5));
2448 contact << 0, 0, -1.25;
2455 tf2 = transform * Transform3f(
Vec3f(0, 0, 2.5));
2462 tf1 = Transform3f();
2463 tf2 = Transform3f(
Vec3f(0, 0, -2.5));
2464 contact << 0, 0, -3.75;
2471 tf2 = transform * Transform3f(
Vec3f(0, 0, -2.5));
2478 tf1 = Transform3f();
2479 tf2 = Transform3f(
Vec3f(0, 0, 5.1));
2480 contact << 0, 0, 0.05;
2487 tf2 = transform * Transform3f(
Vec3f(0, 0, 5.1));
2494 tf1 = Transform3f();
2495 tf2 = Transform3f(
Vec3f(0, 0, -5.1));
2500 tf2 = transform * Transform3f(
Vec3f(0, 0, -5.1));
2538 contact << 2.5, 0, -2.5;
2552 tf1 = Transform3f();
2553 tf2 = Transform3f(
Vec3f(-2.5, 0, 0));
2554 contact << -2.5, 0, -2.5;
2561 tf2 = transform * Transform3f(
Vec3f(-2.5, 0, 0));
2568 tf1 = Transform3f();
2569 tf2 = Transform3f(
Vec3f(5.1, 0, 0));
2574 tf2 = transform * Transform3f(
Vec3f(5.1, 0, 0));
2578 tf1 = Transform3f();
2579 tf2 = Transform3f(
Vec3f(-5.1, 0, 0));
2584 tf2 = transform * Transform3f(
Vec3f(-5.1, 0, 0));
2590 tf1 = Transform3f();
2591 tf2 = Transform3f();
2606 tf1 = Transform3f();
2607 tf2 = Transform3f(
Vec3f(0, 2.5, 0));
2608 contact << 0, 2.5, -2.5;
2615 tf2 = transform * Transform3f(
Vec3f(0, 2.5, 0));
2622 tf1 = Transform3f();
2623 tf2 = Transform3f(
Vec3f(0, -2.5, 0));
2624 contact << 0, -2.5, -2.5;
2631 tf2 = transform * Transform3f(
Vec3f(0, -2.5, 0));
2638 tf1 = Transform3f();
2639 tf2 = Transform3f(
Vec3f(0, 5.1, 0));
2644 tf2 = transform * Transform3f(
Vec3f(0, 5.1, 0));
2648 tf1 = Transform3f();
2649 tf2 = Transform3f(
Vec3f(0, -5.1, 0));
2654 tf2 = transform * Transform3f(
Vec3f(0, -5.1, 0));
2660 tf1 = Transform3f();
2661 tf2 = Transform3f();
2676 tf1 = Transform3f();
2677 tf2 = Transform3f(
Vec3f(0, 0, 2.5));
2678 contact << 0, 0, 2.5;
2685 tf2 = transform * Transform3f(
Vec3f(0, 0, 2.5));
2692 tf1 = Transform3f();
2693 tf2 = Transform3f(
Vec3f(0, 0, -2.5));
2694 contact << 0, 0, -2.5;
2701 tf2 = transform * Transform3f(
Vec3f(0, 0, -2.5));
2708 tf1 = Transform3f();
2709 tf2 = Transform3f(
Vec3f(0, 0, 10.1));
2714 tf2 = transform * Transform3f(
Vec3f(0, 0, 10.1));
2718 tf1 = Transform3f();
2719 tf2 = Transform3f(
Vec3f(0, 0, -10.1));
2724 tf2 = transform * Transform3f(
Vec3f(0, 0, -10.1));
2738 Vec3f closest_p1, closest_p2, normal;
2741 dist, closest_p1, closest_p2, normal);
2742 BOOST_CHECK(fabs(dist - 10) < 0.001);
2747 closest_p2, normal);
2748 BOOST_CHECK(fabs(dist - 0.1) < 0.001);
2753 closest_p2, normal);
2754 BOOST_CHECK(dist < 0);
2759 dist, closest_p1, closest_p2, normal);
2760 BOOST_CHECK(fabs(dist - 10) < 0.001);
2766 BOOST_CHECK(fabs(dist - 0.1) < 0.001);
2772 BOOST_CHECK(dist < 0);
2777 closest_p1, closest_p2, normal);
2779 BOOST_CHECK(fabs(dist - 10) < 0.1);
2784 closest_p1, closest_p2, normal);
2785 BOOST_CHECK(fabs(dist - 0.1) < 0.06);
2790 closest_p1, closest_p2, normal);
2791 BOOST_CHECK(dist < 0);
2795 transform, dist, closest_p1, closest_p2, normal);
2796 BOOST_CHECK(fabs(dist - 10) < 0.1);
2801 transform, dist, closest_p1, closest_p2, normal);
2802 BOOST_CHECK(fabs(dist - 0.1) < 0.1);
2807 transform, dist, closest_p1, closest_p2, normal);
2808 BOOST_CHECK(dist < 0);
2815 Vec3f closest_p1, closest_p2, normal;
2824 closest_p1, closest_p2, normal);
2825 BOOST_CHECK(dist <= 0);
2828 res = solver1.
shapeDistance(s1, transform, s2, transform, dist, closest_p1,
2829 closest_p2, normal);
2830 BOOST_CHECK(dist <= 0);
2835 closest_p2, normal);
2836 BOOST_CHECK(fabs(dist - 0.1) < 0.001);
2841 closest_p2, normal);
2842 BOOST_CHECK(fabs(dist - 10.1) < 0.001);
2847 closest_p2, normal);
2848 BOOST_CHECK(fabs(dist - 10.2) < 0.001);
2853 closest_p1, closest_p2, normal);
2854 BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001);
2859 closest_p2, normal);
2860 BOOST_CHECK(fabs(dist - 0.1) < 0.001);
2865 closest_p2, normal);
2866 BOOST_CHECK(fabs(dist - 10.1) < 0.001);
2871 closest_p2, normal);
2872 BOOST_CHECK(fabs(dist - 10.1) < 0.001);
2877 closest_p1, closest_p2, normal);
2878 BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001);
2883 closest_p1, closest_p2, normal);
2884 BOOST_CHECK(fabs(dist - 0.1) < 0.001);
2889 dist, closest_p1, closest_p2, normal);
2890 BOOST_CHECK(fabs(dist - 5) < 0.001);
2895 closest_p1, closest_p2, normal);
2896 BOOST_CHECK(fabs(dist - 5) < 0.001);
2903 Vec3f closest_p1, closest_p2, normal;
2912 for (
int i = 0; i < N + 1; ++i) {
2917 BOOST_CHECK_CLOSE(dist, (dbox - s1.
radius - s2.
halfSide(0)), 1e-6);
2920 res = solver1.
shapeDistance(s1, transform, s2, transform, dist, closest_p1,
2921 closest_p2, normal);
2924 closest_p1, closest_p2, normal);
2925 BOOST_CHECK_CLOSE(dist, (dbox - s1.
radius - s2.
halfSide(0)), 1e-6);
2930 closest_p1, closest_p2, normal);
2931 BOOST_CHECK(dist <= 0);
2934 res = solver1.
shapeDistance(s1, transform, s2, transform, dist, closest_p1,
2935 closest_p2, normal);
2936 BOOST_CHECK(dist <= 0);
2941 closest_p2, normal);
2942 BOOST_CHECK(fabs(dist - 0.1) < 0.001);
2947 closest_p1, closest_p2, normal);
2948 BOOST_CHECK(fabs(dist - 0.1) < 0.05);
2953 dist, closest_p1, closest_p2, normal);
2954 BOOST_CHECK(fabs(dist - 17.5) < 0.001);
2959 closest_p1, closest_p2, normal);
2960 BOOST_CHECK(fabs(dist - 17.5) < 0.001);
2967 Vec3f closest_p1, closest_p2, normal;
2976 closest_p1, closest_p2, normal);
2977 BOOST_CHECK(dist <= 0);
2980 res = solver1.
shapeDistance(s1, transform, s2, transform, dist, closest_p1,
2981 closest_p2, normal);
2982 BOOST_CHECK(dist <= 0);
2987 closest_p2, normal);
2988 BOOST_CHECK(fabs(dist - 0.1) < 0.001);
2993 closest_p1, closest_p2, normal);
2994 BOOST_CHECK(fabs(dist - 0.1) < 0.001);
2999 dist, closest_p1, closest_p2, normal);
3000 BOOST_CHECK(fabs(dist - 30) < 0.001);
3005 closest_p1, closest_p2, normal);
3006 BOOST_CHECK(fabs(dist - 30) < 0.001);
3013 Vec3f closest_p1, closest_p2, normal;
3022 closest_p1, closest_p2, normal);
3023 BOOST_CHECK(dist <= 0);
3026 res = solver1.
shapeDistance(s1, transform, s2, transform, dist, closest_p1,
3027 closest_p2, normal);
3028 BOOST_CHECK(dist <= 0);
3033 closest_p2, normal);
3034 BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3039 closest_p1, closest_p2, normal);
3040 BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3045 dist, closest_p1, closest_p2, normal);
3046 BOOST_CHECK(fabs(dist - 30) < 1);
3051 closest_p1, closest_p2, normal);
3052 BOOST_CHECK(fabs(dist - 30) < 1);
3059 Vec3f closest_p1, closest_p2, normal;
3068 closest_p1, closest_p2, normal);
3069 BOOST_CHECK(dist <= 0);
3072 res = solver1.
shapeDistance(s1, transform, s2, transform, dist, closest_p1,
3073 closest_p2, normal);
3074 BOOST_CHECK(dist <= 0);
3079 closest_p2, normal);
3080 BOOST_CHECK(fabs(dist - 0.1) < 0.01);
3085 closest_p1, closest_p2, normal);
3086 BOOST_CHECK(fabs(dist - 0.1) < 0.02);
3091 dist, closest_p1, closest_p2, normal);
3092 BOOST_CHECK(fabs(dist - 30) < 0.01);
3097 closest_p1, closest_p2, normal);
3098 BOOST_CHECK(fabs(dist - 30) < 0.1);
3126 tf1 = Transform3f();
3127 tf2 = Transform3f(
Vec3f(30, 0, 0));
3132 tf1 = Transform3f();
3133 tf2 = Transform3f(
Vec3f(30.01, 0, 0));
3138 tf2 = transform * Transform3f(
Vec3f(30.01, 0, 0));
3143 tf1 = Transform3f();
3144 tf2 = Transform3f(
Vec3f(29.9, 0, 0));
3150 tf2 = transform * Transform3f(
Vec3f(29.9, 0, 0));
3155 tf1 = Transform3f();
3156 tf2 = Transform3f();
3169 tf1 = Transform3f();
3170 tf2 = Transform3f(
Vec3f(-29.9, 0, 0));
3176 tf2 = transform * Transform3f(
Vec3f(-29.9, 0, 0));
3181 tf1 = Transform3f();
3182 tf2 = Transform3f(
Vec3f(-30.0, 0, 0));
3187 tf1 = Transform3f();
3188 tf2 = Transform3f(
Vec3f(-30.01, 0, 0));
3194 tf2 = transform * Transform3f(
Vec3f(-30.01, 0, 0));
3244 tf1 = Transform3f();
3245 tf2 = Transform3f(q);
3251 tf2 = transform * Transform3f(q);
3290 s1, tf1, s2, tf2,
true, NULL, NULL, &normal,
false,
3298 tf1 = Transform3f();
3299 tf2 = Transform3f(
Vec3f(22.4, 0, 0));
3303 s1, tf1, s2, tf2,
true, NULL, NULL, &normal,
false,
3307 tf2 = transform * Transform3f(
Vec3f(22.4, 0, 0));
3355 tf1 = Transform3f();
3356 tf2 = Transform3f(
Vec3f(25, 0, 0));
3362 tf2 = transform * Transform3f(
Vec3f(25.1, 0, 0));
3412 tf1 = Transform3f();
3413 tf2 = Transform3f(
Vec3f(10, 0, 0));
3420 tf2 = transform * Transform3f(
Vec3f(10.01, 0, 0));
3462 tf1 = transform *
tf1;
3463 tf2 = transform *
tf2;
3479 tf1 = Transform3f();
3480 tf2 = Transform3f(
Vec3f(0, 0, 9.9));
3487 tf2 = transform * Transform3f(
Vec3f(0, 0, 9.9));
3532 tf1 = Transform3f();
3533 tf2 = Transform3f(
Vec3f(10, 0, 0));
3538 tf2 = transform * Transform3f(
Vec3f(10, 0, 0));
3542 tf1 = Transform3f();
3543 tf2 = Transform3f(
Vec3f(0, 0, 9.9));
3550 tf2 = transform * Transform3f(
Vec3f(0, 0, 9.9));
3556 tf1 = Transform3f();
3557 tf2 = Transform3f(
Vec3f(0, 0, 10));
3564 tf2 = transform * Transform3f(
Vec3f(0, 0, 10.1));
3590 transform, distance, c1, c2, normal);
3594 t[1] << 9.9, -20, 0;
3602 transform, distance, c1, c2, normal);
3612 transform, distance, c1, c2, normal);
3638 transform, distance, c1, c2, normal);
3650 transform, distance, c1, c2, normal);
3660 transform, distance, c1, c2, normal);
3686 transform, distance, c1, c2, normal);
3690 t[1] << -0.1, -20, 0;
3691 t[2] << -0.1, 20, 0;
3698 transform, distance, c1, c2, normal);
3708 transform, distance, c1, c2, normal);
3716 Vec3f closest_p1, closest_p2, normal;
3726 dist, closest_p1, closest_p2, normal);
3727 BOOST_CHECK(fabs(dist - 10) < 0.001);
3732 closest_p2, normal);
3733 BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3738 closest_p2, normal);
3739 BOOST_CHECK(dist <= 0);
3744 dist, closest_p1, closest_p2, normal);
3745 BOOST_CHECK(fabs(dist - 10) < 0.001);
3751 BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3757 BOOST_CHECK(dist <= 0);
3762 closest_p1, closest_p2, normal);
3763 BOOST_CHECK(fabs(dist - 10) < 0.001);
3768 closest_p1, closest_p2, normal);
3769 BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3774 closest_p1, closest_p2, normal);
3775 BOOST_CHECK(dist <= 0);
3779 transform, dist, closest_p1, closest_p2, normal);
3780 BOOST_CHECK(fabs(dist - 10) < 0.001);
3785 transform, dist, closest_p1, closest_p2, normal);
3786 BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3791 transform, dist, closest_p1, closest_p2, normal);
3792 BOOST_CHECK(dist <= 0);
3799 Vec3f closest_p1, closest_p2, normal;
3808 closest_p1, closest_p2, normal);
3809 BOOST_CHECK(dist <= 0);
3812 res = solver2.
shapeDistance(s1, transform, s2, transform, dist, closest_p1,
3813 closest_p2, normal);
3814 BOOST_CHECK(dist <= 0);
3819 closest_p2, normal);
3820 BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3825 closest_p1, closest_p2, normal);
3826 BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3831 dist, closest_p1, closest_p2, normal);
3832 BOOST_CHECK(fabs(dist - 5) < 0.001);
3837 closest_p1, closest_p2, normal);
3838 BOOST_CHECK(fabs(dist - 5) < 0.001);
3845 Vec3f closest_p1, closest_p2, normal;
3854 closest_p1, closest_p2, normal);
3855 BOOST_CHECK(dist <= 0);
3858 res = solver2.
shapeDistance(s1, transform, s2, transform, dist, closest_p1,
3859 closest_p2, normal);
3860 BOOST_CHECK(dist <= 0);
3865 closest_p2, normal);
3866 BOOST_CHECK(fabs(dist - 0.1) < 0.01);
3871 closest_p1, closest_p2, normal);
3872 BOOST_CHECK(fabs(dist - 0.1) < 0.01);
3877 dist, closest_p1, closest_p2, normal);
3878 BOOST_CHECK(fabs(dist - 17.5) < 0.001);
3883 closest_p1, closest_p2, normal);
3884 BOOST_CHECK(fabs(dist - 17.5) < 0.001);
3891 Vec3f closest_p1, closest_p2, normal;
3900 closest_p1, closest_p2, normal);
3901 BOOST_CHECK(dist <= 0);
3904 res = solver2.
shapeDistance(s1, transform, s2, transform, dist, closest_p1,
3905 closest_p2, normal);
3906 BOOST_CHECK(dist <= 0);
3911 closest_p2, normal);
3912 BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3917 closest_p1, closest_p2, normal);
3918 BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3923 dist, closest_p1, closest_p2, normal);
3924 BOOST_CHECK(fabs(dist - 30) < 0.001);
3929 closest_p1, closest_p2, normal);
3930 BOOST_CHECK(fabs(dist - 30) < 0.001);
3937 Vec3f closest_p1, closest_p2, normal;
3946 closest_p1, closest_p2, normal);
3947 BOOST_CHECK(dist <= 0);
3950 res = solver2.
shapeDistance(s1, transform, s2, transform, dist, closest_p1,
3951 closest_p2, normal);
3952 BOOST_CHECK(dist <= 0);
3957 closest_p2, normal);
3958 BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3963 closest_p1, closest_p2, normal);
3964 BOOST_CHECK(fabs(dist - 0.1) < 0.001);
3969 dist, closest_p1, closest_p2, normal);
3970 BOOST_CHECK(fabs(dist - 30) < 0.001);
3975 closest_p1, closest_p2, normal);
3976 BOOST_CHECK(fabs(dist - 30) < 0.001);
3980 template <
typename S1,
typename S2>
3992 Vec3f normalA, normalB;
3997 const double tol = 1e-6;
3999 resA = solver1.
shapeDistance(s1, tf1, s2, tf2, distA, p1A, p2A, normalA);
4000 resB = solver1.
shapeDistance(s2, tf2, s1, tf1, distB, p1B, p2B, normalB);
4004 BOOST_CHECK_CLOSE(distA, distB, tol);
4007 BOOST_CHECK(
isEqual(p2A, p1B, tol));
4009 resA = solver2.
shapeDistance(s1, tf1, s2, tf2, distA, p1A, p2A, normalA);
4010 resB = solver2.
shapeDistance(s2, tf2, s1, tf1, distB, p1B, p2B, normalB);
4014 BOOST_CHECK_CLOSE(distA, distB, tol);
4015 BOOST_CHECK(
isEqual(p1A, p2B, tol));
4016 BOOST_CHECK(
isEqual(p2A, p1B, tol));
Vec3f halfSide
box side half-length
request to the distance computation
BOOST_AUTO_TEST_CASE(box_to_bvh)
FCL_REAL halfLength
Half Length along z axis.
Cylinder along Z axis. The cylinder is defined at its centroid.
Vec3f nearest_points[2]
nearest points
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; Poi...
size_t numContacts() const
number of contacts found
bool compareContactPoints(const Vec3f &c1, const Vec3f &c2)
void compareContact(const S1 &s1, const Transform3f &tf1, const S2 &s2, const Transform3f &tf2, const Vec3f &contact, Vec3f *expected_point, FCL_REAL depth, FCL_REAL *expected_depth, const Vec3f &normal, Vec3f *expected_normal, bool check_opposite_normal, FCL_REAL tol)
void printComparisonError(const std::string &comparison_type, const S1 &s1, const Transform3f &tf1, const S2 &s2, const Transform3f &tf2, const Vec3f &contact_or_normal, const Vec3f &expected_contact_or_normal, bool check_opposite_normal, FCL_REAL tol)
#define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision)
#define FCL_CHECK_EQUAL(a, b)
Base class for all basic geometric shapes.
void testBoxBoxContactPoints(const Matrix3f &R)
request to the collision algorithm
void clear()
clear the results obtained
HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
FCL_REAL radius
Radius of the cone.
bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol)
FCL_REAL min_distance
minimum distance between two objects. if two objects are in collision, min_distance <= 0...
Center at zero point, axis aligned box.
Vec3f normal
In case both objects are in collision, store the normal.
static std::ostream & operator<<(std::ostream &o, const Quaternion3f &q)
void testReversibleShapeDistance(const S1 &s1, const S2 &s2, FCL_REAL distance)
std::string getNodeTypeName(NODE_TYPE node_type)
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Cone The base of the cone is at and the top is at .
FCL_REAL radius
Radius of the sphere.
FCL_REAL radius
Radius of the cylinder.
bool shapeDistance(const S1 &s1, const Transform3f &tf1, const S2 &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal) const
distance computation between two shapes
Eigen::Quaternion< FCL_REAL > Quaternion3f
Center at zero point sphere.
Capsule It is where is the distance between the point x and the capsule segment AB...
Eigen::AngleAxis< FCL_REAL > AngleAxis
bool shapeTriangleInteraction(const S &s, const Transform3f &tf1, const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal) const
intersection checking between one shape and a triangle with transformation
HPP_FCL_DLLAPI std::size_t collide(const CollisionObject *o1, const CollisionObject *o2, const CollisionRequest &request, CollisionResult &result)
Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function performs the collision between them. Return value is the number of contacts generated between the two objects.
bool shapeIntersect(const S1 &s1, const Transform3f &tf1, const S2 &s2, const Transform3f &tf2, FCL_REAL &distance_lower_bound, bool enable_penetration, Vec3f *contact_points, Vec3f *normal) const
intersection checking between two shapes
const Contact & getContact(size_t i) const
get the i-th contact calculated
#define BOOST_CHECK_FALSE(p)
bool enable_contact
whether the contact information (normal, penetration depth and contact position) will return ...
FCL_REAL halfLength
Half Length along z axis.
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
FCL_REAL gjk_tolerance
the threshold used in GJK to stop iteration
HPP_FCL_DLLAPI Halfspace transform(const Halfspace &a, const Transform3f &tf)
void generateBVHModel(BVHModel< BV > &model, const Box &shape, const Transform3f &pose)
Generate BVH model from box.
bool isEqual(const Eigen::MatrixBase< Derived > &lhs, const Eigen::MatrixBase< OtherDerived > &rhs, const FCL_REAL tol=std::numeric_limits< FCL_REAL >::epsilon() *100)
FCL_REAL epa_tolerance
the threshold used in EPA to stop iteration
void generateRandomTransform(FCL_REAL extents[6], Transform3f &transform)
Generate one random transform whose translation is constrained by extents and rotation without constr...
void testShapeIntersection(const S1 &s1, const Transform3f &tf1, const S2 &s2, const Transform3f &tf2, bool expect_collision, Vec3f *expected_point=NULL, FCL_REAL *expected_depth=NULL, Vec3f *expected_normal=NULL, bool check_opposite_normal=false, FCL_REAL tol=1e-9)