43 #include <gtest/gtest.h>
44 #include <Eigen/Dense>
60 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
94 Transform3<S>{AngleAxis<S>(pi, Vector3<S>::UnitX())};
96 Transform3<S>{AngleAxis<S>(pi / 2, Vector3<S>::UnitX())};
98 Transform3<S>{AngleAxis<S>(3 * pi / 2, Vector3<S>::UnitX())};
100 Transform3<S>{AngleAxis<S>(pi / 2, Vector3<S>::UnitY())};
102 Transform3<S>{AngleAxis<S>(3 * pi / 2, Vector3<S>::UnitY())};
125 const std::string& origin_name) {
129 for (
const auto& reorient_pair :
iso_poses_) {
130 const std::string& top_face = reorient_pair.first;
146 origin_name +
" (1, 2) - " + top_face);
156 origin_name +
" (2, 1) - " + top_face);
186 const std::string& origin_name) {
187 using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometry<S>>;
188 CollisionGeometryPtr_t box_geometry_A(
new fcl::Box<S>(box_spec_A.
size));
189 CollisionGeometryPtr_t box_geometry_B(
new fcl::Box<S>(box_spec_B.
size));
198 fcl::collide(&box_A, &box_B, collisionRequest, collisionResult);
200 std::vector<fcl::Contact<S>> contacts;
202 GTEST_ASSERT_EQ(contacts.size(), 1u) << origin_name;
210 << origin_name <<
":\n\texpected: "
211 << expected_contact.
normal.transpose()
212 <<
"\n\tcontact.normal: " << contact.
normal.transpose();
213 contact_pos_test(contact.
pos, test_tolerance, origin_name);
218 map<string, fcl::Transform3<S>, std::less<string>,
219 Eigen::aligned_allocator<std::pair<const string, fcl::Transform3<S>>>>
258 template <
typename S>
277 const std::string& origin_name) {
278 const double expected_pos_z = -size_1 * std::sqrt(2) / 4;
281 EXPECT_LE(pos(1), 0.5) << origin_name;
282 EXPECT_GE(pos(1), -0.5) << origin_name;
287 contact_pos_test,
"test_colliion_box_box_all_contacts");
296 GTEST_TEST(FCL_BOX_BOX, collision_box_box_all_contacts_indep)
342 template <
typename S>
348 const S tilt = pi / 8;
356 box_2.
X_FB.translation() << 0, 0, -0.75;
368 const S expected_y{-sqrt(2) / 2 * sin(pi / 4 - tilt)};
369 const S expected_x{sqrt(2) / 2 + expected_y +
373 EXPECT_GE(pos(0), -expected_x);
374 EXPECT_LE(pos(0), expected_x);
379 contact_pos_test,
"test_collision_box_box_cull_contacts");
388 GTEST_TEST(FCL_BOX_BOX, collision_box_box_cull_contacts_indep)
422 template <
typename S>
433 box_1.X_FB.translation() = dir * (size * sqrt(2) -
expected_depth);
445 <<
"\n\tpos: " << pos.transpose();
450 contact_pos_test,
"test_collision_box_box_edge_contact");
459 GTEST_TEST(FCL_BOX_BOX, collision_box_box_edge_contact_indep)
466 int main(
int argc,
char* argv[])
468 ::testing::InitGoogleTest(&argc, argv);
469 return RUN_ALL_TESTS();