39 #include <gtest/gtest.h>
40 #include <Eigen/Dense>
103 template <
typename S>
113 const Real h_c = 0.0025;
114 const Real
r_s = 0.0015;
116 auto sphere_geometry = std::make_shared<fcl::Sphere<S>>(
r_s);
117 auto cylinder_geometry = std::make_shared<fcl::Cylinder<S>>(
r_c, h_c);
127 const Real target_depth =
r_s * 0.5;
129 const Real sz = h_c / 2 +
r_s - target_depth;
130 const Real pz = h_c / 2 - target_depth / 2;
159 const std::vector<Real> r_values{0, eps,
r_c / 2,
r_c -
r_s};
161 const std::vector<Real> theta_values{0, pi/2, pi, 3 * pi / 4};
163 for (
const Real
r : r_values) {
164 for (
const Real theta : theta_values ) {
167 const Real angle = theta + pi / 7;
168 const Real sx = cos(angle) *
r;
169 const Real sy = sin(angle) *
r;
171 X_WS.translation() << sx, sy, sz;
174 auto evaluate_collision = [&collision_request, &X_WS](
180 std::size_t contact_count =
181 fcl::collide(s1, s2, collision_request, collision_result);
185 std::vector<fcl::Contact<S>> contacts;
191 <<
"Sphere at: " << X_WS.translation().transpose();
196 <<
"Sphere at: " << X_WS.translation().transpose();
199 <<
"Sphere at: " << X_WS.translation().transpose();
201 EXPECT_TRUE(
false) <<
"No contacts reported for sphere at: "
202 << X_WS.translation().transpose();
215 GTEST_TEST(FCL_SPHERE_CYLINDER, LargCylinderSmallSphere_ccd) {
226 int main(
int argc,
char* argv[]) {
227 ::testing::InitGoogleTest(&argc, argv);
228 return RUN_ALL_TESTS();