diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h index 82b0f72d5..d1edb1e88 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h @@ -226,48 +226,122 @@ _ccd_inline void tripleCross(const ccd_vec3_t *a, const ccd_vec3_t *b, ccdVec3Cross(d, &e, c); } -static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir) -{ - const ccd_support_t *A, *B; - ccd_vec3_t AB, AO, tmp; - ccd_real_t dot; - - // get last added as A - A = ccdSimplexLast(simplex); - // get the other point - B = ccdSimplexPoint(simplex, 0); - // compute AB oriented segment - ccdVec3Sub2(&AB, &B->v, &A->v); - // compute AO vector - ccdVec3Copy(&AO, &A->v); - ccdVec3Scale(&AO, -CCD_ONE); +/* This is *not* an implementation of the general function: what's the nearest + point on the line segment AB to the origin O? It is not intended to be. + This is a limited, special case which exploits the known (or at least + expected) construction history of AB. The history is as follows: + + 1. We originally started with the Minkowski support vertex B (p_OB), which + was *not* the origin. + 2. We define a support direction as p_BO = -p_OB and use that to get the + Minkowski support vector A. + 3. We confirm that O is not strictly beyond A in the direction p_BO. + 4. Then, and only then, do we invoke this method. + + The method will do one of two things: + + - determine if the origin lies within the simplex (i.e. lies on the line + segment) and reports if this is the case, otherwise + - it computes a new support direction: a vector pointing to the origin from + the nearest point on the segment AB. + + The algorithm exploits the construction history as outlined below. Without + loss of generality, we place B some non-zero distance away from the origin + along the î direction (all other orientations can be rigidly transformed to + this canonical example). The diagram below shows the origin O and the vertex + B. It also shows three regions: 1, 2, and 3. + + ĵ + 1 ⯅ 2 3 + │░░░░░░░░░░┆▒▒▒▒▒ + │░░░░░░░░░░┆▒▒▒▒▒ + │░░░░░░░░░░┆▒▒▒▒▒ + │░░░░░░░░░░┆▒▒▒▒▒ + │░░░░░░░░░░┆▒▒▒▒▒ + ───────────O──────────B────⯈ î + │░░░░░░░░░░┆▒▒▒▒▒ + │░░░░░░░░░░┆▒▒▒▒▒ + │░░░░░░░░░░┆▒▒▒▒▒ + │░░░░░░░░░░┆▒▒▒▒▒ + │░░░░░░░░░░┆▒▒▒▒▒ + + The point A cannot lie in region 3. + - B is a support vertex of the Minkowski difference. p_BO defines the + support vector that produces the support vertex A. Vertex A must lie at + least as far in the p_BO as B otherwise A is not actually a valid support + vertex for that direction. It could lie on the boundary of regions 2 & 3 + and still be a valid support vertex. + The point cannot lie in region 2 (or on the boundary between 2 & 3). + - We confirm that O is not strictly beyond A in the direction p_BO. For all + A in region 2, O lies beyond A (when projected onto the p_BO vector). + The point _must_ lie in region 1 (including the boundary between regions 1 & + 2) by the process of elimination. + + The implication of this is that the O must project onto the _interior_ of the + line segment AB (with one notable exception). If A = O, then the projection of + O is at the end point and, is in fact, itself. + + Therefore, this function can only have two possible outcomes: + + 1. In the case where p_BA = k⋅p_BO (i.e., they are co-linear), we know the + origin lies "in" the simplex. If A = O, it lies on the simplex's surface + and the objects are touching, otherwise, the objects are penetrating. + Either way, we can report that they are definitely *not* separated. + 2. Otherwise, the simplex remains unchanged, and the new support direction + is perpendicular to the line segment AB, pointing to O from the nearest + point on the segment to O. + + Return value indicates concrete knowledge that the origin lies "in" the + 2-simplex (encoded as a 1), or indication that computation should continue (0). + + @note: doSimplex2 should _only_ be called with the construction history + outlined above: promotion of a 1-simplex. doSimplex2() is only invoked by + doSimplex(). This follows the computation of A and the promotion of the + simplex. Therefore, the history is always valid; even though doSimplex3() can + demote itself to a 2-simplex, that 2-simplex immediately gets promoted back to + a 3-simplex via the same construction process. Therefore, as long as + doSimplex2() is only called from doSimplex() its region 1 assumption _should_ + remain valid. +*/ +static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir) { + const ccd_real_t eps = constants::eps(); - // dot product AB . AO - dot = ccdVec3Dot(&AB, &AO); + const Vector3 p_OA(simplex->ps[simplex->last].v.v); + const Vector3 p_OB(simplex->ps[0].v.v); - // check if origin doesn't lie on AB segment - ccdVec3Cross(&tmp, &AB, &AO); - if (ccdIsZero(ccdVec3Len2(&tmp)) && dot > CCD_ZERO){ - return 1; - } + // Confirm that A is in region 1. + assert(p_OA.normalized().dot(p_OB.normalized()) <= 0); - // check if origin is in area where AB segment is - if (ccdIsZero(dot) || dot < CCD_ZERO){ - // origin is in outside are of A - ccdSimplexSet(simplex, 0, A); - ccdSimplexSetSize(simplex, 1); - ccdVec3Copy(dir, &AO); - }else{ - // origin is in area where AB segment is - - // keep simplex untouched and set direction to - // AB x AO x AB - tripleCross(&AB, &AO, &AB, dir); - } + // Test for co-linearity. Given A is in region 1, co-linearity --> O is "in" + // the simplex. + // |A × B| = |A||B|sin(θ). If they are co-linear then theta will be zero --> + // sin(θ) = 0. So, |A × B| = 0. Equivalently, |A × B|² = 0 and since + // |A| = |B| = 1 we get |A × B|² = 0 is our test or, numerically, + // |A × B|² < ε². + // + // We normalize the vectors p_AB and p_OB so that the evaluation of the angle + // between them is *not* sensitive to the length of those vectors. Otherwise, + // disparity in length would lead to noise, or large vectors would make the + // epsilon meaningless. It's not enough to simply scale epsilon by |p_AB| and + // |p_OB| because it doesn't address the issue of |p_OB| << |p_AB| (or vice + // versa); the imprecision would be introduced in the cross-product operation. + const Vector3 phat_AB = (p_OB - p_OA).normalized(); + const Vector3 norm = p_OB.normalized().cross(phat_AB); + if (norm.squaredNorm() < eps * eps) return 1; + + // Otherwise the direction lies perpendicular to the segment, pointing towards + // O. Note: we don't normalize norm because we've already decided that it's + // magnitude is large enough (> ε) to provide a meaningful direction. + const Vector3 new_dir = norm.cross(phat_AB); + ccdVec3Set(dir, new_dir(0), new_dir(1), new_dir(2)); return 0; } +// TODO(SeanCurtis-TRI): Define the return value: +// 1: (like doSimplex2) --> origin is "in" the simplex. +// 0: +// -1: If the 3-simplex is degenerate. How is this intepreted? static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir) { const ccd_support_t *A, *B, *C; @@ -281,14 +355,21 @@ static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir) C = ccdSimplexPoint(simplex, 0); // check touching contact + // TODO(SeanCurtis-TRI): This is dist2 (i.e., dist_squared) and should be + // tested to be zero within a tolerance of ε² (and not just ε). dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, nullptr); if (ccdIsZero(dist)){ return 1; } // check if triangle is really triangle (has area > 0) - // if not simplex can't be expanded and thus no itersection is found + // if not simplex can't be expanded and thus no intersection is found + // TODO(SeanCurtis-TRI): Coincident points is sufficient but not necessary + // for a zero-area triangle. What about co-linearity? Can we guarantee that + // co-linearity can't happen? See the `triangle_area_is_zero()` method in + // this same file. if (ccdVec3Eq(&A->v, &B->v) || ccdVec3Eq(&A->v, &C->v)){ + // TODO(SeanCurtis-TRI): Why do we simply quit if the simplex is degenerate? return -1; } diff --git a/test/narrowphase/detail/convexity_based_algorithm/CMakeLists.txt b/test/narrowphase/detail/convexity_based_algorithm/CMakeLists.txt index f3a9465c3..c4695700b 100644 --- a/test/narrowphase/detail/convexity_based_algorithm/CMakeLists.txt +++ b/test/narrowphase/detail/convexity_based_algorithm/CMakeLists.txt @@ -1,6 +1,7 @@ set(tests test_gjk_libccd-inl_epa.cpp test_gjk_libccd-inl_extractClosestPoints.cpp + test_gjk_libccd-inl_gjk_doSimplex2.cpp test_gjk_libccd-inl_signed_distance.cpp ) diff --git a/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp new file mode 100644 index 000000000..2a25e85bc --- /dev/null +++ b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp @@ -0,0 +1,335 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019. Toyota Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of CNRS-LAAS and AIST nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Sean Curtis (sean@tri.global) */ + +#include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h" + +#include + +#include + +namespace fcl { +namespace detail { +namespace libccd_extension { +namespace { + +using Vector3Ccd = Vector3; + +// Generally, we assume libccd is build with ccd_real_t = double. But we express +// these functions in terms of ccd_real_t to maintain compatibility. +ccd_vec3_t eigen_to_ccd(const Vector3Ccd& vector) { + ccd_vec3_t out; + out.v[0] = vector(0); + out.v[1] = vector(1); + out.v[2] = vector(2); + return out; +} + +Vector3Ccd ccd_to_eigen(const ccd_vec3_t& vector) { + return Vector3Ccd{vector.v}; +} + +class DoSimplex2Test : public ::testing::Test { + protected: + // Sets the simplex to points A and B and _confirms_ the validity of the + // simplex as meeting doSimplex2()'s assumptions. + ::testing::AssertionResult SetValidSimplex(const ccd_vec3_t& A, + const ccd_vec3_t& B, + bool validate = true) { + Vector3Ccd p_OA(A.v); + Vector3Ccd p_OB(B.v); + // NOTE: This assertion should echo the assertion in doSimplex2(). + if (validate && p_OA.dot(p_OB) > 0) { + return ::testing::AssertionFailure() + << "Simplex points are not valid; A is not in region 1: " + << "\n p_OA: " << p_OA.transpose() + << "\n p_OB: " << p_OB.transpose(); + } + line_.ps[0].v = B; + line_.ps[1].v = A; + // This guarantees that whatever may have happened to `line` in previous + // tests, that it's still configured to be a 2-simplex. + line_.last = 1; + // Reset dir_ so that tests are independent with a recognizable magic value. + dir_ = {-1.23, 4.56, 7.89}; + return ::testing::AssertionSuccess(); + } + + // Configures the test for a death test; defines the point B to be at the + // given distance (`dist_OB`) from the origin in an arbitrary direction. + // Point A is defined by a linear combination of the direction to B (phat_OB) + // and its normal (norm_OB) as: A = u·phat_OB + v·norm_OB. + void ConfigureDeathTest(double u, double v, double dist_OB) { + const Vector3Ccd phat_OB = Vector3Ccd(1, -2, 3).normalized(); + const Vector3Ccd norm_OB = + Vector3Ccd(-phat_OB(2), 0, phat_OB(0)).normalized(); + EXPECT_EQ(norm_OB.dot(phat_OB), 0); + const Vector3Ccd p_OB = phat_OB * dist_OB; + + const Vector3Ccd p_OA = phat_OB * u + norm_OB * v; + SetValidSimplex(eigen_to_ccd(p_OA), eigen_to_ccd(p_OB), false); + } + + // The simplex the tests will operate on. + ccd_simplex_t line_; + ccd_vec3_t dir_; + + // Interpretation of doSimplex2() return values; they can't be constexpr if + // they are referenced in GTEST macros. + static const int kNeedMoreComputing; + static const int kNotSeparated; + + // Epsilon to use with tests. + static const double kEps; +}; + +const int DoSimplex2Test::kNeedMoreComputing = 0; +const int DoSimplex2Test::kNotSeparated = 1; +const double DoSimplex2Test::kEps = std::numeric_limits::epsilon(); + +// Tests the case where the origin lies on the simplex -- i.e., p_AB = k·p_OB. +// The direction value is never set (so the result is never tested), but +// doSimplex2() should always report not separated. +// NOTE: This limits the values A to being valid (i.e., within Region 1). +TEST_F(DoSimplex2Test, OriginInSimplex) { + // Create a small perturbation *slightly larger* than numerical precision. + const double delta = kEps * 2; + + // A non-axis-aligned line direction so the tests don't get fooled by easy + // zeros. + const Vector3Ccd phat_OB = Vector3Ccd(1, -2, 3).normalized(); + const Vector3Ccd norm_OB = + Vector3Ccd(-phat_OB(2), 0, phat_OB(0)).normalized(); + EXPECT_EQ(norm_OB.dot(phat_OB), 0); + const ccd_real_t dist_OB = 3; + const Vector3Ccd p_OB = phat_OB * dist_OB; + + // Case: A *is* the origin. + EXPECT_TRUE(SetValidSimplex({0., 0., 0.}, eigen_to_ccd(p_OB))); + EXPECT_EQ(kNotSeparated, doSimplex2(&line_, &dir_)); + + // Case: A is beyond the origin a very small amount. + EXPECT_TRUE( + SetValidSimplex(eigen_to_ccd(phat_OB * -delta), eigen_to_ccd(p_OB))); + EXPECT_EQ(kNotSeparated, doSimplex2(&line_, &dir_)); + + // Case: A is beyond the origin a HUGE amount. + EXPECT_TRUE( + SetValidSimplex(eigen_to_ccd(phat_OB * -1e10), eigen_to_ccd(p_OB))); + EXPECT_EQ(kNotSeparated, doSimplex2(&line_, &dir_)); + + // Case: A is as far from the origin as B, but it has an epsilon perturbation + // off the line. + EXPECT_TRUE(SetValidSimplex(eigen_to_ccd(phat_OB * -dist_OB + norm_OB * kEps), + eigen_to_ccd(p_OB))); + EXPECT_EQ(kNotSeparated, doSimplex2(&line_, &dir_)); + + // Larger perturbations from co-linear, which should be categorized as + // "needs more computing", are evaluated in the NeedMoreComputing test. +} + +// Tests the case where the origin does *not* lie on the simplex. The return +// value should always be that it needs more computing, and we validate the +// direction vector: +// dir · p_AB = 0 // perpendicular to the segment AB. +// dir · p_OA < 0 // points in the direction towards O from AB. +// // Must be strictly < 0, because we exclude +// // co-linearity cases in this test. +// dir · (p_OA × p_AB) = 0 // Lies on the plane defined b A, B, and O. +TEST_F(DoSimplex2Test, NeedMoreComputing) { + auto is_valid_dir = [this](const Vector3Ccd& p_OA, const Vector3Ccd& p_OB, + const Vector3Ccd& dir) { + // If p_OA or p_OB are large vectors, we need to scale EPS up to be able + // to recognize a valid direction to machine precision. + const ccd_real_t eps = std::max({1., p_OA.norm(), p_OB.norm()}) * kEps; + const Vector3Ccd phat_AB = (p_OB - p_OB).normalized(); + const Vector3Ccd dir_hat = dir.normalized(); + if (std::abs(dir_hat.dot(phat_AB)) > eps) { + return ::testing::AssertionFailure() + << "Direction is not perpendicular to the line segments:" + << "\n dir: " << dir.transpose() + << "\n p_OA: " << p_OA.transpose() + << "\n p_OB: " << p_OB.transpose() + << "\n dir_hat.dot(phat_AB): " << dir_hat.dot(phat_AB) + << " bigger than tolerance << " << eps; + } + // Note, in this case dir · p_OA < 0 is treated as dir · p_OA < ε to + // account for numerical issues arising from scale disparity in the vectors. + if (dir_hat.dot(p_OA) >= eps) { + return ::testing::AssertionFailure() + << "Direction does not point toward origin:" + << "\n dir: " << dir.transpose() + << "\n p_OA: " << p_OA.transpose() + << "\n p_OB: " << p_OB.transpose() + << "\n dir_hat.dot(p_OA): " << dir_hat.dot(p_OA) + << "; should be negative"; + } + if (std::abs(dir.dot(p_OA.normalized().cross(phat_AB))) > eps) { + return ::testing::AssertionFailure() + << "Direction does not lie on the triangle formed by OAB:" + << "\n dir: " << dir.transpose() + << "\n p_OA: " << p_OA.transpose() + << "\n p_OB: " << p_OB.transpose() + << "\n dir.dot(phat_OA.cross(phat_AB)): " + << dir.dot(p_OA.normalized().cross(phat_AB)) + << " bigger than tolerance << " << eps; + } + return ::testing::AssertionSuccess(); + }; + + // Create a small perturbation *slightly larger* than numerical precision. + const double delta = kEps * 2; + + // A non-axis-aligned line direction so the tests don't get fooled by easy + // zeros. + const Vector3Ccd phat_OB = Vector3Ccd(1, -2, 3).normalized(); + const Vector3Ccd norm_OB = + Vector3Ccd(-phat_OB(2), 0, phat_OB(0)).normalized(); + EXPECT_EQ(norm_OB.dot(phat_OB), 0); + const ccd_real_t dist_OB = 3; + const Vector3Ccd p_OB = phat_OB * dist_OB; + + // Case 1a: A is *near* co-linear. + { + const ccd_real_t offset = delta * 2 * dist_OB; + const Vector3Ccd p_OA = phat_OB * -dist_OB + norm_OB * offset; + EXPECT_TRUE(SetValidSimplex(eigen_to_ccd(p_OA), eigen_to_ccd(p_OB))); + EXPECT_EQ(kNeedMoreComputing, doSimplex2(&line_, &dir_)); + EXPECT_TRUE(is_valid_dir(p_OA, p_OB, ccd_to_eigen(dir_))); + } + + // Case 1b: A is *near* co-linear on the other side of AB. + { + const ccd_real_t offset = delta * 2 * dist_OB; + const Vector3Ccd p_OA = phat_OB * -dist_OB - norm_OB * offset; + EXPECT_TRUE(SetValidSimplex(eigen_to_ccd(p_OA), eigen_to_ccd(p_OB))); + EXPECT_EQ(kNeedMoreComputing, doSimplex2(&line_, &dir_)); + EXPECT_TRUE(is_valid_dir(p_OA, p_OB, ccd_to_eigen(dir_))); + } + + // Case 2a: A is approximately same distance to origin as B, but far off + // the line. + { + const Vector3Ccd p_OA = phat_OB * -dist_OB + norm_OB * dist_OB; + EXPECT_TRUE(SetValidSimplex(eigen_to_ccd(p_OA), eigen_to_ccd(p_OB))); + EXPECT_EQ(kNeedMoreComputing, doSimplex2(&line_, &dir_)); + EXPECT_TRUE(is_valid_dir(p_OA, p_OB, ccd_to_eigen(dir_))); + } + + // Case2b : A is approximately same distance to origin as B, but far off + // the line on the other side of AB. + { + const Vector3Ccd p_OA = phat_OB * -dist_OB - norm_OB * dist_OB; + EXPECT_TRUE(SetValidSimplex(eigen_to_ccd(p_OA), eigen_to_ccd(p_OB))); + EXPECT_EQ(kNeedMoreComputing, doSimplex2(&line_, &dir_)); + EXPECT_TRUE(is_valid_dir(p_OA, p_OB, ccd_to_eigen(dir_))); + } + + // Case 3a: A is far away from the origin but, relatively, a small distance + // off the line. + { + const Vector3Ccd p_OA = phat_OB * -1e10 + norm_OB * dist_OB; + EXPECT_TRUE(SetValidSimplex(eigen_to_ccd(p_OA), eigen_to_ccd(p_OB))); + EXPECT_EQ(kNeedMoreComputing, doSimplex2(&line_, &dir_)); + EXPECT_TRUE(is_valid_dir(p_OA, p_OB, ccd_to_eigen(dir_))); + } + + // Case 3b: A is far away from the origin but, relatively, a small distance + // off the line on the other side of AB. + { + const Vector3Ccd p_OA = phat_OB * -1e10 - norm_OB * dist_OB; + EXPECT_TRUE(SetValidSimplex(eigen_to_ccd(p_OA), eigen_to_ccd(p_OB))); + EXPECT_EQ(kNeedMoreComputing, doSimplex2(&line_, &dir_)); + EXPECT_TRUE(is_valid_dir(p_OA, p_OB, ccd_to_eigen(dir_))); + } +} + +#ifndef NDEBUG + +// These test the various conditions under which the assertion in doSimplex2() +// determines that the simplex is ill-defined (in that point A lies in region 2 +// or region 3). See documentation for doSimplex2() for details on these +// regions. There are a number of cases, each with a single failure instance. +// Exercised via an assertion, so only tested in debug mode. + +// Test an A that is co-linear with O and B, but only barely in region 1. +TEST_F(DoSimplex2Test, Region1Boundary1) { + ::testing::FLAGS_gtest_death_test_style = "threadsafe"; + ConfigureDeathTest(kEps, 0, 3.); + + ASSERT_DEATH(doSimplex2(&line_, &dir_), + ".*Assertion `p_OA.*dot\\(p_OB.*\\) <= 0' failed.*"); +} + +// Test an A that is barely in region 1, but as far removed from O as B. +TEST_F(DoSimplex2Test, Region1Boundary2) { + ::testing::FLAGS_gtest_death_test_style = "threadsafe"; + const double dist_OB = 3.; + const double dist_OA = dist_OB; + // As A gets farther from the O, the distance to the Region1 boundary needs + // to increase to be detectable; less than this scaled epsilon is considered + // *on the boundary*. + ConfigureDeathTest(dist_OA * kEps, dist_OA, dist_OB); + + ASSERT_DEATH(doSimplex2(&line_, &dir_), + ".*Assertion `p_OA.*dot\\(p_OB.*\\) <= 0' failed.*"); +} + +// Test an A that is barely in region 1, but far removed from O. +TEST_F(DoSimplex2Test, Region1Boundary3) { + ::testing::FLAGS_gtest_death_test_style = "threadsafe"; + const double dist_OB = 3.; + const double dist_OA = 100 * dist_OB; + // As A gets farther from the O, the distance to the Region1 boundary needs + // to increase to be detectable; less than this scaled epsilon is considered + // *on the boundary*. + ConfigureDeathTest(dist_OA * kEps, dist_OA, dist_OB); + + ASSERT_DEATH(doSimplex2(&line_, &dir_), + ".*Assertion `p_OA.*dot\\(p_OB.*\\) <= 0' failed.*"); +} + +#endif // !NDEBUG + +} // namespace +} // namespace libccd_extension +} // namespace detail +} // namespace fcl + +//============================================================================== +int main(int argc, char* argv[]) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}