38 #define BOOST_TEST_MODULE FCL_CAPSULE_CAPSULE
39 #include <boost/test/included/unit_test.hpp>
41 #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps))
57 const double radius = 1.;
74 for (
int i = 0; i < num_tests; ++i) {
75 Eigen::Vector3d
p1 = Eigen::Vector3d::Random() * (2. * radius);
76 Eigen::Vector3d p2 = Eigen::Vector3d::Random() * (2. * radius);
78 Eigen::Matrix3d rot1 =
79 Eigen::Quaterniond(Eigen::Vector4d::Random().normalized())
81 Eigen::Matrix3d rot2 =
82 Eigen::Quaterniond(Eigen::Vector4d::Random().normalized())
85 tf1.setTranslation(
p1);
86 tf1.setRotation(rot1);
87 tf2.setTranslation(p2);
88 tf2.setRotation(rot2);
100 size_t sphere_num_collisions =
collide(
101 &sphere_o1, &sphere_o2, collisionRequest, sphere_collisionResult);
102 size_t capsule_num_collisions =
collide(
103 &capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult);
105 BOOST_CHECK_EQUAL(sphere_num_collisions, capsule_num_collisions);
106 if (sphere_num_collisions == 0 && capsule_num_collisions == 0)
113 const double radius = 0.01;
114 const double length = 0.2;
127 Eigen::Vector3d
p1 = Eigen::Vector3d::Zero();
128 Eigen::Vector3d p2_no_collision =
129 Eigen::Vector3d(0., 0.,
130 2 * (length / 2. + radius) +
133 for (
int i = 0; i < num_tests; ++i) {
134 Eigen::Matrix3d rot =
135 Eigen::Quaterniond(Eigen::Vector4d::Random().normalized())
138 tf1.setTranslation(
p1);
139 tf1.setRotation(rot);
140 tf2.setTranslation(p2_no_collision);
141 tf2.setRotation(rot);
150 size_t capsule_num_collisions =
collide(
151 &capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult);
153 BOOST_CHECK(capsule_num_collisions == 0);
156 Eigen::Vector3d p2_with_collision =
157 Eigen::Vector3d(0., 0., std::min(length / 2., radius) * (1. - 1e-2));
158 for (
int i = 0; i < num_tests; ++i) {
159 Eigen::Matrix3d rot =
160 Eigen::Quaterniond(Eigen::Vector4d::Random().normalized())
163 tf1.setTranslation(
p1);
164 tf1.setRotation(rot);
165 tf2.setTranslation(p2_with_collision);
166 tf2.setRotation(rot);
175 size_t capsule_num_collisions =
collide(
176 &capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult);
178 BOOST_CHECK(capsule_num_collisions > 0);
181 p2_no_collision = Eigen::Vector3d(0., 0., 2 * (length / 2. + radius) + 1e-3);
183 Transform3f geom1_placement(Eigen::Matrix3d::Identity(),
184 Eigen::Vector3d::Zero());
185 Transform3f geom2_placement(Eigen::Matrix3d::Identity(), p2_no_collision);
187 for (
int i = 0; i < num_tests; ++i) {
188 Eigen::Matrix3d rot =
189 Eigen::Quaterniond(Eigen::Vector4d::Random().normalized())
191 Eigen::Vector3d trans = Eigen::Vector3d::Random();
204 size_t capsule_num_collisions =
collide(
205 &capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult);
207 BOOST_CHECK(capsule_num_collisions == 0);
212 p2_with_collision = Eigen::Vector3d(0., 0., 0.01);
215 for (
int i = 0; i < num_tests; ++i) {
216 Eigen::Matrix3d rot =
217 Eigen::Quaterniond(Eigen::Vector4d::Random().normalized())
219 Eigen::Vector3d trans = Eigen::Vector3d::Random();
232 size_t capsule_num_collisions =
collide(
233 &capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult);
235 BOOST_CHECK(capsule_num_collisions > 0);
253 distance(&o1, &o2, distanceRequest, distanceResult);
255 std::cerr <<
"Applied translation on two capsules";
256 std::cerr <<
" T1 = " <<
tf1.getTranslation()
257 <<
", T2 = " <<
tf2.getTranslation() << std::endl;
258 std::cerr <<
"Closest points: p1 = " << distanceResult.
nearest_points[0]
260 <<
", distance = " << distanceResult.
min_distance << std::endl;
262 BOOST_CHECK_CLOSE(distanceResult.
min_distance, 10.1, 1e-6);
279 distance(&o1, &o2, distanceRequest, distanceResult);
281 std::cerr <<
"Applied translation on two capsules";
282 std::cerr <<
" T1 = " <<
tf1.getTranslation()
283 <<
", T2 = " <<
tf2.getTranslation() << std::endl;
284 std::cerr <<
"Closest points: p1 = " << distanceResult.
nearest_points[0]
286 <<
", distance = " << distanceResult.
min_distance << std::endl;
289 BOOST_CHECK_CLOSE(distanceResult.
min_distance, expected, 1e-6);
306 distance(&o1, &o2, distanceRequest, distanceResult);
308 std::cerr <<
"Applied translation on two capsules";
309 std::cerr <<
" T1 = " <<
tf1.getTranslation()
310 <<
", T2 = " <<
tf2.getTranslation() << std::endl;
311 std::cerr <<
"Closest points: p1 = " << distanceResult.
nearest_points[0]
313 <<
", distance = " << distanceResult.
min_distance << std::endl;
315 BOOST_CHECK_CLOSE(distanceResult.
min_distance, 0.1, 1e-6);
332 distance(&o1, &o2, distanceRequest, distanceResult);
334 std::cerr <<
"Applied rotation and translation on two capsules" << std::endl;
335 std::cerr <<
"R1 = " <<
tf1.getRotation() << std::endl
336 <<
"T1 = " <<
tf1.getTranslation().transpose() << std::endl
337 <<
"R2 = " <<
tf2.getRotation() << std::endl
338 <<
"T2 = " <<
tf2.getTranslation().transpose() << std::endl;
339 std::cerr <<
"Closest points:" << std::endl
344 <<
"distance = " << distanceResult.
min_distance << std::endl;
349 BOOST_CHECK_CLOSE(distanceResult.
min_distance, 10.1, 1e-6);
352 BOOST_CHECK_CLOSE(
p1[2], 10, 1e-4);
355 BOOST_CHECK_CLOSE(p2[2], 20.1, 1e-4);