test_gjk_libccd-inl_gjk_doSimplex2.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019. 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 
38 
39 #include <algorithm>
40 
41 #include <gtest/gtest.h>
42 
43 namespace fcl {
44 namespace detail {
45 namespace libccd_extension {
46 namespace {
47 
48 using Vector3Ccd = Vector3<ccd_real_t>;
49 
50 // Generally, we assume libccd is build with ccd_real_t = double. But we express
51 // these functions in terms of ccd_real_t to maintain compatibility.
52 ccd_vec3_t eigen_to_ccd(const Vector3Ccd& vector) {
53  ccd_vec3_t out;
54  out.v[0] = vector(0);
55  out.v[1] = vector(1);
56  out.v[2] = vector(2);
57  return out;
58 }
59 
60 Vector3Ccd ccd_to_eigen(const ccd_vec3_t& vector) {
61  return Vector3Ccd{vector.v};
62 }
63 
64 class DoSimplex2Test : public ::testing::Test {
65  protected:
66  void SetUp() override {
67  // A non-axis-aligned line direction so the tests don't get fooled by easy
68  // zeros.
69  phat_OB_ = Vector3Ccd(1, -2, 3).normalized();
70  norm_OB_ =
71  Vector3Ccd(-phat_OB_(2), 0, phat_OB_(0)).normalized();
72  EXPECT_EQ(norm_OB_.dot(phat_OB_), 0);
73  dist_OB_ = 3;
75  }
76 
77  // Sets the simplex to points A and B and _confirms_ the validity of the
78  // simplex as meeting doSimplex2()'s assumptions.
79  ::testing::AssertionResult SetValidSimplex(const ccd_vec3_t& A,
80  const ccd_vec3_t& B,
81  bool validate = true) {
82  const Vector3Ccd p_OA(A.v);
83  const Vector3Ccd p_OB(B.v);
84  const ccd_real_t eps = constants<ccd_real_t>::eps();
85  // NOTE: This assertion should echo the assertion in doSimplex2().
86  if (validate && p_OA.dot(p_OB) > p_OB.squaredNorm() * eps) {
88  << "Simplex points are not valid; A is not in region 1: "
89  << "\n p_OA: " << p_OA.transpose()
90  << "\n p_OB: " << p_OB.transpose();
91  }
92  line_.ps[0].v = B;
93  line_.ps[1].v = A;
94  // This guarantees that whatever may have happened to `line` in previous
95  // tests, that it's still configured to be a 2-simplex.
96  line_.last = 1;
97  // Reset dir_ so that tests are independent with a recognizable magic value.
98  dir_ = {{-1.23, 4.56, 7.89}};
100  }
101 
102  // Configures the test for a death test; given the test class's definition
103  // of p_OB, defines point A as a linear combination of the direction to B
104  // (phat_OB) and its a direction perpendicular to that direction (norm_OB) as:
105  // A = u·phat_OB + v·norm_OB. For death tests, this skips testing that the
106  // defined point A is in Region 1.
107  void ConfigureDeathTest(double u, double v) {
108  const Vector3Ccd p_OA = phat_OB_ * u + norm_OB_ * v;
109  SetValidSimplex(eigen_to_ccd(p_OA), eigen_to_ccd(p_OB_), false);
110  }
111 
112  // The simplex the tests will operate on.
114  ccd_vec3_t dir_;
115 
116  // Configuration of O and B (tests configure A).
117  Vector3Ccd phat_OB_;
118  Vector3Ccd norm_OB_;
119  ccd_real_t dist_OB_;
120  Vector3Ccd p_OB_;
121 
122  // Interpretation of doSimplex2() return values; they can't be constexpr if
123  // they are referenced in GTEST macros.
124  static const int kNeedMoreComputing;
125  static const int kNotSeparated;
126 
127  // Epsilon to use with tests.
128  static const double kEps;
129 };
130 
132 const int DoSimplex2Test::kNotSeparated = 1;
134 
135 // Tests the case where the origin lies on the simplex -- i.e., p_AB = k·p_OB.
136 // The direction value is never set (so the result is never tested), but
137 // doSimplex2() should always report not separated.
138 // NOTE: This limits the values A to being valid (i.e., within Region 1).
139 TEST_F(DoSimplex2Test, OriginInSimplex) {
140  // Create a small perturbation *slightly larger* than numerical precision.
141  const double delta = kEps * 2;
142 
143  // Case: A *is* the origin.
144  EXPECT_TRUE(SetValidSimplex({{0., 0., 0.}}, eigen_to_ccd(p_OB_)));
146 
147  // Case: A is beyond the origin a very small amount.
148  EXPECT_TRUE(
149  SetValidSimplex(eigen_to_ccd(phat_OB_ * -delta), eigen_to_ccd(p_OB_)));
151 
152  // Case: A is beyond the origin a HUGE amount.
153  EXPECT_TRUE(
154  SetValidSimplex(eigen_to_ccd(phat_OB_ * -1e10), eigen_to_ccd(p_OB_)));
156 
157  // Case: A is as far from the origin as B, but it has an epsilon perturbation
158  // off the line.
159  EXPECT_TRUE(
160  SetValidSimplex(eigen_to_ccd(phat_OB_ * -dist_OB_ + norm_OB_ * kEps),
161  eigen_to_ccd(p_OB_)));
163 
164  // Larger perturbations from co-linear, which should be categorized as
165  // "needs more computing", are evaluated in the NeedMoreComputing test.
166 }
167 
168 // Tests the case where the origin does *not* lie on the simplex. The return
169 // value should always be that it needs more computing, and we validate the
170 // direction vector:
171 // dir · p_AB = 0 // perpendicular to the segment AB.
172 // dir · p_OA < 0 // points in the direction towards O from AB.
173 // // Must be strictly < 0, because we exclude
174 // // co-linearity cases in this test.
175 // dir · (p_OA × p_AB) = 0 // Lies on the plane defined by A, B, and O.
176 TEST_F(DoSimplex2Test, NeedMoreComputing) {
177  auto is_valid_dir = [](const Vector3Ccd& p_OA, const Vector3Ccd& p_OB,
178  const Vector3Ccd& dir) {
179  // If p_OA or p_OB are large vectors, we need to scale EPS up to be able
180  // to recognize a valid direction to machine precision.
181  const ccd_real_t eps =
182  std::max({ccd_real_t(1), p_OA.norm(), p_OB.norm()}) * kEps;
183  const Vector3Ccd phat_AB = (p_OB - p_OA).normalized();
184  const Vector3Ccd dir_hat = dir.normalized();
185  if (std::abs(dir_hat.dot(phat_AB)) > eps) {
187  << "Direction is not perpendicular to the line segments:"
188  << "\n dir: " << dir.transpose()
189  << "\n p_OA: " << p_OA.transpose()
190  << "\n p_OB: " << p_OB.transpose()
191  << "\n dir_hat.dot(phat_AB): " << dir_hat.dot(phat_AB)
192  << " bigger than tolerance << " << eps;
193  }
194  // Note, in this case dir · p_OA < 0 is treated as dir · p_OA < ε to
195  // account for numerical issues arising from scale disparity in the vectors.
196  if (dir_hat.dot(p_OA) >= eps) {
198  << "Direction does not point toward origin:"
199  << "\n dir: " << dir.transpose()
200  << "\n p_OA: " << p_OA.transpose()
201  << "\n p_OB: " << p_OB.transpose()
202  << "\n dir_hat.dot(p_OA): " << dir_hat.dot(p_OA)
203  << "; should be negative";
204  }
205  if (std::abs(dir.dot(p_OA.normalized().cross(phat_AB))) > eps) {
207  << "Direction does not lie on the plane formed by OAB:"
208  << "\n dir: " << dir.transpose()
209  << "\n p_OA: " << p_OA.transpose()
210  << "\n p_OB: " << p_OB.transpose()
211  << "\n dir.dot(phat_OA.cross(phat_AB)): "
212  << dir.dot(p_OA.normalized().cross(phat_AB))
213  << " bigger than tolerance << " << eps;
214  }
216  };
217 
218  // Create a small perturbation *slightly larger* than numerical precision.
219  const double delta = kEps * 2;
220 
221  // Case 1a: A is *near* co-linear.
222  {
223  const ccd_real_t offset = delta * 2 * dist_OB_;
224  const Vector3Ccd p_OA = phat_OB_ * -dist_OB_ + norm_OB_ * offset;
225  EXPECT_TRUE(SetValidSimplex(eigen_to_ccd(p_OA), eigen_to_ccd(p_OB_)));
227  EXPECT_TRUE(is_valid_dir(p_OA, p_OB_, ccd_to_eigen(dir_)));
228  }
229 
230  // Case 1b: A is *near* co-linear on the other side of AB.
231  {
232  const ccd_real_t offset = delta * 2 * dist_OB_;
233  const Vector3Ccd p_OA = phat_OB_ * -dist_OB_ - norm_OB_ * offset;
234  EXPECT_TRUE(SetValidSimplex(eigen_to_ccd(p_OA), eigen_to_ccd(p_OB_)));
236  EXPECT_TRUE(is_valid_dir(p_OA, p_OB_, ccd_to_eigen(dir_)));
237  }
238 
239  // Case 2a: A is approximately same distance to origin as B, but far off
240  // the line.
241  {
242  const Vector3Ccd p_OA = phat_OB_ * -dist_OB_ + norm_OB_ * dist_OB_;
243  EXPECT_TRUE(SetValidSimplex(eigen_to_ccd(p_OA), eigen_to_ccd(p_OB_)));
245  EXPECT_TRUE(is_valid_dir(p_OA, p_OB_, ccd_to_eigen(dir_)));
246  }
247 
248  // Case2b : A is approximately same distance to origin as B, but far off
249  // the line on the other side of AB.
250  {
251  const Vector3Ccd p_OA = phat_OB_ * -dist_OB_ - norm_OB_ * dist_OB_;
252  EXPECT_TRUE(SetValidSimplex(eigen_to_ccd(p_OA), eigen_to_ccd(p_OB_)));
254  EXPECT_TRUE(is_valid_dir(p_OA, p_OB_, ccd_to_eigen(dir_)));
255  }
256 
257  // Case 3a: A is far away from the origin but, relatively, a small distance
258  // off the line.
259  {
260  const Vector3Ccd p_OA = phat_OB_ * -1e10 + norm_OB_ * dist_OB_;
261  EXPECT_TRUE(SetValidSimplex(eigen_to_ccd(p_OA), eigen_to_ccd(p_OB_)));
263  EXPECT_TRUE(is_valid_dir(p_OA, p_OB_, ccd_to_eigen(dir_)));
264  }
265 
266  // Case 3b: A is far away from the origin but, relatively, a small distance
267  // off the line on the other side of AB.
268  {
269  const Vector3Ccd p_OA = phat_OB_ * -1e10 - norm_OB_ * dist_OB_;
270  EXPECT_TRUE(SetValidSimplex(eigen_to_ccd(p_OA), eigen_to_ccd(p_OB_)));
272  EXPECT_TRUE(is_valid_dir(p_OA, p_OB_, ccd_to_eigen(dir_)));
273  }
274 }
275 
276 #ifndef NDEBUG
277 
278 // These test the various conditions under which the assertion in doSimplex2()
279 // determines that the simplex is ill-defined (in that point A lies in region 2
280 // or region 3). See documentation for doSimplex2() for details on these
281 // regions. There are a number of cases, each with a single failure instance.
282 // Exercised via an assertion, so only tested in debug mode.
283 
284 // Test an A that is co-linear with O and B, but only barely in region 1.
285 TEST_F(DoSimplex2Test, Region1Boundary1) {
286  ::testing::FLAGS_gtest_death_test_style = "threadsafe";
287  ConfigureDeathTest(dist_OB_ * 2 * kEps, 0);
288 
289  ASSERT_DEATH(doSimplex2(&line_, &dir_),
290  ".*Assertion.*"
291  "p_OA.dot\\(p_OB\\) <= p_OB.squaredNorm\\(\\) \\* eps.*");
292 }
293 
294 // Test an A that is barely in region 1, but as far removed from O as B.
295 TEST_F(DoSimplex2Test, Region1Boundary2) {
296  ::testing::FLAGS_gtest_death_test_style = "threadsafe";
297  const double dist_OA = dist_OB_;
298  // As A gets farther from the O, the distance to the Region1 boundary needs
299  // to increase to be detectable; less than this scaled epsilon is considered
300  // *on the boundary*.
301  ConfigureDeathTest(dist_OA * 2 * kEps, dist_OA);
302 
303  ASSERT_DEATH(doSimplex2(&line_, &dir_),
304  ".*Assertion.*"
305  "p_OA.dot\\(p_OB\\) <= p_OB.squaredNorm\\(\\) \\* eps.*");
306 }
307 
308 // Test an A that is barely in region 1, but far removed from O.
309 TEST_F(DoSimplex2Test, Region1Boundary3) {
310  ::testing::FLAGS_gtest_death_test_style = "threadsafe";
311  const double dist_OA = 100 * dist_OB_;
312  // As A gets farther from the O, the distance to the Region1 boundary needs
313  // to increase to be detectable; less than this scaled epsilon is considered
314  // *on the boundary*.
315  ConfigureDeathTest(dist_OA * 2 * kEps, dist_OA);
316 
317  ASSERT_DEATH(doSimplex2(&line_, &dir_),
318  ".*Assertion.*"
319  "p_OA.dot\\(p_OB\\) <= p_OB.squaredNorm\\(\\) \\* eps.*");
320 }
321 
322 #endif // !NDEBUG
323 
324 } // namespace
325 } // namespace libccd_extension
326 } // namespace detail
327 } // namespace fcl
328 
329 //==============================================================================
330 int main(int argc, char* argv[]) {
331  ::testing::InitGoogleTest(&argc, argv);
332  return RUN_ALL_TESTS();
333 }
testing::AssertionFailure
GTEST_API_ AssertionResult AssertionFailure()
Definition: gtest.cc:976
kEps
static const double kEps
Definition: test_gjk_libccd-inl_gjk_doSimplex2.cpp:128
fcl::detail::TEST_F
TEST_F(ExtractClosestPoint, ExtractFrom1SimplexSupport)
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:356
gtest.h
epsilon
S epsilon()
Definition: test_fcl_simple.cpp:56
_ccd_simplex_t::ps
ccd_support_t ps[4]
Definition: simplex.h:44
dir_
ccd_vec3_t dir_
Definition: test_gjk_libccd-inl_gjk_doSimplex2.cpp:114
fcl::constants::eps
static constexpr Real eps()
Returns ε for the precision of the underlying scalar type.
Definition: constants.h:151
_ccd_support_t::v
ccd_vec3_t v
Support point in minkowski sum.
Definition: support.h:43
dist_OB_
ccd_real_t dist_OB_
Definition: test_gjk_libccd-inl_gjk_doSimplex2.cpp:119
fcl::detail::eigen_to_ccd
ccd_vec3_t eigen_to_ccd(const Vector3d &vector)
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:52
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::detail::libccd_extension::doSimplex2
static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir)
Definition: gjk_libccd-inl.h:313
testing::Test
Definition: gtest.h:347
EXPECT_TRUE
#define EXPECT_TRUE(condition)
Definition: gtest.h:1914
norm_OB_
Vector3Ccd norm_OB_
Definition: test_gjk_libccd-inl_gjk_doSimplex2.cpp:118
kNeedMoreComputing
static const int kNeedMoreComputing
Definition: test_gjk_libccd-inl_gjk_doSimplex2.cpp:124
EXPECT_EQ
#define EXPECT_EQ(expected, actual)
Definition: gtest.h:1977
gjk_libccd-inl.h
testing::AssertionResult
Definition: gtest.h:256
_ccd_simplex_t
Definition: simplex.h:28
kNotSeparated
static const int kNotSeparated
Definition: test_gjk_libccd-inl_gjk_doSimplex2.cpp:125
RUN_ALL_TESTS
int RUN_ALL_TESTS() GTEST_MUST_USE_RESULT_
Definition: gtest.h:2287
testing::AssertionSuccess
GTEST_API_ AssertionResult AssertionSuccess()
Definition: gtest.cc:971
fcl::detail::ccd_to_eigen
Vector3d ccd_to_eigen(const ccd_vec3_t &vector)
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:60
testing::InitGoogleTest
GTEST_API_ void InitGoogleTest(int *argc, char **argv)
Definition: gtest.cc:5008
line_
ccd_simplex_t line_
Definition: test_gjk_libccd-inl_gjk_doSimplex2.cpp:113
main
int main(int argc, char *argv[])
Definition: test_gjk_libccd-inl_gjk_doSimplex2.cpp:330
_ccd_simplex_t::last
int last
index of last added point
Definition: simplex.h:45
p_OB_
Vector3Ccd p_OB_
Definition: test_gjk_libccd-inl_gjk_doSimplex2.cpp:120
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
phat_OB_
Vector3Ccd phat_OB_
Definition: test_gjk_libccd-inl_gjk_doSimplex2.cpp:117


fcl
Author(s):
autogenerated on Fri Jun 11 2021 02:38:04