From 250ab4b588b4c4241d871ea7aebdf1e3ed295193 Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Tue, 5 Feb 2019 13:00:01 -0800 Subject: [PATCH] Reworking gjk_libccd doSimplex2() 1. Add a ton of documentation about this portion of the whole algorithm. 2. Rephrase the test with a greater understanding and simpler results. 3. Add unit tests - the old code failed in the following ways: - Didn't recognize when A *was* the origin. - Much larger tolerance for determining if A was co-linear with B. 4. Incidentally add some todos to doSimplex3() while examining relationships. --- .../gjk_libccd-inl.h | 185 ++++++++-- .../convexity_based_algorithm/CMakeLists.txt | 1 + .../test_gjk_libccd-inl_gjk_doSimplex2.cpp | 332 ++++++++++++++++++ 3 files changed, 481 insertions(+), 37 deletions(-) create mode 100644 test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp 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..2cd84eb72 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 @@ -40,8 +40,8 @@ #include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd.h" -#include #include +#include #include "fcl/common/unused.h" #include "fcl/common/warning.h" @@ -226,48 +226,152 @@ _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); - - // dot product AB . AO - dot = ccdVec3Dot(&AB, &AO); +/* 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 point 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 point A. + 3. We confirm that O is not strictly beyond A in the direction p_BO + (confirming separation). + 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, confirming non-separation) 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 direction is + guaranteed; the only guarantee about the magnitude is that it is + numerically viable (i.e. greater than epsilon). + + 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 point + 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 point of the Minkowski difference. p_BO defines the + support vector that produces the support point A. Vertex A must lie at + least as far in the p_BO as B otherwise A is not actually a valid support + point for that direction. It could lie on the boundary of regions 2 & 3 + and still be a valid support point. + + The point A 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 A _must_ lie in region 1 (including the boundary between regions 1 & + 2) by 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. p_BA ≠ k⋅p_BO, we define the new support direction as 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) { + // Used to define numerical thresholds near zero; typically scaled to the size + // of the quantities being tested. + const ccd_real_t eps = constants::eps(); - // check if origin doesn't lie on AB segment - ccdVec3Cross(&tmp, &AB, &AO); - if (ccdIsZero(ccdVec3Len2(&tmp)) && dot > CCD_ZERO){ + const Vector3 p_OA(simplex->ps[simplex->last].v.v); + const Vector3 p_OB(simplex->ps[0].v.v); + + // Confirm that A is in region 1. Given that A may be very near to the origin, + // we must avoid normalizing p_OA. So, we use this instead. + // let A' be the projection of A onto the line defined by O and B. + // |A'B| >= |OB| iff A is in region 1. + // Numerically, we can express it as follows (allowing for |A'B| to be ever + // so slightly *less* than |OB|): + // p_AB ⋅ phat_OB >= |p_OB| - |p_OB| * ε = |p_OB| * (1 - ε) + // p_AB ⋅ phat_OB ⋅ |p_OB| >= |p_OB|⋅|p_OB| * (1 - ε) + // p_AB ⋅ p_OB >= |p_OB|² * (1 - ε) + // (p_OB - p_OA) ⋅ p_OB >= |p_OB|² * (1 - ε) + // p_OB ⋅ p_OB - p_OA ⋅ p_OB >= |p_OB|² * (1 - ε) + // |p_OB|² - p_OA ⋅ p_OB >= |p_OB|² * (1 - ε) + // -p_OA ⋅ p_OB >= -|p_OB|²ε + // p_OA ⋅ p_OB <= |p_OB|²ε + assert(p_OA.dot(p_OB) <= p_OB.squaredNorm() * eps && "A is not in region 1"); + + // Test for co-linearity. Given A is in region 1, co-linearity --> O is "in" + // the simplex. + // We'll use the angle between two vectors to determine co-linearity: p_AB + // and p_OB. If they are co-linear, then the angle between them (θ) is zero. + // Similarly, sin(θ) is zero. Ideally, it can be expressed as: + // |p_AB × p_OB| = |p_AB||p_OB||sin(θ)| = 0 + // Numerically, we allow θ (and sin(θ)) to be slightly larger than zero + // leading to a numerical formulation as: + // |p_AB × p_OB| = |p_AB||p_OB||sin(θ)| < |p_AB||p_OB|ε + // Finally, to reduce the computational cost, we eliminate the square roots by + // evaluating the equivalently discriminating test: + // |p_AB × p_OB|² < |p_AB|²|p_OB|²ε² + // + // In addition to providing a measure of co-linearity, the cross product gives + // us the normal to the plane on which points A, B, and O lie (which we will + // use later to compute a new support direction, as necessary). + const Vector3 p_AB = p_OB - p_OA; + const Vector3 plane_normal = p_OB.cross(p_AB); + if (plane_normal.squaredNorm() < + p_AB.squaredNorm() * p_OB.squaredNorm() * eps * eps) { return 1; } - // 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); - } - + // O is not co-linear with AB, so dist(O, AB) > ε. Define `dir` as the + // direction to O from the nearest point on AB. + // Note: We use the normalized `plane_normal` (n̂) because we've already + // concluded that the origin is farther from AB than ε. We want to make sure + // `dir` likewise has a magnitude larger than ε. With normalization, we know + // |dir| = |n̂ × AB| = |AB| > dist(O, AB) > ε. + // Without normalizing, if |OA| and |OB| were smaller than ³√ε but + // sufficiently larger than ε, dist(O, AB) > ε, but |dir| < ε. + const Vector3 new_dir = plane_normal.normalized().cross(p_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 +385,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..a6b125f50 --- /dev/null +++ b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp @@ -0,0 +1,332 @@ +/* + * 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: + void SetUp() override { + // A non-axis-aligned line direction so the tests don't get fooled by easy + // zeros. + phat_OB_ = Vector3Ccd(1, -2, 3).normalized(); + norm_OB_ = + Vector3Ccd(-phat_OB_(2), 0, phat_OB_(0)).normalized(); + EXPECT_EQ(norm_OB_.dot(phat_OB_), 0); + dist_OB_ = 3; + p_OB_ = phat_OB_ * dist_OB_; + } + + // 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) { + const Vector3Ccd p_OA(A.v); + const Vector3Ccd p_OB(B.v); + const ccd_real_t eps = constants::eps(); + // NOTE: This assertion should echo the assertion in doSimplex2(). + if (validate && p_OA.dot(p_OB) > p_OB.squaredNorm() * eps) { + 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; given the test class's definition + // of p_OB, defines point A as a linear combination of the direction to B + // (phat_OB) and its a direction perpendicular to that direction (norm_OB) as: + // A = u·phat_OB + v·norm_OB. For death tests, this skips testing that the + // defined point A is in Region 1. + void ConfigureDeathTest(double u, double v) { + 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_; + + // Configuration of O and B (tests configure A). + Vector3Ccd phat_OB_; + Vector3Ccd norm_OB_; + ccd_real_t dist_OB_; + Vector3Ccd p_OB_; + + // 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; + + // 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 by A, B, and O. +TEST_F(DoSimplex2Test, NeedMoreComputing) { + auto is_valid_dir = [](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_OA).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 plane 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; + + // 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(dist_OB_ * 2 * kEps, 0); + + ASSERT_DEATH(doSimplex2(&line_, &dir_), + ".*Assertion.*" + "p_OA.dot\\(p_OB\\) <= p_OB.squaredNorm\\(\\) \\* eps.*"); +} + +// 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_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 * 2 * kEps, dist_OA); + + ASSERT_DEATH(doSimplex2(&line_, &dir_), + ".*Assertion.*" + "p_OA.dot\\(p_OB\\) <= p_OB.squaredNorm\\(\\) \\* eps.*"); +} + +// 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_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 * 2 * kEps, dist_OA); + + ASSERT_DEATH(doSimplex2(&line_, &dir_), + ".*Assertion.*" + "p_OA.dot\\(p_OB\\) <= p_OB.squaredNorm\\(\\) \\* eps.*"); +} + +#endif // !NDEBUG + +} // namespace +} // namespace libccd_extension +} // namespace detail +} // namespace fcl + +//============================================================================== +int main(int argc, char* argv[]) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}