test_half_space_convex.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2020. Toyota Research Institute
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of CNRS-LAAS and AIST nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
37 // Tests the Halfspace-Convex primitive collision test.
38 
39 // TODO(SeanCurtis-TRI): Currently, there are no half space-X distance primitive
40 // implementations.
41 
43 
44 #include <memory>
45 #include <string>
46 #include <vector>
47 
48 #include <gtest/gtest.h>
49 
50 #include "eigen_matrix_compare.h"
53 
54 namespace fcl {
55 namespace detail {
56 namespace {
57 
58 using std::make_shared;
59 using std::move;
60 using std::shared_ptr;
61 using std::string;
62 using std::vector;
63 
64 /*
65  Creates a simple tetrahedron as a Convex geometry.
66  +z
67 
68 
69  o v3
70 
71 
72  │ v0 v2
73  o─────────o───── +y
74 
75 
76 
77  o v1
78 
79  +x
80 
81  The tet is defined in geometry frame G, shown above with vertex positions
82  marked. The tet is then posed in frame T, and the final convex mesh is
83  produced with its vertices measured and expressed in frame T.
84  */
85 template <typename S>
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});
92 
93  auto faces = make_shared<vector<int>>();
94  auto add_triangle = [&faces](int v0, int v1, int v2) {
95  faces->push_back(3); // number of vertices in the face.
96  faces->push_back(v0);
97  faces->push_back(v1);
98  faces->push_back(v2);
99  };
100 
101  // The ordering of the vertex indices in each triangle is important but the
102  // ordering of the triangles isn't.
103  add_triangle(0, 2, 1); // face on xy plane.
104  add_triangle(0, 1, 3); // face on xz plane.
105  add_triangle(0, 3, 2); // face on yz plane.
106  add_triangle(2, 3, 1); // diagonal face.
107 
108  return {vertices, 4, faces};
109 }
110 
111 /* Specifies the data necessary for a single test and the expected results.
112  The Convex tetrahedron is defined in frame T, the Halfspace is defined in frame
113  H, and both are posed relative to the configuration frame C with X_CT and X_CH,
114  respectively. */
115 template <typename S>
116 struct Configuration {
117  /* Convenience constructor for creating an expected _non-colliding_
118  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) {}
122 
123  /* Constructor for expected _colliding_ configuration. */
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) {
128  }
129 
130  /* Full constructor. */
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())
135  : label(label_in),
136  X_CT(X_CT_in),
137  X_CH(X_CH_in),
138  expected_colliding(expected),
139  expected_depth(depth),
140  expected_normal_C(normal_C),
141  expected_pos_C(pos_C) {}
142 
143  string label;
144  // Pose of the tetrahedron in the configuration frame C.
145  Transform3<S> X_CT;
146  // Pose of the half space in the configuration frame C.
147  Transform3<S> X_CH;
148 
149  // Expected results; if expected_colliding is false, the remaining fields
150  // should be ignored.
151  bool expected_colliding{false};
153  Vector3<S> expected_normal_C;
154  Vector3<S> expected_pos_C;
155 };
156 
157 // Constructs a transform from an angle-axis rotation and origin point.
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();
163  transform.translation() = origin;
164  return transform;
165 }
166 
167 template <typename S>
168 aligned_vector<Configuration<S>> GetConfigurations() {
169  const Transform3<S> I = Transform3<S>::Identity();
170  const S kPi = constants<S>::pi();
171 
172  aligned_vector<Configuration<S>> configurations;
173 
174  // Vanilla configuration -- the un-rotated tet floats slightly above the
175  // plane.
176  configurations.push_back(Configuration<S>{
177  "simple non-colliding",
178  MakeTransform(AngleAxis<S>::Identity(), Vector3<S>{0, 0, S(0.1)}), I});
179 
180  // Simple penetration - the tet is rotated so the point V3 is down and
181  // penetrates the plane.
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)}});
187 
188  // Orient the half-space so it is not axis aligned and does not pass through
189  // the origin. Then position the tet so that the point V3 is near penetration
190  // and submit *two* configurations: one in collision, one not.
191 
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)});
195 
196  // Steal the orientation from the previous configuration so that V3 is
197  // pointing downwards into the half space.
198  const AngleAxis<S> R_HT_point_down = R_CT_point_down;
199 
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});
204 
205  // We pose the tet relative to the half plane, and then transform it again into
206  // the configuration frame. Offset of 0.5 in the z-direction gives us a
207  // penetration depth of 0.5.
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);
212  // By construction, we're colliding V3 (0, 0, 1). So, let's find where it is
213  // in this configuration.
214  const Vector3<S> p_TV3 = Vector3<S>::UnitZ();
215  const Vector3<S> p_CV3 = X_CT_colliding * p_TV3;
216  const S depth(0.5);
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});
220 
221  return configurations;
222 }
223 
224 /* Evaluates convexHalfspaceIntersect() with the given parameters and tests
225  the result based on the given test configuration's expectations. */
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;
232  bool colliding =
233  convexHalfspaceIntersect(convex_T, X_WT, half_space_H, X_WH, &results_W);
234  EXPECT_EQ(colliding, config.expected_colliding) << label;
235  if (config.expected_colliding) {
236  EXPECT_EQ(results_W.size(), 1u) << label;
237  const ContactPoint<S>& point_W = results_W[0];
238  EXPECT_NEAR(point_W.penetration_depth, config.expected_depth, eps) << label;
239  // The expected vector quantities are given in Frame C, the results are in
240  // Frame W. We need to put them in the same frame for a valid comparison.
241  EXPECT_TRUE(CompareMatrices(point_W.normal,
242  X_WC.linear() * config.expected_normal_C, eps))
243  << label;
244  EXPECT_TRUE(CompareMatrices(point_W.pos, X_WC * config.expected_pos_C, eps))
245  << label;
246  }
247 }
248 
249 /* This evaluates an instance of a test configuration and confirms the results
250  match the expected data.
251 
252  The test configuration defines the relative position of tet and half space.
253  in a configuration frame C. This is transformed into a test world frame W
254  such that X_WC has no identities (0s or 1s) (to test numerical robustness
255  against non-trivial transforms). */
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());
260  X_WCs.emplace_back(
261  MakeTransform(AngleAxis<S>{constants<S>::pi() / 7,
262  Vector3<S>{1, -2, S(-1.3)}.normalized()},
263  Vector3<S>{S(-0.25), S(0.5), S(-0.75)}));
264 
265  for (const auto& X_WC : X_WCs) {
266  // For a given configuration, we can set it up two ways:
267  // X_CT and X_CH multiply convex_T and half_space_H, respectively, or
268  // identity multiplies convex_C and half_space_C.
269  // We'll do both; their answers should be the same. This will allow us to
270  // easily confirm that all of the values contribute.
271 
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);
276 
277  // First test without penetration data. Sampling with only one of the
278  // geometry definitions seems sufficient.
279  bool colliding = convexHalfspaceIntersect<S>(convex_C, X_WC, half_space_C,
280  X_WC, nullptr);
281  EXPECT_EQ(colliding, config.expected_colliding) << config.label;
282 
283  // Test with penetration data.
284 
285  // First test where X_CH = X_CT = I and geometries are defined directly in
286  // Frame C.
287  TestCollision(convex_C, X_WC, half_space_C, X_WC, config, X_WC, eps,
288  config.label + ", X_CH = X_CT = I");
289 
290  // Now test X_CH != X_CT != I and geometries are defined in their canonical
291  // frames H and T.
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");
297  }
298 }
299 
300 // Simply evaluate collision on each of the given configurations.
301 template <typename S>
302 void QueryCollision(const aligned_vector<Configuration<S>>& configs) {
303  // Half space collision is clean enough that even under ugly rotations, we
304  // still get good precision.
305  const S eps = 2 * constants<S>::eps();
306  for (const auto& config : configs) {
307  EvalCollisionForTestConfiguration(config, eps);
308  }
309 }
310 
311 GTEST_TEST(HalfspaceConvexPrimitiveTest, CollisionTests) {
312  QueryCollision<float>(GetConfigurations<float>());
313  QueryCollision<double>(GetConfigurations<double>());
314 }
315 
316 } // namespace
317 } // namespace detail
318 } // namespace fcl
319 
320 //==============================================================================
321 int main(int argc, char* argv[]) {
322  ::testing::InitGoogleTest(&argc, argv);
323  return RUN_ALL_TESTS();
324 }
eigen_matrix_compare.h
gtest.h
fcl::CompareMatrices
::testing::AssertionResult CompareMatrices(const Eigen::MatrixBase< DerivedA > &m1, const Eigen::MatrixBase< DerivedB > &m2, double tolerance=0.0, MatrixCompareType compare_type=MatrixCompareType::absolute)
Definition: eigen_matrix_compare.h:63
fcl::constants::eps
static constexpr Real eps()
Returns ε for the precision of the underlying scalar type.
Definition: constants.h:151
EXPECT_NEAR
#define EXPECT_NEAR(val1, val2, abs_error)
Definition: gtest.h:2098
halfspace.h
fcl::transform
template Halfspace< double > transform(const Halfspace< double > &a, const Transform3< double > &tf)
label
string label
Definition: test_half_space_convex.cpp:143
EXPECT_TRUE
#define EXPECT_TRUE(condition)
Definition: gtest.h:1914
testing::internal::string
::std::string string
Definition: gtest-port.h:875
X_CH
Transform3< S > X_CH
Definition: test_half_space_convex.cpp:147
fcl::detail::convexHalfspaceIntersect
template bool convexHalfspaceIntersect(const Convex< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, Vector3< double > *contact_points, double *penetration_depth, Vector3< double > *normal)
EXPECT_EQ
#define EXPECT_EQ(expected, actual)
Definition: gtest.h:1977
expected_colliding
bool expected_colliding
Definition: test_half_space_convex.cpp:151
convex.h
RUN_ALL_TESTS
int RUN_ALL_TESTS() GTEST_MUST_USE_RESULT_
Definition: gtest.h:2287
expected_pos_C
Vector3< S > expected_pos_C
Definition: test_half_space_convex.cpp:154
expected_depth
S expected_depth
Definition: test_half_space_convex.cpp:152
main
int main(int argc, char *argv[])
Definition: test_half_space_convex.cpp:321
testing::InitGoogleTest
GTEST_API_ void InitGoogleTest(int *argc, char **argv)
Definition: gtest.cc:5008
fcl::detail::GTEST_TEST
GTEST_TEST(FCL_GJK_EPA, faceNormalPointingOutward)
Definition: test_gjk_libccd-inl_epa.cpp:205
fcl::constants::pi
static constexpr S pi()
The mathematical constant pi.
Definition: constants.h:134
halfspace-inl.h
X_CT
Transform3< S > X_CT
Definition: test_half_space_convex.cpp:145
expected_normal_C
Vector3< S > expected_normal_C
Definition: test_half_space_convex.cpp:153
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::aligned_vector
std::vector< _Tp, Eigen::aligned_allocator< _Tp > > aligned_vector
Definition: types.h:122


fcl
Author(s):
autogenerated on Fri Apr 2 2021 02:38:01