48 #include <gtest/gtest.h>
58 using std::make_shared;
60 using std::shared_ptr;
86 Convex<S> MakeTetrahedron(
const Transform3<S>& X_TG) {
87 auto vertices = make_shared<vector<Vector3<S>>>();
88 vertices->emplace_back(X_TG * Vector3<S>{0, 0, 0});
89 vertices->emplace_back(X_TG * Vector3<S>{1, 0, 0});
90 vertices->emplace_back(X_TG * Vector3<S>{0, 1, 0});
91 vertices->emplace_back(X_TG * Vector3<S>{0, 0, 1});
93 auto faces = make_shared<vector<int>>();
94 auto add_triangle = [&faces](
int v0,
int v1,
int v2) {
103 add_triangle(0, 2, 1);
104 add_triangle(0, 1, 3);
105 add_triangle(0, 3, 2);
106 add_triangle(2, 3, 1);
108 return {vertices, 4, faces};
115 template <
typename S>
116 struct Configuration {
119 Configuration(
const string& name_in,
const Transform3<S>& X_CT_in,
120 const Transform3<S>& X_CH_in)
121 : Configuration(name_in, X_CT_in, X_CH_in, false) {}
124 Configuration(
const string& name_in,
const Transform3<S>& X_CT_in,
125 const Transform3<S>& X_CH_in, S depth,
126 const Vector3<S>& normal_C,
const Vector3<S> pos_C)
127 : Configuration(name_in, X_CT_in, X_CH_in, true, depth, normal_C, pos_C) {
131 Configuration(
const string& label_in,
const Transform3<S>& X_CT_in,
132 const Transform3<S>& X_CH_in,
bool expected, S depth = S(0),
133 const Vector3<S>& normal_C = Vector3<S>::Zero(),
134 const Vector3<S> pos_C = Vector3<S>::Zero())
158 template <
typename S>
159 Transform3<S> MakeTransform(
const AngleAxis<S>& angle_axis,
160 const Vector3<S>& origin) {
161 Transform3<S>
transform = Transform3<S>::Identity();
162 transform.linear() = angle_axis.matrix();
167 template <
typename S>
168 aligned_vector<Configuration<S>> GetConfigurations() {
169 const Transform3<S> I = Transform3<S>::Identity();
172 aligned_vector<Configuration<S>> configurations;
176 configurations.push_back(Configuration<S>{
177 "simple non-colliding",
178 MakeTransform(AngleAxis<S>::Identity(), Vector3<S>{0, 0, S(0.1)}), I});
182 const AngleAxis<S> R_CT_point_down{kPi, Vector3<S>::UnitX()};
183 configurations.push_back(Configuration<S>{
184 "simple penetration",
185 MakeTransform(R_CT_point_down, Vector3<S>{0, 0, 0.5}), I, S(0.5),
186 Vector3<S>{0, 0, -1}, Vector3<S>{0, 0, S(-0.25)}});
192 const Transform3<S>
X_CH =
193 MakeTransform(AngleAxis<S>{kPi / 5, Vector3<S>{1, 2, 3}.normalized()},
194 Vector3<S>{S(0.25), S(0.5), S(0.75)});
198 const AngleAxis<S> R_HT_point_down = R_CT_point_down;
200 const Transform3<S> X_HT_separated =
201 MakeTransform(R_HT_point_down, Vector3<S>{0, 0, S(1.01)});
202 configurations.push_back(Configuration<S>{
203 "non-trivial half space, non-colliding",
X_CH * X_HT_separated,
X_CH});
208 const Transform3<S> X_HT_colliding =
209 MakeTransform(R_HT_point_down, Vector3<S>{0, 0, S(0.5)});
210 const Transform3<S> X_CT_colliding =
X_CH * X_HT_colliding;
211 const Vector3<S> Hz_C =
X_CH.linear().col(2);
214 const Vector3<S> p_TV3 = Vector3<S>::UnitZ();
215 const Vector3<S> p_CV3 = X_CT_colliding * p_TV3;
217 configurations.push_back(Configuration<S>{
"non-trivial half space, colliding",
218 X_CT_colliding,
X_CH, depth, -Hz_C,
219 p_CV3 + Hz_C * S(0.5) * depth});
221 return configurations;
226 template <
typename S>
227 void TestCollision(
const Convex<S>& convex_T,
const Transform3<S>& X_WT,
228 const Halfspace<S>& half_space_H,
const Transform3<S>& X_WH,
229 const Configuration<S>& config,
const Transform3<S>& X_WC,
230 S eps,
const string&
label) {
231 vector<ContactPoint<S>> results_W;
235 if (config.expected_colliding) {
237 const ContactPoint<S>& point_W = results_W[0];
238 EXPECT_NEAR(point_W.penetration_depth, config.expected_depth, eps) <<
label;
242 X_WC.linear() * config.expected_normal_C, eps))
256 template <
typename S>
257 void EvalCollisionForTestConfiguration(
const Configuration<S>& config, S eps) {
258 aligned_vector<Transform3<S>> X_WCs{};
259 X_WCs.emplace_back(Transform3<S>::Identity());
262 Vector3<S>{1, -2, S(-1.3)}.normalized()},
263 Vector3<S>{S(-0.25), S(0.5), S(-0.75)}));
265 for (
const auto& X_WC : X_WCs) {
272 const Convex<S> convex_C = MakeTetrahedron<S>(config.X_CT);
273 const Vector3<S> Hz_C = config.X_CH.linear().col(2);
274 const S d = Hz_C.dot(config.X_CH.translation());
275 const Halfspace<S> half_space_C(Hz_C, d);
279 bool colliding = convexHalfspaceIntersect<S>(convex_C, X_WC, half_space_C,
281 EXPECT_EQ(colliding, config.expected_colliding) << config.label;
287 TestCollision(convex_C, X_WC, half_space_C, X_WC, config, X_WC, eps,
288 config.label +
", X_CH = X_CT = I");
292 const Convex<S> convex_T = MakeTetrahedron<S>(Transform3<S>::Identity());
293 const Halfspace<S> half_space_H(Vector3<S>{0, 0, 1}, 0);
294 TestCollision(convex_T, X_WC * config.X_CT, half_space_H,
295 X_WC * config.X_CH, config, X_WC, eps,
296 config.label +
", X_CH != X_CT != I");
301 template <
typename S>
302 void QueryCollision(
const aligned_vector<Configuration<S>>& configs) {
306 for (
const auto& config : configs) {
307 EvalCollisionForTestConfiguration(config, eps);
311 GTEST_TEST(HalfspaceConvexPrimitiveTest, CollisionTests) {
312 QueryCollision<float>(GetConfigurations<float>());
313 QueryCollision<double>(GetConfigurations<double>());
321 int main(
int argc,
char* argv[]) {
322 ::testing::InitGoogleTest(&argc, argv);
323 return RUN_ALL_TESTS();