From 9dba579158109c0164bfe0e8b4a75c16bfc788f6 Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Mon, 18 Jun 2018 21:49:53 -0700 Subject: [PATCH 01/34] extractClosestPoints() from simplex made more robust to degenerate simplex (#296) * Expose numerical errors in gjk_libccd-inl.h extractClosestPoints() If the simplex is degenerate, it can lead to divide-by-zero errors. This test is drawn from the real world and exposes such a problem - nearest points are returned as NaN. See https://github.com/flexible-collision-library/fcl/issues/293 for the discussion * Make extractClosestPoints more robust For simplex of size n, if the simplex is degenerate, extracts from a simplex of size n - 1 (to prevent returning NaN). 1. Refactor point extraction into independently callable methods. 2. At each level, add method for downgrading the simplex. 3. Add unit tests on the local methods. 4. Add integration test with motivating example. 5. Update documentation of this implementation. --- .../gjk_libccd-inl.h | 378 ++++++++---- .../convexity_based_algorithm/CMakeLists.txt | 1 + ...st_gjk_libccd-inl_extractClosestPoints.cpp | 584 ++++++++++++++++++ test/test_fcl_distance.cpp | 47 ++ 4 files changed, 886 insertions(+), 124 deletions(-) create mode 100644 test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_extractClosestPoints.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 e716fc5cf..7008b8863 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 @@ -1005,148 +1005,278 @@ static int __ccdEPA(const void *obj1, const void *obj2, return 0; } -/** Returns the points p1 and p2 on the original shapes that correspond to point - * p in the given simplex.*/ -static void extractClosestPoints(ccd_simplex_t* simplex, - ccd_vec3_t* p1, ccd_vec3_t* p2, ccd_vec3_t* p) -{ + +/** Reports true if p and q are coincident. */ +static bool are_coincident(const ccd_vec3_t& p, const ccd_vec3_t& q) { + // This uses a scale-dependent basis for determining coincidence. It examines + // each axis independently, and only, if all three axes are sufficiently + // close (relative to its own scale), are the two points considered + // coincident. + // + // For dimension i, two values are considered the same if: + // |pᵢ - qᵢ| <= ε·max(1, |pᵢ|, |pᵢ|) + // And the points are coincident if the previous condition for all + // `i ∈ {0, 1, 2}` (i.e. the x-, y-, *and* z-dimensions). + using std::abs; + using std::max; + + const ccd_real_t eps = constants::eps(); + // NOTE: Wrapping "1.0" with ccd_real_t accounts for mac problems where ccd + // is actually float based. + for (int i = 0; i < 3; ++i) { + const ccd_real_t scale = + max({ccd_real_t{1}, abs(p.v[i]), abs(q.v[i])}) * eps; + const ccd_real_t delta = abs(p.v[i] - q.v[i]); + if (delta > scale) return false; + } + return true; +} + +/** Determines if the the triangle defined by the three vertices has zero area. + Area can be zero for one of two reasons: + - the triangle is so small that the vertices are functionally coincident, or + - the vertices are co-linear. + Both conditions are computed with respect to machine precision. + @returns true if the area is zero. */ +static bool triangle_area_is_zero(const ccd_vec3_t& a, const ccd_vec3_t& b, + const ccd_vec3_t& c) { + // First coincidence condition. This doesn't *explicitly* test for b and c + // being coincident. That will be captured in the subsequent co-linearity + // test. If b and c *were* coincident, it would be cheaper to perform the + // coincidence test than the co-linearity test. + // However, the expectation is that typically the triangle will not have zero + // area. In that case, we want to minimize the number of tests performed on + // the average, so we prefer to eliminate one coincidence test. + if (are_coincident(a, b) || are_coincident(a, c)) return true; + + // We're going to compute the *sine* of the angle θ between edges (given that + // the vertices are *not* coincident). If the sin(θ) < ε, the edges are + // co-linear. + ccd_vec3_t AB, AC, n; + ccdVec3Sub2(&AB, &b, &a); + ccdVec3Sub2(&AC, &c, &a); + ccdVec3Normalize(&AB); + ccdVec3Normalize(&AC); + ccdVec3Cross(&n, &AB, &AC); + const ccd_real_t eps = constants::eps(); + // Second co-linearity condition. + if (ccdVec3Len2(&n) < eps * eps) return true; + return false; +} + +/** Given a single support point, `q`, extract the point `p1` and `p2`, the + points on object 1 and 2, respectively, in the support data of `q`. */ +static void extractObjectPointsFromPoint(ccd_support_t *q, ccd_vec3_t *p1, + ccd_vec3_t *p2) { + // TODO(SeanCurtis-TRI): Determine if I should be demanding that p1 and p2 + // are defined. + // Closest points are the ones stored in the simplex + if (p1) *p1 = q->v1; + if (p2) *p2 = q->v2; +} + +/** Given two support points which define a line segment (`a` and `b`), and a + point on that line segment `p`, computes the points `p1` and `p2`, the points + on object 1 and 2, respectively, in the support data which correspond to `p`. + @pre `p = a + s(b - a), 0 <= s <= 1` */ +static void extractObjectPointsFromSegment(ccd_support_t *a, ccd_support_t *b, + ccd_vec3_t *p1, ccd_vec3_t *p2, + ccd_vec3_t *p) { + // Closest points lie on the segment defined by the points in the simplex + // Let the segment be defined by points A and B. We can write p as + // + // p = A + s*AB, 0 <= s <= 1 + // p - A = s*AB + ccd_vec3_t AB; + ccdVec3Sub2(&AB, &(b->v), &(a->v)); + + // This defines three equations, but we only need one. Taking the i-th + // component gives + // + // p_i - A_i = s*AB_i. + // + // Thus, s is given by + // + // s = (p_i - A_i)/AB_i. + // + // To avoid dividing by an AB_i ≪ 1, we choose i such that |AB_i| is + // maximized + ccd_real_t abs_AB_x{std::abs(ccdVec3X(&AB))}; + ccd_real_t abs_AB_y{std::abs(ccdVec3Y(&AB))}; + ccd_real_t abs_AB_z{std::abs(ccdVec3Z(&AB))}; + + ccd_real_t A_i, AB_i, p_i; + if (abs_AB_x >= abs_AB_y && abs_AB_x >= abs_AB_z) { + A_i = ccdVec3X(&(a->v)); + AB_i = ccdVec3X(&AB); + p_i = ccdVec3X(p); + } else if (abs_AB_y >= abs_AB_z) { + A_i = ccdVec3Y(&(a->v)); + AB_i = ccdVec3Y(&AB); + p_i = ccdVec3Y(p); + } else { + A_i = ccdVec3Z(&(a->v)); + AB_i = ccdVec3Z(&AB); + p_i = ccdVec3Z(p); + } + + if (std::abs(AB_i) < constants::eps()) { + // Points are coincident; treat as a single point. + extractObjectPointsFromPoint(a, p1, p2); + return; + } + + auto calc_p = [](ccd_vec3_t *p_a, ccd_vec3_t *p_b, ccd_vec3_t *p, + ccd_real_t s) { + ccd_vec3_t sAB; + ccdVec3Sub2(&sAB, p_b, p_a); + ccdVec3Scale(&sAB, s); + ccdVec3Copy(p, p_a); + ccdVec3Add(p, &sAB); + }; + + // TODO(SeanCurtis-TRI): If p1 or p2 is null, there seems little point in + // calling this method. It seems that both of these being non-null should be + // a *requirement*. Determine that this is the case and do so. + ccd_real_t s = (p_i - A_i) / AB_i; + + if (p1) { + // p1 = A1 + s*A1B1 + calc_p(&(a->v1), &(b->v1), p1, s); + } + if (p2) { + // p2 = A2 + s*A2B2 + calc_p(&(a->v2), &(b->v2), p2, s); + } +} + +/** Returns the points `p1` and `p2` on the original shapes that correspond to + point `p` in the given simplex. + @pre simplex size <= 3. + @pre p lies _on_ the simplex (i.e., within the triangle, line segment, or is + coincident with the point). */ +static void extractClosestPoints(ccd_simplex_t *simplex, ccd_vec3_t *p1, + ccd_vec3_t *p2, ccd_vec3_t *p) { const int simplex_size = ccdSimplexSize(simplex); assert(simplex_size <= 3); if (simplex_size == 1) { - // Closest points are the ones stored in the simplex - if(p1) *p1 = simplex->ps[simplex->last].v1; - if(p2) *p2 = simplex->ps[simplex->last].v2; + extractObjectPointsFromPoint(&simplex->ps[0], p1, p2); + } + else if (simplex_size == 2) + { + extractObjectPointsFromSegment(&simplex->ps[0], &simplex->ps[1], p1, p2, p); } - else if (simplex_size ==2) + else // simplex_size == 3 { - // Closest points lie on the segment defined by the points in the simplex - // Let the segment be defined by points A and B. We can write p as - // - // p = A + s*AB, 0 <= s <= 1 - // p - A = s*AB - ccd_vec3_t AB; + if (triangle_area_is_zero(simplex->ps[0].v, simplex->ps[1].v, + simplex->ps[2].v)) { + // The triangle is degenerate; compute the nearest point to a line + // segment. The segment is defined by the most-distant vertex pair. + int a_index, b_index; + ccd_vec3_t AB, AC, BC; ccdVec3Sub2(&AB, &(simplex->ps[1].v), &(simplex->ps[0].v)); - - // This defines three equations, but we only need one. Taking the i-th - // component gives - // - // p_i - A_i = s*AB_i. - // - // Thus, s is given by - // - // s = (p_i - A_i)/AB_i. - // - // To avoid dividing by an AB_i ≪ 1, we choose i such that |AB_i| is - // maximized - ccd_real_t abs_AB_x{std::abs(ccdVec3X(&AB))}; - ccd_real_t abs_AB_y{std::abs(ccdVec3Y(&AB))}; - ccd_real_t abs_AB_z{std::abs(ccdVec3Z(&AB))}; - ccd_real_t s{0}; - - if (abs_AB_x >= abs_AB_y && abs_AB_x >= abs_AB_z) { - ccd_real_t A_x{ccdVec3X(&(simplex->ps[0].v))}; - ccd_real_t AB_x{ccdVec3X(&AB)}; - ccd_real_t p_x{ccdVec3X(p)}; - s = (p_x - A_x) / AB_x; - } else if (abs_AB_y >= abs_AB_z) { - ccd_real_t A_y{ccdVec3Y(&(simplex->ps[0].v))}; - ccd_real_t AB_y{ccdVec3Y(&AB)}; - ccd_real_t p_y{ccdVec3Y(p)}; - s = (p_y - A_y) / AB_y; + ccdVec3Sub2(&AC, &(simplex->ps[2].v), &(simplex->ps[0].v)); + ccdVec3Sub2(&BC, &(simplex->ps[2].v), &(simplex->ps[1].v)); + ccd_real_t AB_len2 = ccdVec3Len2(&AB); + ccd_real_t AC_len2 = ccdVec3Len2(&AC); + ccd_real_t BC_len2 = ccdVec3Len2(&BC); + if (AB_len2 >= AC_len2 && AB_len2 >= BC_len2) { + a_index = 0; + b_index = 1; + } else if (AC_len2 >= AB_len2 && AC_len2 >= BC_len2) { + a_index = 0; + b_index = 2; } else { - ccd_real_t A_z{ccdVec3Z(&(simplex->ps[0].v))}; - ccd_real_t AB_z{ccdVec3Z(&AB)}; - ccd_real_t p_z{ccdVec3Z(p)}; - s = (p_z - A_z) / AB_z; + a_index = 1; + b_index = 2; } + extractObjectPointsFromSegment(&simplex->ps[a_index], + &simplex->ps[b_index], p1, p2, p); + return; + } - if (p1) - { - // p1 = A1 + s*A1B1 - ccd_vec3_t sAB; - ccdVec3Sub2(&sAB, &(simplex->ps[1].v1), &(simplex->ps[0].v1)); - ccdVec3Scale(&sAB, s); - ccdVec3Copy(p1, &(simplex->ps[0].v1)); - ccdVec3Add(p1, &sAB); - } - if (p2) - { - // p2 = A2 + s*A2B2 - ccd_vec3_t sAB; - ccdVec3Sub2(&sAB, &(simplex->ps[1].v2), &(simplex->ps[0].v2)); - ccdVec3Scale(&sAB, s); - ccdVec3Copy(p2, &(simplex->ps[0].v2)); - ccdVec3Add(p2, &sAB); - } - } - else // simplex_size == 3 - { - // Closest points lie in the triangle defined by the points in the simplex - ccd_vec3_t AB, AC, n, v_prime, AB_cross_v_prime, AC_cross_v_prime; - // Let the triangle be defined by points A, B, and C. The triangle lies in - // the plane that passes through A, whose normal is given by n̂, where + // Compute the barycentric coordinates of point p in triangle ABC. // - // n = AB × AC - // n̂ = n / ‖n‖ - ccdVec3Sub2(&AB, &(simplex->ps[1].v), &(simplex->ps[0].v)); - ccdVec3Sub2(&AC, &(simplex->ps[2].v), &(simplex->ps[0].v)); - ccdVec3Cross(&n, &AB, &AC); - - // Since p lies in ABC, it can be expressed as + // A + // ╱╲ p = αA + βB + γC + // ╱ |╲ + // ╱ | ╲ α = 1 - β - γ + // ╱ p | ╲ β = AREA(pAC) / AREA(ABC) + // ╱ / \ ╲ γ = AREA(pAB) / AREA(ABC) + // ╱__/_____\_╲ + // B C AREA(XYZ) = |r_XY × r_XZ| / 2 // - // p = A + p' - // p' = s*AB + t*AC + // Rewrite coordinates in terms of cross products. // - // where 0 <= s, t, s+t <= 1. - ccdVec3Sub2(&v_prime, p, &(simplex->ps[0].v)); - - // To find the corresponding v1 and v2, we need - // values for s and t. Taking cross products with AB and AC gives the - // following system: + // β = AREA(pAC) / AREA(ABC) = |r_Ap × r_AC| / |r_AB × r_AC| + // γ = AREA(pAB) / AREA(ABC) = |r_AB × r_Ap| / |r_AB × r_AC| // - // AB × p' = t*(AB × AC) = t*n - // AC × p' = -s*(AB × AC) = -s*n - ccdVec3Cross(&AB_cross_v_prime, &AB, &v_prime); - ccdVec3Cross(&AC_cross_v_prime, &AC, &v_prime); - - // To convert this to a system of scalar equations, we take the dot product - // with n: + // NOTE: There are multiple options for the cross products, these have been + // selected to re-use as many symbols as possible. // - // n ⋅ (AB × p') = t * ‖n‖² - // n ⋅ (AC × p') = -s * ‖n‖² - ccd_real_t norm_squared_n{ccdVec3Len2(&n)}; - - // Therefore, s and t are given by + // Solving for β and γ: + // + // β = |r_Ap × r_AC| / |r_AB × r_AC| + // β = |r_Ap × r_AC| / |n| n ≙ r_AB × r_AC, n̂ = n / |n| + // β = n̂·(r_Ap × r_AC) / n̂·n This step arises from the fact + // that (r_Ap × r_AC) and n point + // in the same direction. It + // allows us to take a single sqrt + // instead of three. + // β = (n/|n|)·(r_Ap × r_AC) / (n/|n|)·n + // β = n·(r_Ap × r_AC) / n·n + // β = n·(r_Ap × r_AC) / |n|² // - // s = -n ⋅ (AC × p') / ‖n‖² - // t = n ⋅ (AB × p') / ‖n‖² - ccd_real_t s{-ccdVec3Dot(&n, &AC_cross_v_prime) / norm_squared_n}; - ccd_real_t t{ccdVec3Dot(&n, &AB_cross_v_prime) / norm_squared_n}; + // A similar process to solve for gamma + // γ = n·(r_AB × r_Ap) / |n|² + + // Compute n and |n|². + ccd_vec3_t r_AB, r_AC, n; + ccdVec3Sub2(&r_AB, &(simplex->ps[1].v), &(simplex->ps[0].v)); + ccdVec3Sub2(&r_AC, &(simplex->ps[2].v), &(simplex->ps[0].v)); + ccdVec3Cross(&n, &r_AB, &r_AC); + ccd_real_t norm_squared_n{ccdVec3Len2(&n)}; - if (p1) - { - // p1 = A1 + s*A1B1 + t*A1C1 - ccd_vec3_t sAB, tAC; - ccdVec3Sub2(&sAB, &(simplex->ps[1].v1), &(simplex->ps[0].v1)); - ccdVec3Scale(&sAB, s); - ccdVec3Sub2(&tAC, &(simplex->ps[2].v1), &(simplex->ps[0].v1)); - ccdVec3Scale(&tAC, t); - ccdVec3Copy(p1, &(simplex->ps[0].v1)); - ccdVec3Add(p1, &sAB); - ccdVec3Add(p1, &tAC); + // Compute r_Ap. + ccd_vec3_t r_Ap; + ccdVec3Sub2(&r_Ap, p, &(simplex->ps[0].v)); + + // Compute the cross products in the numerators. + ccd_vec3_t r_Ap_cross_r_AC, r_AB_cross_r_Ap; + ccdVec3Cross(&r_Ap_cross_r_AC, &r_Ap, &r_AC); // r_Ap × r_AC + ccdVec3Cross(&r_AB_cross_r_Ap, &r_AB, &r_Ap); // r_AB × r_Ap + + // Compute beta and gamma. + ccd_real_t beta{ccdVec3Dot(&n, &r_Ap_cross_r_AC) / norm_squared_n}; + ccd_real_t gamma{ccdVec3Dot(&n, &r_AB_cross_r_Ap) / norm_squared_n}; + + // Evaluate barycentric interpolation (with the locally defined barycentric + // coordinates). + auto interpolate = [&beta, &gamma](const ccd_vec3_t& r_WA, + const ccd_vec3_t& r_WB, + const ccd_vec3_t& r_WC, + ccd_vec3_t* r_WP) { + // r_WP = r_WA + β * r_AB + γ * r_AC + ccdVec3Copy(r_WP, &r_WA); + + ccd_vec3_t beta_r_AB; + ccdVec3Sub2(&beta_r_AB, &r_WB, &r_WA); + ccdVec3Scale(&beta_r_AB, beta); + ccdVec3Add(r_WP, &beta_r_AB); + + ccd_vec3_t gamma_r_AC; + ccdVec3Sub2(&gamma_r_AC, &r_WC, &r_WA); + ccdVec3Scale(&gamma_r_AC, gamma); + ccdVec3Add(r_WP, &gamma_r_AC); + }; + + if (p1) { + interpolate(simplex->ps[0].v1, simplex->ps[1].v1, simplex->ps[2].v1, p1); } - if (p2) - { - // p2 = A2 + s*A2B2 + t*A2C2 - ccd_vec3_t sAB, tAC; - ccdVec3Sub2(&sAB, &(simplex->ps[1].v2), &(simplex->ps[0].v2)); - ccdVec3Scale(&sAB, s); - ccdVec3Sub2(&tAC, &(simplex->ps[2].v2), &(simplex->ps[0].v2)); - ccdVec3Scale(&tAC, t); - ccdVec3Copy(p2, &(simplex->ps[0].v2)); - ccdVec3Add(p2, &sAB); - ccdVec3Add(p2, &tAC); + + if (p2) { + interpolate(simplex->ps[0].v2, simplex->ps[1].v2, simplex->ps[2].v2, p2); } } } diff --git a/test/narrowphase/detail/convexity_based_algorithm/CMakeLists.txt b/test/narrowphase/detail/convexity_based_algorithm/CMakeLists.txt index 65a6843b3..82f645866 100644 --- a/test/narrowphase/detail/convexity_based_algorithm/CMakeLists.txt +++ b/test/narrowphase/detail/convexity_based_algorithm/CMakeLists.txt @@ -1,5 +1,6 @@ set(tests test_gjk_libccd-inl.cpp + test_gjk_libccd-inl_extractClosestPoints.cpp ) # Build all the tests diff --git a/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_extractClosestPoints.cpp b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_extractClosestPoints.cpp new file mode 100644 index 000000000..a8b565290 --- /dev/null +++ b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_extractClosestPoints.cpp @@ -0,0 +1,584 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. 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 */ + +#include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h" + +#include +#include + +#include "fcl/common/types.h" + +namespace fcl { +namespace detail { + +using Vector3d = Vector3; + +// Helper functions to work with libccd types -- the ccd_vec3_t is a nuisance. +// This allows working with Eigen Vector3d's instead. + +ccd_vec3_t eigen_to_ccd(const Vector3d& vector) { + ccd_vec3_t out; + out.v[0] = vector(0); + out.v[1] = vector(1); + out.v[2] = vector(2); + return out; +} + +Vector3d ccd_to_eigen(const ccd_vec3_t& vector) { + // TODO(SeanCurtis-TRI): When libccd is *always* double precision, this can + // become: `return Vector3d{vector.v};` + return Vector3d{vector.v[0], vector.v[1], vector.v[2]}; +} + +// Tests that the given vector and libccd vector are the same with in a given +// tolerance. +::testing::AssertionResult are_same(const Vector3d& expected, + const ccd_vec3_t& tested, + double tolerance) { + if (tolerance < 0) { + return ::testing::AssertionFailure() << "Invalid tolerance: " + << tolerance; + } + + Vector3d ccd = ccd_to_eigen(tested); + Vector3d error = (expected - ccd).cwiseAbs(); + + for (int i = 0; i < 3; ++i) { + if (error(i) > tolerance) { + return ::testing::AssertionFailure() + << "Values at index " << i + << " exceed tolerance; " << expected(i) << " vs " + << ccd(i) << ", diff = " << error(i) + << ", tolerance = " << tolerance << "\nexpected = " + << expected.transpose() << "\ntested = " + << ccd.transpose() << "\n|delta| = " + << error.transpose(); + } + } + return ::testing::AssertionSuccess(); +} + +// These are tests on the extractClosestPoints() function (and its underlying +// support functions). +// +// These functions map a simplex and a point of interest on the "surface" of the +// simplex into a pair of points. The simplex is defined by support vertices +// (ccd_support_t). Each support vertex contains three vectors in R3. They +// represent: +// 1. A vertex `v` on the boundary of Minkowski difference of two objects: +// O₁ ⊕ -O₂. It is, by definition the difference of two vertex positions +// on the two objects: `v = v₁ - v₂`. +// 2. The vertex `v₁` on O₁ that contributes to the definition of `v`. +// 3. The vertex `v₂` on O₂ that contributes to the definition of `v`. +// +// extractClosestPoints() supports 1-, 2- and 3-simplices. +// +// The corresponding points are written to output parameters. If the output +// parameters are null, the value is not computed. These tests exercise all +// permutations of the two output parameters. + +// Simple wrapper to facilitate working with Eigen primitives in place of libccd +// primitives. +bool are_coincident(const Vector3d& p, const Vector3d& q) { + return libccd_extension::are_coincident(eigen_to_ccd(p), eigen_to_ccd(q)); +} + +// Tests the `are_coincident` function. +GTEST_TEST(DegenerateGeometry, CoincidentPoints) { + // The coincidence test uses this as the threshold. + const ccd_real_t eps = constants::eps(); + const ccd_real_t almost_eps = eps * 0.75; + const ccd_real_t extra_eps = eps * 1.5; + + Vector3d p{1, 1, 1}; + Vector3d q{p}; + // Exact coincidence at unit scale (down to the last bit) + EXPECT_TRUE(are_coincident(p, q)); + + // Coincidence within a unit-scaled epsilon. + EXPECT_TRUE(are_coincident(p + Vector3d{almost_eps, 0, 0}, q)); + EXPECT_TRUE(are_coincident(p + Vector3d{0, almost_eps, 0}, q)); + EXPECT_TRUE(are_coincident(p + Vector3d{0, 0, almost_eps}, q)); + + // Distinct points just outside a unit-scaled epsilon. + EXPECT_FALSE(are_coincident(p + Vector3d{extra_eps, 0, 0}, q)); + EXPECT_FALSE(are_coincident(p + Vector3d{0, extra_eps, 0}, q)); + EXPECT_FALSE(are_coincident(p + Vector3d{0, 0, extra_eps}, q)); + + // Coincidence within a larger-than-unit-scale scale factor. + const double scale = 100; + p << scale, scale, scale; + q = p; + // Exact coincidence at a larger-than-unit-scale (down to the last bit). + EXPECT_TRUE(are_coincident(p, q)); + + // Coincidence within a larger-than-unit-scale epsilon. + EXPECT_TRUE(are_coincident(p + Vector3d{almost_eps * scale, 0, 0}, q)); + EXPECT_TRUE(are_coincident(p + Vector3d{0, almost_eps * scale, 0}, q)); + EXPECT_TRUE(are_coincident(p + Vector3d{0, 0, almost_eps * scale}, q)); + + // Distinct points just outside a larger-than-unit-scaled epsilon. + EXPECT_FALSE(are_coincident(p + Vector3d{extra_eps * scale, 0, 0}, q)); + EXPECT_FALSE(are_coincident(p + Vector3d{0, extra_eps * scale, 0}, q)); + EXPECT_FALSE(are_coincident(p + Vector3d{0, 0, extra_eps * scale}, q)); + + // Coincidence within a smaller-than-unit-scale scale factor. NOTE: eps + // stays at an *absolute* tolerance as the scale gets smaller. + p << 0.01, 0.01, 0.01; + q = p; + // Exact coincidence at a smaller-than-unit-scale (down to the last bit). + EXPECT_TRUE(are_coincident(p, q)); + + // Coincidence within a smaller-than-unit-scale epsilon. + EXPECT_TRUE(are_coincident(p + Vector3d{almost_eps, 0, 0}, q)); + EXPECT_TRUE(are_coincident(p + Vector3d{0, almost_eps, 0}, q)); + EXPECT_TRUE(are_coincident(p + Vector3d{0, 0, almost_eps}, q)); + + // Distinct points just outside a smaller-than-unit-scaled epsilon. + EXPECT_FALSE(are_coincident(p + Vector3d{extra_eps, 0, 0}, q)); + EXPECT_FALSE(are_coincident(p + Vector3d{0, extra_eps, 0}, q)); + EXPECT_FALSE(are_coincident(p + Vector3d{0, 0, extra_eps}, q)); +} + +// Wrapper to allow invocation of triangle_area_is_zero with eigen primitives. +bool triangle_area_is_zero(const Vector3d& a, const Vector3d& b, + const Vector3d& c) { + return fcl::detail::libccd_extension::triangle_area_is_zero(eigen_to_ccd(a), + eigen_to_ccd(b), + eigen_to_ccd(c)); +} + +// Tests the `triangle_area_is_zero()` function. NOTE: This computation +// makes use of `are_coincident()`. Only a single test of coincident +// vertices is provided (to exercise the code path). The permutations for how +// two points can be considered coincident are done in their own tests. +GTEST_TEST(DegenerateGeometry, ZeroAreaTriangle) { + using std::asin; + Vector3d a{0, 0, 0}; + Vector3d b{1, 0, 0}; + Vector3d c{0, 1, 0}; + + // Viable triangle + EXPECT_FALSE(triangle_area_is_zero(a, b, c)); + + // Triangle with token coincident vertices to make sure it keys on it. + EXPECT_TRUE(triangle_area_is_zero(a, a, c)); + + // Co-linearity tests. + + // Obvious co-linearity + EXPECT_TRUE(triangle_area_is_zero(a, b, 3 * b)); + + // Test co-linearity + // The co-linear threshold is based on the angles of the triangle. If the + // triangle has an angle θ, such that |sin(θ)| < ε, then the triangle is + // considered to be degenerate. This condition implicitly defines an envelope + // around θ. We define θₑ such that sin(θₑ) = ε. The condition |sin(θ)| < ε + // implies |θ| < θₑ. So, if the smallest angle is less than θₑ, the triangle + // will be considered co-linear. If the smallest angle is larger than θₑ, the + // triangle is considered valid. + // + // The test wants to provide proof as to the boundary of the tolerance + // envelope. Therefore, it will construct two triangles. One whose smallest + // angle is θₑ - δ and the other triangle's smallest angle is θₑ + δ, for + // some suitably small δ. + // + // The `triangle_area_is_zero()` function has a second failure condition: + // coincident points. We'll make sure that the points are all far enough away + // from each other that this failure condition won't be triggered. + // + // The triangle consists of three vertices: A, B, C. + // A: [0, 0, 0] (for simplicity). + // B: [1, 1, 1] (again, for simplicity). + // C: We must define C such that the angle ∠CAB is θ = θₑ ± δ. + // + // We'll do it by construction: + // 1. Pick a vector v perpendicular to [1, 1, 1] (e.g., v = [1, 1, -2]). + // 2. C' = B + v̂ * |B|·tan(θ) (with v̂ = v / |v|). This produces a point C' + // that: + // a) forms a triangle where ∠C`AB < θₑ, if θ < θₑ, but + // b) the points B and C' *may* be coincident. + // 3. Move the point C' farther away (in the AC' direction). This preserves + // the angle, but distances B from C'. So, C ≙ s·C' (for some scalar + // s ≠ 1). This trick works because A is the origin and we generally + // assume that |C'| >> ε. + // + // This triangle illustrates the construction (but rotated for ascii art). + // + // A + // /| θ = ∠BAC = ∠BAC' + // / | + // C'/__| + // / / B + // / / + // // + // C + + const ccd_real_t eps = constants::eps(); + const ccd_real_t theta_e = asin(eps); + + a << 0, 0, 0; + b << 1, 1, 1; + const Vector3d v_hat = Vector3d(1, 1, -2).normalized(); + + // Triangle where θ < θₑ. + const ccd_real_t colinear_theta = tan(theta_e * 0.75); + Vector3d c_prime = b + (b.norm() * colinear_theta) * v_hat; + c = c_prime * 2; + EXPECT_FALSE(are_coincident(b, c)); + EXPECT_TRUE(triangle_area_is_zero(a, b, c)); + + // Triangle where θ > θₑ. + const ccd_real_t good_tan = tan(theta_e * 1.5); + c_prime = b + (b.norm() * good_tan) * v_hat; + c = c_prime * 2; + // Confirm the test doesn't report false because the points are coincident. + EXPECT_FALSE(are_coincident(b, c)); + ASSERT_FALSE(triangle_area_is_zero(a, b, c)); +} + +// This class creates a single simplex. It is the Minkowski difference of +// one triangles and a vertex. +class ExtractClosestPoint : public ::testing::Test { + protected: + void SetUp() override { + // Configure a 3-simplex; "last" is the index of the last valid simplex. + // For an n-simplex, last is always n - 1. + simplex_.last = 2; + for (int i = 0; i < 3; ++i) { + const Vector3d minkowski_diff{v0_ - t1_[i]}; + write_support(minkowski_diff, v0_, t1_[i], &simplex_.ps[i]); + } + } + + // Write the three Eigen vector values into a ccd support vector. + static void write_support(const Vector3d& minkowski_diff, const Vector3d& v0, + const Vector3d& v1, ccd_support_t* support) { + support->v = eigen_to_ccd(minkowski_diff); + support->v1 = eigen_to_ccd(v0); + support->v2 = eigen_to_ccd(v1); + } + + // Performs the common work of evaluating extractClosetPoint() on a + // permutation of parameters. + void EvaluateExtractClosestPoint(ccd_simplex_t* simplex, + const Vector3d& p0_expected, + const Vector3d& p1_expected, + ccd_vec3_t* closest, + const char* message) { + using fcl::detail::libccd_extension::extractClosestPoints; + + const Vector3d& dummy1{-1, -2, -3}; + const Vector3d& dummy2{-2, -3, -4}; + ccd_vec3_t p0 = eigen_to_ccd(dummy1); + ccd_vec3_t p1 = eigen_to_ccd(dummy2); + + // Confirm expected solution are not the dummy values. + EXPECT_FALSE(are_same(p0_expected, p0, kTolerance)); + EXPECT_FALSE(are_same(p1_expected, p1, kTolerance)); + + // Test extraction of neither. + EXPECT_NO_THROW(extractClosestPoints(simplex, nullptr, nullptr, closest)) + << message; + + // Test extraction of p0. + EXPECT_NO_THROW(extractClosestPoints(simplex, &p0, nullptr, closest)) + << message; + EXPECT_TRUE(are_same(p0_expected, p0, kTolerance)) << message; + + // Test extraction of p1. + EXPECT_NO_THROW(extractClosestPoints(simplex, nullptr, &p1, closest)) + << message; + EXPECT_TRUE(are_same(p1_expected, p1, kTolerance)) << message; + + // Test extraction of both. + p0 = eigen_to_ccd(dummy1); + p1 = eigen_to_ccd(dummy2); + EXPECT_NO_THROW(extractClosestPoints(simplex, &p0, &p1, closest)) + << message; + EXPECT_TRUE(are_same(p0_expected, p0, kTolerance)) << message; + EXPECT_TRUE(are_same(p1_expected, p1, kTolerance)) << message; + } + + // Perform linear interpolation between points a & b. + // @pre 0 <= s <= 1. + static Vector3d lerp(const Vector3d& a, const Vector3d& b, double s) { + return a * s + b * (1 - s); + }; + + ccd_simplex_t simplex_; + + // Vertex on object 0. + const Vector3d v0_{0.5, 1, 0}; + + // Vertices 0, 1, & 2 for triangle 1. + const Vector3d t1_[3] = {{0, 0.25, 0.25}, {1, 0.25, 0.25}, {0.5, 0.8, 1.0}}; + + // TODO(SeanCurtis-TRI): Change this to 1e-15 when the mac libccd + // single/double precision has been worked out. + const double kTolerance{constants::eps_78()}; +}; + +// Test extraction from a 1-simplex support method. +TEST_F(ExtractClosestPoint, ExtractFrom1SimplexSupport) { + using namespace fcl::detail::libccd_extension; + const Vector3d& dummy1{-1, -2, -3}; + const Vector3d& dummy2{-2, -3, -4}; + // Set up the single support *point*. + ccd_vec3_t p0 = eigen_to_ccd(dummy1); + ccd_vec3_t p1 = eigen_to_ccd(dummy2); + + // Test extraction of neither. + EXPECT_NO_THROW( + extractObjectPointsFromPoint(&simplex_.ps[0], nullptr, nullptr)); + + // Test extraction of p1. + EXPECT_NO_THROW(extractObjectPointsFromPoint(&simplex_.ps[0], &p0, nullptr)); + EXPECT_TRUE(are_same(v0_, p0, kTolerance)); + + // Test extraction of p2. + EXPECT_NO_THROW(extractObjectPointsFromPoint(&simplex_.ps[0], nullptr, &p1)); + EXPECT_TRUE(are_same(t1_[0], p1, kTolerance)); + + // Test extraction of both. + p0 = eigen_to_ccd(dummy1); + p1 = eigen_to_ccd(dummy2); + extractObjectPointsFromPoint(&simplex_.ps[0], &p0, &p1); + EXPECT_TRUE(are_same(v0_, p0, kTolerance)); + EXPECT_TRUE(are_same(t1_[0], p1, kTolerance)); +} + +// Test extraction from a 1-simplex through the extractClosestPoints() method. +TEST_F(ExtractClosestPoint, ExtractFrom1Simplex) { + simplex_.last = 0; + + // NOTE: For a 1-simplex, the closest point isn't used. + ccd_vec3_t closest = eigen_to_ccd({0, 0, 0}); + + EvaluateExtractClosestPoint(&simplex_, v0_, t1_[0], &closest, + "ExtractFrom1Simplex"); +} + +// Test extraction from a 2-simplex support method. +TEST_F(ExtractClosestPoint, ExtractFrom2SimplexSupport) { + using namespace fcl::detail::libccd_extension; + const Vector3d& dummy1{-1, -2, -3}; + const Vector3d& dummy2{-2, -3, -4}; + + ccd_vec3_t p0 = eigen_to_ccd(dummy1); + ccd_vec3_t p1 = eigen_to_ccd(dummy2); + + // The query point we're going to use is a simple linear combination of the + // two end points. + const Vector3d m0 = ccd_to_eigen(simplex_.ps[0].v); + const Vector3d m1 = ccd_to_eigen(simplex_.ps[1].v); + + const double s = 1 / 3.0; + ASSERT_TRUE(s >= 0 && s <= 1); + + ccd_vec3_t closest = eigen_to_ccd(lerp(m0, m1, s)); + const Vector3d p0_expected = v0_; + const Vector3d p1_expected = lerp(t1_[0], t1_[1], s); + + // Test extraction of neither. + EXPECT_NO_THROW(extractObjectPointsFromSegment( + &simplex_.ps[0], &simplex_.ps[1], nullptr, nullptr, &closest)); + + // Test extraction of p1. + EXPECT_NO_THROW(extractObjectPointsFromSegment( + &simplex_.ps[0], &simplex_.ps[1], &p0, nullptr, &closest)); + EXPECT_TRUE(are_same(p0_expected, p0, kTolerance)); + + // Test extraction of p2. + EXPECT_NO_THROW(extractObjectPointsFromSegment( + &simplex_.ps[0], &simplex_.ps[1], nullptr, &p1, &closest)); + EXPECT_TRUE(are_same(p1_expected, p1, kTolerance)); + + // Test extraction of both. + p0 = eigen_to_ccd(dummy1); + p1 = eigen_to_ccd(dummy2); + EXPECT_NO_THROW(extractObjectPointsFromSegment( + &simplex_.ps[0], &simplex_.ps[1], &p0, &p1, &closest)); + EXPECT_TRUE(are_same(p0_expected, p0, kTolerance)); + EXPECT_TRUE(are_same(p1_expected, p1, kTolerance)); +} + +// Test extraction from a 2-simplex through the extractClosestPoints() method. +TEST_F(ExtractClosestPoint, ExtractFrom2Simplex) { + simplex_.last = 1; + + // The query point we're going to use is a simple linear combination of the + // two end points. + const Vector3d m0 = ccd_to_eigen(simplex_.ps[0].v); + const Vector3d m1 = ccd_to_eigen(simplex_.ps[1].v); + + const double s = 1 / 3.0; + ASSERT_TRUE(s >= 0 && s <= 1); + + ccd_vec3_t closest = eigen_to_ccd(lerp(m0, m1, s)); + const Vector3d p0_expected = v0_; + const Vector3d p1_expected = lerp(t1_[0], t1_[1], s); + + EvaluateExtractClosestPoint(&simplex_, p0_expected, p1_expected, &closest, + "ExtractFrom2Simplex"); +} + +// Tests the case where the simplex is a degenerate simplex -- i.e., it is +// actually a line segment. +TEST_F(ExtractClosestPoint, ExtractFrom2SimplexDegenerate) { + simplex_.last = 1; + // NOTE: This exercises the knowledge that the coincidence tolerance is eps. + const ccd_real_t eps = 0.5 * constants::eps(); + + // Copy the first support vertex into the second support vertex and then + // perturb the second a small amount. We add a small amount to the + // x-components of the minkowski sum *and* the object1 vertex. + ccdSupportCopy(&simplex_.ps[1], &simplex_.ps[0]); + simplex_.ps[1].v.v[0] += eps; + simplex_.ps[1].v1.v[0] += eps; + // Confirm that the input and expected answer (v0_) match. + ASSERT_TRUE(are_same(v0_, simplex_.ps[0].v1, kTolerance)); + + // The line segment is now of length 1; the answer should be essentially the + // same as evaluating for a single point. + ccd_vec3_t closest = eigen_to_ccd({0, 0, 0}); + + EvaluateExtractClosestPoint(&simplex_, v0_, t1_[0], &closest, + "ExtractFrom2SimplexDegenerate"); +} + +// Test extraction from a 3-simplex through the extractClosestPoints() method. +// Note: there is no support method for the 3-simplex like there is for the 1- +// and 2-simplices. +TEST_F(ExtractClosestPoint, ExtractFrom3Simplex) { + // Compute a "closest point" based on arbitrary barycentric coordinates. + const double alpha = 0.25; + const double beta = 0.33; + ASSERT_TRUE(alpha >= 0 && alpha <= 1 && beta >= 0 && beta <= 1 && + alpha + beta <= 1); + + const Vector3d m0 = ccd_to_eigen(simplex_.ps[0].v); + const Vector3d m1 = ccd_to_eigen(simplex_.ps[1].v); + const Vector3d m2 = ccd_to_eigen(simplex_.ps[2].v); + + // Interpolate three vertices via barycentric coordinates s1 and s2. + auto interpolate = [](const Vector3d& a, const Vector3d& b, const Vector3d& c, + double s1, double s2) -> Vector3d { + return a * s1 + b * s2 + c * (1 - s1 - s2); + }; + + ccd_vec3_t closest = eigen_to_ccd(interpolate(m0, m1, m2, alpha, beta)); + const Vector3d p0_expected = v0_; + const Vector3d p1_expected = interpolate(t1_[0], t1_[1], t1_[2], alpha, beta); + + EvaluateExtractClosestPoint(&simplex_, p0_expected, p1_expected, &closest, + "ExtractFrom3Simplex"); +} + +// Tests the case where the 3-simplex is degenerate -- the points are considered +// coincident. +TEST_F(ExtractClosestPoint, ExtractFrom3SimplesDegenerateCoincident) { + // This test essentially reproduces the *valid* result from + // ExtractFrom2Simplex(). The difference is that it *claims* to be a triangle. + + // NOTE: This exercises the knowledge that the coincidence tolerance is eps. + const ccd_real_t eps = 0.5 * constants::eps(); + + // Copy the first support vertex into the second and third support vertices + // and then perturb the copies a small amount. We add a small amount to the + // x-components of the minkowski sum *and* the object1 vertex. + for (auto i : {1, 2}) { + ccdSupportCopy(&simplex_.ps[i], &simplex_.ps[0]); + simplex_.ps[i].v.v[0] += eps; + simplex_.ps[i].v1.v[0] += eps; + } + // Confirm that the input and expected answer (v0_) match. + ASSERT_TRUE(are_same(v0_, simplex_.ps[0].v1, kTolerance)); + + // The triangle has zero area because the vertices are all coincident; + // the answer should be essentially the same as evaluating for a single point. + ccd_vec3_t closest = eigen_to_ccd({0, 0, 0}); + + EvaluateExtractClosestPoint(&simplex_, v0_, t1_[0], &closest, + "ExtractFrom3SimplexDegenerateCoincident"); +} + +// Tests the case where the 3-simplex is degenerate -- the points are considered +// co-linear. +TEST_F(ExtractClosestPoint, ExtractFrom3SimplesDegenerateColinear) { + // The query point we're going to use is a simple linear combination of the + // v0 and v1 (from the minkowski sum). That means the points on the triangle + // should likewise be a combination of v0 and v1 from each shape. + const Vector3d m0 = ccd_to_eigen(simplex_.ps[0].v); + const Vector3d m1 = ccd_to_eigen(simplex_.ps[1].v); + + const double s = 1 / 3.0; + ASSERT_TRUE(s >= 0 && s <= 1); + + ccd_vec3_t closest = eigen_to_ccd(lerp(m0, m1, s)); + const Vector3d p0_expected = v0_; + const Vector3d p1_expected = lerp(t1_[0], t1_[1], s); + + // Now set up co-linear configuration. Vertex 2 will simply be a linear + // combination of vertex 0 and vertex 1. + // v2 = 2 * (v1 - v0) + v0 = 2 * v1 - v0. + // Note: this puts v2 on the same line, but not inside the line segment + // spanned by v0 and v1. Because the closest point lies on the segment, this + // confirms that arbitrarily extending the degeneracy doesn't change the + // answer. + auto linearly_combine = [](const ccd_vec3_t& a, const ccd_vec3_t& b, + ccd_vec3_t* dst) { + auto A = ccd_to_eigen(a); + auto B = ccd_to_eigen(b); + auto C = 2 * B - A; + *dst = eigen_to_ccd(C); + }; + linearly_combine(simplex_.ps[0].v, simplex_.ps[1].v, &simplex_.ps[2].v); + linearly_combine(simplex_.ps[0].v1, simplex_.ps[1].v1, &simplex_.ps[2].v1); + linearly_combine(simplex_.ps[0].v2, simplex_.ps[1].v2, &simplex_.ps[2].v2); + + EvaluateExtractClosestPoint(&simplex_, p0_expected, p1_expected, &closest, + "ExtractFrom3SimplexDegenerateColinear"); +} + +} // namespace detail +} // namespace fcl + +//============================================================================== +int main(int argc, char* argv[]) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/test_fcl_distance.cpp b/test/test_fcl_distance.cpp index 98b02da0e..d49dc7e1f 100644 --- a/test/test_fcl_distance.cpp +++ b/test/test_fcl_distance.cpp @@ -41,6 +41,9 @@ #include "test_fcl_utility.h" #include "fcl_resources/config.h" +// TODO(SeanCurtis-TRI): A file called `test_fcl_distance.cpp` should *not* have +// collision tests. + using namespace fcl; bool verbose = false; @@ -305,6 +308,50 @@ GTEST_TEST(FCL_DISTANCE, mesh_distance) test_mesh_distance(); } +template +void NearestPointFromDegenerateSimplex() { + // Tests a historical bug. In certain configurations, the distance query + // would terminate with a degenerate 3-simplex; the triangle was actually a + // line segment. As a result, nearest points were populated with NaN values. + // See https://github.com/flexible-collision-library/fcl/issues/293 for + // more discussion. + DistanceResult result; + DistanceRequest request; + request.enable_nearest_points = true; + request.gjk_solver_type = GJKSolverType::GST_LIBCCD; + + // These values were extracted from a real-world scenario that produced NaNs. + std::shared_ptr> box_geometry_1( + new Box(2.750000, 6.000000, 0.050000)); + std::shared_ptr> box_geometry_2( + new Box(0.424000, 0.150000, 0.168600)); + CollisionObject box_object_1( + box_geometry_1, Eigen::Quaterniond(1, 0, 0, 0).matrix(), + Eigen::Vector3d(1.625000, 0.000000, 0.500000)); + CollisionObject box_object_2( + box_geometry_2, + Eigen::Quaterniond(0.672811, 0.340674, 0.155066, 0.638138).matrix(), + Eigen::Vector3d(0.192074, -0.277870, 0.273546)); + + EXPECT_NO_THROW(fcl::distance(&box_object_1, &box_object_2, request, result)); + + // The values here have been visually confirmed from the computation. + S expected_dist{0.053516322172152138}; + Vector3 expected_p0{-1.375, -0.098881502700918666, -0.025000000000000022}; + Vector3 expected_p1{0.21199965773384655, 0.074999692703297122, + 0.084299993303443954}; + EXPECT_TRUE(nearlyEqual(result.nearest_points[0], expected_p0)); + EXPECT_TRUE(nearlyEqual(result.nearest_points[1], expected_p1)); + // TODO(SeanCurtis-TRI): Change this tolerance to constants::eps_34() when + // the mac single/double libccd problem is resolved. + EXPECT_NEAR(expected_dist, result.min_distance, + constants::eps_34()); +} + +GTEST_TEST(FCL_DISTANCE, NearestPointFromDegenerateSimplex) { + NearestPointFromDegenerateSimplex(); +} + template void distance_Test_Oriented(const Transform3& tf, const std::vector>& vertices1, const std::vector& triangles1, From 07c8480f70e4b5bfa15cb37b76fc5271d2502e82 Mon Sep 17 00:00:00 2001 From: Hongkai Dai Date: Tue, 3 Jul 2018 08:43:44 -0700 Subject: [PATCH 02/34] Fix FCL's EPA implementation, when calling libccd solver. (#305) Rewrite FCL's EPA implementation and add many unit tests. --- .../gjk_libccd-inl.h | 956 ++++++++---- .../convexity_based_algorithm/CMakeLists.txt | 3 +- .../test_gjk_libccd-inl.cpp | 163 --- .../test_gjk_libccd-inl_epa.cpp | 1279 +++++++++++++++++ .../test_gjk_libccd-inl_signed_distance.cpp | 342 +++++ test/test_fcl_signed_distance.cpp | 15 +- 6 files changed, 2300 insertions(+), 458 deletions(-) delete mode 100644 test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp create mode 100644 test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp create mode 100644 test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_signed_distance.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 7008b8863..572455826 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,6 +40,9 @@ #include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd.h" +#include +#include + #include "fcl/common/unused.h" #include "fcl/common/warning.h" @@ -555,90 +558,134 @@ static int simplexToPolytope2(const void *obj1, const void *obj2, return 0; } - -/** Transforms simplex to polytope, three vertices required */ -static int simplexToPolytope3(const void *obj1, const void *obj2, - const ccd_t *ccd, - const ccd_simplex_t *simplex, - ccd_pt_t *pt, ccd_pt_el_t **nearest) -{ - const ccd_support_t *a, *b, *c; - ccd_support_t d, d2; - ccd_vec3_t ab, ac, dir; - ccd_pt_vertex_t *v[5]; - ccd_pt_edge_t *e[9]; - ccd_real_t dist, dist2; - - *nearest = NULL; - - a = ccdSimplexPoint(simplex, 0); - b = ccdSimplexPoint(simplex, 1); - c = ccdSimplexPoint(simplex, 2); - - // If only one triangle left from previous GJK run origin lies on this - // triangle. So it is necessary to expand triangle into two - // tetrahedrons connected with base (which is exactly abc triangle). - - // get next support point in direction of normal of triangle - ccdVec3Sub2(&ab, &b->v, &a->v); - ccdVec3Sub2(&ac, &c->v, &a->v); - ccdVec3Cross(&dir, &ab, &ac); - __ccdSupport(obj1, obj2, &dir, ccd, &d); - dist = ccdVec3PointTriDist2(&d.v, &a->v, &b->v, &c->v, NULL); - - // and second one take in opposite direction - ccdVec3Scale(&dir, -CCD_ONE); - __ccdSupport(obj1, obj2, &dir, ccd, &d2); - dist2 = ccdVec3PointTriDist2(&d2.v, &a->v, &b->v, &c->v, NULL); - - // check if face isn't already on edge of minkowski sum and thus we - // have touching contact - if (ccdIsZero(dist) || ccdIsZero(dist2)){ - v[0] = ccdPtAddVertex(pt, a); - v[1] = ccdPtAddVertex(pt, b); - v[2] = ccdPtAddVertex(pt, c); - e[0] = ccdPtAddEdge(pt, v[0], v[1]); - e[1] = ccdPtAddEdge(pt, v[1], v[2]); - e[2] = ccdPtAddEdge(pt, v[2], v[0]); - *nearest = (ccd_pt_el_t *)ccdPtAddFace(pt, e[0], e[1], e[2]); - if (*nearest == NULL) - return -2; - - return -1; +#ifndef NDEBUG +static bool isPolytopeEmpty(const ccd_pt_t& polytope) { + ccd_pt_vertex_t* v = nullptr; + ccdListForEachEntry(&polytope.vertices, v, ccd_pt_vertex_t, list) { + if (v) { + return false; } + } + ccd_pt_edge_t* e = nullptr; + ccdListForEachEntry(&polytope.edges, e, ccd_pt_edge_t, list) { + if (e) { + return false; + } + } + ccd_pt_face_t* f = nullptr; + ccdListForEachEntry(&polytope.faces, f, ccd_pt_face_t, list) { + if (f) { + return false; + } + } + return true; +} +#endif - // form polyhedron - v[0] = ccdPtAddVertex(pt, a); - v[1] = ccdPtAddVertex(pt, b); - v[2] = ccdPtAddVertex(pt, c); - v[3] = ccdPtAddVertex(pt, &d); - v[4] = ccdPtAddVertex(pt, &d2); - - e[0] = ccdPtAddEdge(pt, v[0], v[1]); - e[1] = ccdPtAddEdge(pt, v[1], v[2]); - e[2] = ccdPtAddEdge(pt, v[2], v[0]); - - e[3] = ccdPtAddEdge(pt, v[3], v[0]); - e[4] = ccdPtAddEdge(pt, v[3], v[1]); - e[5] = ccdPtAddEdge(pt, v[3], v[2]); - - e[6] = ccdPtAddEdge(pt, v[4], v[0]); - e[7] = ccdPtAddEdge(pt, v[4], v[1]); - e[8] = ccdPtAddEdge(pt, v[4], v[2]); +/** Transforms a 2-simplex (triangle) to a polytope (tetrahedron), three + * vertices required. + * Both the simplex and the transformed polytope contain the origin. The simplex + * vertices lie on the surface of the Minkowski difference obj1 ⊖ obj2. + * @param[in] obj1 object 1 on which the distance is queried. + * @param[in] obj2 object 2 on which the distance is queried. + * @param[in] ccd The ccd solver. + * @param[in] simplex The simplex (with three vertices) that contains the + * origin. + * @param[out] polytope The polytope (tetrahedron) that also contains the origin + * on one of its faces. At input, the polytope should be empty. + * @param[out] nearest If the function detects that obj1 and obj2 are touching, + * then set *nearest to be the nearest points on obj1 and obj2 respectively; + * otherwise set *nearest to NULL. @note nearest cannot be NULL. + * @retval status return 0 on success, -1 if touching contact is detected, and + * -2 on non-recoverable failure (mostly due to memory allocation bug). + */ +static int convert2SimplexToTetrahedron(const void* obj1, const void* obj2, + const ccd_t* ccd, const ccd_simplex_t* simplex, + ccd_pt_t* polytope, ccd_pt_el_t** nearest) { + assert(nearest); + assert(isPolytopeEmpty(*polytope)); + assert(simplex->last == 2); // a 2-simplex. + const ccd_support_t *a, *b, *c; + ccd_support_t d, d2; + ccd_vec3_t ab, ac, dir; + ccd_pt_vertex_t* v[4]; + ccd_pt_edge_t* e[6]; + ccd_real_t dist, dist2; + + *nearest = NULL; + + a = ccdSimplexPoint(simplex, 0); + b = ccdSimplexPoint(simplex, 1); + c = ccdSimplexPoint(simplex, 2); + + // The 2-simplex is just a triangle containing the origin. We will expand this + // triangle to a tetrahedron, by adding the support point along the normal + // direction of the triangle. + ccdVec3Sub2(&ab, &b->v, &a->v); + ccdVec3Sub2(&ac, &c->v, &a->v); + ccdVec3Cross(&dir, &ab, &ac); + __ccdSupport(obj1, obj2, &dir, ccd, &d); + dist = ccdVec3PointTriDist2(&d.v, &a->v, &b->v, &c->v, NULL); + + // and second one take in opposite direction + ccdVec3Scale(&dir, -CCD_ONE); + __ccdSupport(obj1, obj2, &dir, ccd, &d2); + dist2 = ccdVec3PointTriDist2(&d2.v, &a->v, &b->v, &c->v, NULL); + + // check if face isn't already on edge of minkowski sum and thus we + // have touching contact + if (ccdIsZero(dist) || ccdIsZero(dist2)) { + v[0] = ccdPtAddVertex(polytope, a); + v[1] = ccdPtAddVertex(polytope, b); + v[2] = ccdPtAddVertex(polytope, c); + e[0] = ccdPtAddEdge(polytope, v[0], v[1]); + e[1] = ccdPtAddEdge(polytope, v[1], v[2]); + e[2] = ccdPtAddEdge(polytope, v[2], v[0]); + *nearest = (ccd_pt_el_t*)ccdPtAddFace(polytope, e[0], e[1], e[2]); + if (*nearest == NULL) return -2; - if (ccdPtAddFace(pt, e[3], e[4], e[0]) == NULL - || ccdPtAddFace(pt, e[4], e[5], e[1]) == NULL - || ccdPtAddFace(pt, e[5], e[3], e[2]) == NULL + return -1; + } + // Form a tetrahedron with abc as one face, pick either d or d2, based + // on which one has larger distance to the face abc. We pick the larger + // distance because it gives a tetrahedron with larger volume, so potentially + // more "expanded" than the one with the smaller volume. + auto FormTetrahedron = [polytope, a, b, c, &v, + &e](const ccd_support_t& new_support) -> int { + v[0] = ccdPtAddVertex(polytope, a); + v[1] = ccdPtAddVertex(polytope, b); + v[2] = ccdPtAddVertex(polytope, c); + v[3] = ccdPtAddVertex(polytope, &new_support); + + e[0] = ccdPtAddEdge(polytope, v[0], v[1]); + e[1] = ccdPtAddEdge(polytope, v[1], v[2]); + e[2] = ccdPtAddEdge(polytope, v[2], v[0]); + e[3] = ccdPtAddEdge(polytope, v[0], v[3]); + e[4] = ccdPtAddEdge(polytope, v[1], v[3]); + e[5] = ccdPtAddEdge(polytope, v[2], v[3]); - || ccdPtAddFace(pt, e[6], e[7], e[0]) == NULL - || ccdPtAddFace(pt, e[7], e[8], e[1]) == NULL - || ccdPtAddFace(pt, e[8], e[6], e[2]) == NULL){ - return -2; + // ccdPtAdd*() functions return NULL either if the memory allocation + // failed of if any of the input pointers are NULL, so the bad + // allocation can be checked by the last calls of ccdPtAddFace() + // because the rest of the bad allocations eventually "bubble up" here + // Note, there is no requirement on the winding of the face, namely we do + // not guarantee if all f.e(0).cross(f.e(1)) points outward (or inward) for + // all the faces added below. + if (ccdPtAddFace(polytope, e[0], e[1], e[2]) == NULL || + ccdPtAddFace(polytope, e[3], e[4], e[0]) == NULL || + ccdPtAddFace(polytope, e[4], e[5], e[1]) == NULL || + ccdPtAddFace(polytope, e[5], e[3], e[2]) == NULL) { + return -2; } - return 0; -} + }; + if (std::abs(dist) > std::abs(dist2)) { + return FormTetrahedron(d); + } else { + return FormTetrahedron(d2); + } +} /** Transforms simplex to polytope. It is assumed that simplex has 4 * vertices! */ @@ -660,13 +707,15 @@ static int simplexToPolytope4(const void *obj1, const void *obj2, d = ccdSimplexPoint(simplex, 3); // check if origin lies on some of tetrahedron's face - if so use - // simplexToPolytope3() + // convert2SimplexToTetrahedron() use_polytope3 = 0; dist = ccdVec3PointTriDist2(ccd_vec3_origin, &a->v, &b->v, &c->v, NULL); if (ccdIsZero(dist)){ use_polytope3 = 1; } dist = ccdVec3PointTriDist2(ccd_vec3_origin, &a->v, &c->v, &d->v, NULL); + // TODO(hongkai.dai@tri.global) Optimize this part, check whether these + // "if" branches are mutually exclusive. if (ccdIsZero(dist)){ use_polytope3 = 1; ccdSimplexSet(simplex, 1, c); @@ -687,7 +736,7 @@ static int simplexToPolytope4(const void *obj1, const void *obj2, if (use_polytope3){ ccdSimplexSetSize(simplex, 3); - return simplexToPolytope3(obj1, obj2, ccd, simplex, pt, nearest); + return convert2SimplexToTetrahedron(obj1, obj2, ccd, simplex, pt, nearest); } // no touching contact - simply create tetrahedron @@ -716,190 +765,498 @@ static int simplexToPolytope4(const void *obj1, const void *obj2, return 0; } +/** + * Computes the normal vector of a triangular face on a polytope, and the normal + * vector points outward from the polytope. Notice we assume that the origin + * lives within the polytope, and the normal vector may not have unit length. + * @param[in] polytope The polytope on which the face lives. We assume that the + * origin also lives inside the polytope. + * @param[in] face The face for which the normal vector is computed. + * @retval dir The vector normal to the face, and points outward from the + * polytope. `dir` is unnormalized, that it does not necessarily have a unit + * length. + */ +static ccd_vec3_t faceNormalPointingOutward(const ccd_pt_t* polytope, + const ccd_pt_face_t* face) { + // We find two edges of the triangle as e1 and e2, and the normal vector + // of the face is e1.cross(e2). + ccd_vec3_t e1, e2; + ccdVec3Sub2(&e1, &(face->edge[0]->vertex[1]->v.v), + &(face->edge[0]->vertex[0]->v.v)); + ccdVec3Sub2(&e2, &(face->edge[1]->vertex[1]->v.v), + &(face->edge[1]->vertex[0]->v.v)); + ccd_vec3_t dir; + // TODO(hongkai.dai): we ignore the degeneracy here, namely we assume e1 and + // e2 are not colinear. We should check if e1 and e2 are colinear, and handle + // this corner case. + ccdVec3Cross(&dir, &e1, &e2); + const ccd_real_t dir_norm = std::sqrt(ccdVec3Len2(&dir)); + // The winding of the triangle is *not* guaranteed. The normal `n = e₁ × e₂` + // may point inside or outside. We rely on the fact that the origin lies + // within the polytope to resolve this ambiguity. A vector from the origin to + // any point on the triangle must point in the "same" direction as the normal + // (positive dot product). + + // However, the distance to the origin may be too small for the origin to + // serve as a reliable witness of inside-ness. In that case, we examine the + // polytope's *other* vertices; they should all lie on the "inside" of the + // current triangle. If at least one is a reliable distance, then that is + // considered to be the inside. If all vertices are "too close" (like the + // origin), then "inside" is defined as the side of the triangle that had the + // most distant vertex. + + // For these tests, we use the arbitrary distance of 0.01 unit length as a + // "reliable" distance for both the origin and other vertices. Even if it + // seems large, the fall through case of comparing the maximum distance will + // always guarantee correctness. + const ccd_real_t dist_tol = 0.01; + ccd_real_t tol = dist_tol * dir_norm; + ccd_real_t projection = ccdVec3Dot(&dir, &(face->edge[0]->vertex[0]->v.v)); + if (projection < -tol) { + // Origin is more than `dist_tol` away from the plane, but the negative + // value implies that the normal vector is pointing in the wrong direction; + // flip it. + ccdVec3Scale(&dir, ccd_real_t(-1)); + } else if (projection >= -tol && projection <= tol) { + // The origin is close to the plane of the face. Pick another vertex to test + // the normal direction. + ccd_real_t max_projection = -CCD_REAL_MAX; + ccd_real_t min_projection = CCD_REAL_MAX; + ccd_pt_vertex_t* v; + // If the magnitude of the projection is larger than tolerance, then it + // means one of the vertices is at least 1cm away from the plane coinciding + // with the face. + ccdListForEachEntry(&polytope->vertices, v, ccd_pt_vertex_t, list) { + projection = ccdVec3Dot(&dir, &(v->v.v)); + if (projection > tol) { + // The vertex is at least dist_tol away from the face plane, on the same + // direction of `dir`. So we flip dir to point it outward from the + // polytope. + ccdVec3Scale(&dir, ccd_real_t(-1)); + break; + } else if (projection < -tol) { + // The vertex is at least 1cm away from the face plane, on the opposite + // direction of `dir`. So `dir` points outward already. + break; + } else { + max_projection = std::max(max_projection, projection); + min_projection = std::min(min_projection, projection); + } + } + // If max_projection > |min_projection|, it means that the vertices that are + // on the positive side of the plane, has a larger maximal distance than the + // vertices on the negative side of the plane. Thus we regard that `dir` + // points into the polytope. Hence we flip `dir`. + if (max_projection > std::abs(min_projection)) { + ccdVec3Scale(&dir, ccd_real_t(-1)); + } + } + return dir; +} + +// Return true if the point `pt` is on the outward side of the half-plane, on +// which the triangle `f1 lives. Notice if the point `pt` is exactly on the +// half-plane, the return is false. +// @param f A triangle on a polytope. +// @param pt A point. +static bool isOutsidePolytopeFace(const ccd_pt_t* polytope, + const ccd_pt_face_t* f, const ccd_vec3_t* pt) { + ccd_vec3_t n = faceNormalPointingOutward(polytope, f); + // r_VP is the vector from a vertex V on the face `f`, to the point P `pt`. + ccd_vec3_t r_VP; + ccdVec3Sub2(&r_VP, pt, &(f->edge[0]->vertex[0]->v.v)); + return ccdVec3Dot(&n, &r_VP) > 0; +} + +#ifndef NDEBUG +// The function ComputeVisiblePatchRecursiveSanityCheck() is only called in the +// debug mode. In the release mode, this function is declared/defined but not +// used. Without this NDEBUG macro, the function will cause a -Wunused-function +// error on CI's release builds. +/** + * Reports true if the visible patch is valid. + * The invariant for computing the visible patch is that for each edge in the + * polytope, if both neighbouring faces are visible, then the edge is an + * internal edge; if only one neighbouring face is visible, then the edge + * is a border edge. + * For each face, if one of its edges is an internal edge, then the face is + * visible. + */ +static bool ComputeVisiblePatchRecursiveSanityCheck( + const ccd_pt_t& polytope, + const std::unordered_set& border_edges, + const std::unordered_set& visible_faces, + const std::unordered_set& internal_edges) { + ccd_pt_face_t* f; + ccdListForEachEntry(&polytope.faces, f, ccd_pt_face_t, list) { + bool has_edge_internal = false; + for (int i = 0; i < 3; ++i) { + // Since internal_edges is a set, internal_edges.count(e) means that e + // is contained in the set internal_edges. + if (internal_edges.count(f->edge[i]) > 0) { + has_edge_internal = true; + break; + } + } + if (has_edge_internal) { + if (visible_faces.count(f) == 0) { + return false; + } + } + } + ccd_pt_edge_t* e; + ccdListForEachEntry(&polytope.edges, e, ccd_pt_edge_t, list) { + if (visible_faces.count(e->faces[0]) > 0 && + visible_faces.count(e->faces[1]) > 0) { + if (internal_edges.count(e) == 0) { + return false; + } + } else if (visible_faces.count(e->faces[0]) + + visible_faces.count(e->faces[1]) == + 1) { + if (border_edges.count(e) == 0) { + return false; + } + } + } + return true; +} +#endif +/** + * This function contains the implementation detail of ComputeVisiblePatch() + * function. It should not be called by any function other than + * ComputeVisiblePatch(). + */ +static void ComputeVisiblePatchRecursive( + const ccd_pt_t& polytope, ccd_pt_face_t& f, int edge_index, + const ccd_vec3_t& query_point, + std::unordered_set* border_edges, + std::unordered_set* visible_faces, + std::unordered_set* internal_edges) { + /* + This function will be called recursively. It first checks if the face `g` + neighouring face `f` along the common edge `f->edge[edge_index]` can be seen + from the point `query_point`. If this face g cannot be seen, then stop. + Otherwise, we continue to check the neighbouring faces of g, by calling this + function recursively. + */ + ccd_pt_face_t* g = f.edge[edge_index]->faces[0] == &f + ? f.edge[edge_index]->faces[1] + : f.edge[edge_index]->faces[0]; + if (visible_faces->count(g) == 0) { + // g is not a visible face + if (!isOutsidePolytopeFace(&polytope, g, &query_point)) { + // Cannot see the neighbouring face from the new vertex. + border_edges->insert(f.edge[edge_index]); + return; + } + visible_faces->insert(g); + internal_edges->insert(f.edge[edge_index]); + for (int i = 0; i < 3; ++i) { + if (g->edge[i] != f.edge[edge_index]) { + // One of the neighbouring face is `f`, so do not need to visit again. + ComputeVisiblePatchRecursive(polytope, *g, i, query_point, border_edges, + visible_faces, internal_edges); + } + } + } else { + // Face f is visible (prerequisite), and g has been previously + // marked visible (via a different path); their shared edge should be marked + // internal. + internal_edges->insert(f.edge[edge_index]); + } +} - -/** Expands polytope's tri by new vertex v. Triangle tri is replaced by - * three triangles each with one vertex in v. */ -static int expandPolytope(ccd_pt_t *pt, ccd_pt_el_t *el, +/** Defines the "visible" patch on the given convex `polytope` (with respect to + * the given `query_vertex` which is a point outside the polytope.) + * + * A patch is characterized by: + * - a contiguous set of visible faces + * - internal edges (the edges for which *both* adjacent faces are in the + * patch) + * - border edges (edges for which only *one* adjacent face is in the patch) + * + * A face `f` (with vertices `(v₀, v₁, v₂)` and outward pointing normal `n̂`) is + * "visible" and included in the patch if, for query vertex `q`: + * + * `n̂ ⋅ (q - v₀) > 0` + * + * Namely the query vertex `q` is on the "outer" side of the face `f`. + * + * @param polytope The polytope to evaluate. + * @param f A face known to be visible to the query point. + * @param query_point The point from which visibility is evaluated. + * @param[out] border_edges The collection of patch border edges. + * @param[out] visible_faces The collection of patch faces. + * @param[out] internal_edges The collection of internal edges. + * + * @pre The `polytope` is convex. + * @pre The face `f` is visible from `query_point`. + * @pre Output parameters are non-null. + * TODO(hongkai.dai@tri.global) Replace patch computation with patch deletion + * and return border edges as an optimization. + * TODO(hongkai.dai@tri.global) Consider caching results of per-face visiblity + * status to prevent redundant recalculation -- or by associating the face + * normal with the face. + */ +static void ComputeVisiblePatch( + const ccd_pt_t& polytope, ccd_pt_face_t& f, + const ccd_vec3_t& query_point, + std::unordered_set* border_edges, + std::unordered_set* visible_faces, + std::unordered_set* internal_edges) { + assert(border_edges); + assert(visible_faces); + assert(internal_edges); + assert(border_edges->empty()); + assert(visible_faces->empty()); + assert(internal_edges->empty()); + assert(isOutsidePolytopeFace(&polytope, &f, &query_point)); + visible_faces->insert(&f); + for (int edge_index = 0; edge_index < 3; ++edge_index) { + ComputeVisiblePatchRecursive(polytope, f, edge_index, query_point, + border_edges, visible_faces, internal_edges); + } + assert(ComputeVisiblePatchRecursiveSanityCheck( + polytope, *border_edges, *visible_faces, *internal_edges)); +} + +/** Expands the polytope by adding a new vertex `newv` to the polytope. The + * new polytope is the convex hull of the new vertex together with the old + * polytope. This new polytope includes new edges (by connecting the new vertex + * with existing vertices) and new faces (by connecting the new vertex with + * existing edges). We only keep the edges and faces that are on the boundary + * of the new polytope. The edges/faces on the original polytope that would be + * interior to the new convex hull are discarded. + * @param[in/out] polytope The polytope. + * @param[in] el A feature that is visible from the point `newv` and contains + * the polytope boundary point that is closest to the origin. This feature + * should be either a face or an edge. A face is visible from a point ouside the + * original polytope, if the point is on the "outer" side of the face. An edge + * is visible from that point, if one of its neighbouring faces is visible. This + * feature contains the point that is closest to the origin on the boundary of + * the polytope. If the feature is an edge, and the two neighbouring faces of + * that edge are not co-planar, then the origin must lie on that edge. The + * feature should not be a vertex, as that would imply the two objects are in + * touching contact, causing the algorithm to exit before calling + * expandPolytope() function. + * @param[in] newv The new vertex add to the polytope. + * @retval status Returns 0 on success. Returns -2 otherwise. + */ +static int expandPolytope(ccd_pt_t *polytope, ccd_pt_el_t *el, const ccd_support_t *newv) { - ccd_pt_vertex_t *v[5]; - ccd_pt_edge_t *e[8]; - ccd_pt_face_t *f[2]; - - - // element can be either segment or triangle - if (el->type == CCD_PT_EDGE){ - // In this case, segment should be replaced by new point. - // Simpliest case is when segment stands alone and in this case - // this segment is replaced by two other segments both connected to - // newv. - // Segment can be also connected to max two faces and in that case - // each face must be replaced by two other faces. To do this - // correctly it is necessary to have correctly ordered edges and - // vertices which is exactly what is done in following code. - // - - ccdPtEdgeVertices((const ccd_pt_edge_t *)el, &v[0], &v[2]); - - ccdPtEdgeFaces((ccd_pt_edge_t *)el, &f[0], &f[1]); - - if (f[0]){ - ccdPtFaceEdges(f[0], &e[0], &e[1], &e[2]); - if (e[0] == (ccd_pt_edge_t *)el){ - e[0] = e[2]; - }else if (e[1] == (ccd_pt_edge_t *)el){ - e[1] = e[2]; - } - ccdPtEdgeVertices(e[0], &v[1], &v[3]); - if (v[1] != v[0] && v[3] != v[0]){ - e[2] = e[0]; - e[0] = e[1]; - e[1] = e[2]; - if (v[1] == v[2]) - v[1] = v[3]; - }else{ - if (v[1] == v[0]) - v[1] = v[3]; - } - - if (f[1]){ - ccdPtFaceEdges(f[1], &e[2], &e[3], &e[4]); - if (e[2] == (ccd_pt_edge_t *)el){ - e[2] = e[4]; - }else if (e[3] == (ccd_pt_edge_t *)el){ - e[3] = e[4]; - } - ccdPtEdgeVertices(e[2], &v[3], &v[4]); - if (v[3] != v[2] && v[4] != v[2]){ - e[4] = e[2]; - e[2] = e[3]; - e[3] = e[4]; - if (v[3] == v[0]) - v[3] = v[4]; - }else{ - if (v[3] == v[2]) - v[3] = v[4]; - } - } - - - v[4] = ccdPtAddVertex(pt, newv); - - ccdPtDelFace(pt, f[0]); - if (f[1]){ - ccdPtDelFace(pt, f[1]); - ccdPtDelEdge(pt, (ccd_pt_edge_t *)el); - } - - e[4] = ccdPtAddEdge(pt, v[4], v[2]); - e[5] = ccdPtAddEdge(pt, v[4], v[0]); - e[6] = ccdPtAddEdge(pt, v[4], v[1]); - if (f[1]) - e[7] = ccdPtAddEdge(pt, v[4], v[3]); - - - if (ccdPtAddFace(pt, e[1], e[4], e[6]) == NULL - || ccdPtAddFace(pt, e[0], e[6], e[5]) == NULL){ - return -2; - } - - if (f[1]){ - FCL_SUPPRESS_MAYBE_UNINITIALIZED_BEGIN - if (ccdPtAddFace(pt, e[3], e[5], e[7]) == NULL - || ccdPtAddFace(pt, e[4], e[7], e[2]) == NULL){ - return -2; - } - FCL_SUPPRESS_MAYBE_UNINITIALIZED_END - }else{ - if (ccdPtAddFace(pt, e[4], e[5], (ccd_pt_edge_t *)el) == NULL) - return -2; - } - } - }else{ // el->type == CCD_PT_FACE - // replace triangle by tetrahedron without base (base would be the - // triangle that will be removed) - - // get triplet of surrounding edges and vertices of triangle face - ccdPtFaceEdges((const ccd_pt_face_t *)el, &e[0], &e[1], &e[2]); - ccdPtEdgeVertices(e[0], &v[0], &v[1]); - ccdPtEdgeVertices(e[1], &v[2], &v[3]); - - // following code sorts edges to have e[0] between vertices 0-1, - // e[1] between 1-2 and e[2] between 2-0 - if (v[2] != v[1] && v[3] != v[1]){ - // swap e[1] and e[2] - e[3] = e[1]; - e[1] = e[2]; - e[2] = e[3]; - } - if (v[3] != v[0] && v[3] != v[1]) - v[2] = v[3]; + // The outline of the algorithm is as follows: + // 1. Compute the visible patch relative to the new vertex (See + // ComputeVisiblePatch() for details). + // 2. Delete the faces and internal edges. + // 3. Build a new face from each border edge and the new vertex. + + // To remove all faces that can be seen from the new vertex, we start with the + // face on which the closest point lives, and then do a depth-first search on + // its neighbouring triangles, until the triangle cannot be seen from the new + // vertex. + // TODO(hongkai.dai@tri.global): it is inefficient to store visible + // faces/edges. A better implementation should remove visible faces and + // internal edges inside ComputeVisiblePatch() function, when traversing the + // faces on the polytope. We focus on the correctness in the first place. + // Later when we make sure that the whole EPA implementation is bug free, we + // will improve the performance. + + ccd_pt_face_t* start_face = NULL; + // If the feature is a point, in the EPA algorithm, this means that the two + // objects are in touching contact. The EPA should terminate before calling + // this expandPolytope function, when it detects touching contact. + assert(el->type != CCD_PT_VERTEX); + // Start with the face on which the closest point lives + if (el->type == CCD_PT_FACE) { + start_face = reinterpret_cast(el); + } else if (el->type == CCD_PT_EDGE) { + // Check the two neighbouring faces of the edge. + ccd_pt_face_t* f[2]; + ccdPtEdgeFaces(reinterpret_cast(el), &f[0], &f[1]); + if (isOutsidePolytopeFace(polytope, f[0], &newv->v)) { + start_face = f[0]; + } else if (isOutsidePolytopeFace(polytope, f[1], &newv->v)) { + start_face = f[1]; + } else { + throw std::logic_error( + "FCL expandPolytope(): This should not happen. Both the nearest " + "point and the new vertex are on an edge, thus the nearest distance " + "should be 0. This is touching contact, and we should not expand the " + "polytope for touching contact."); + } + } - // remove triangle face - ccdPtDelFace(pt, (ccd_pt_face_t *)el); + std::unordered_set visible_faces; + std::unordered_set internal_edges; + std::unordered_set border_edges; + ComputeVisiblePatch(*polytope, *start_face, newv->v, &border_edges, + &visible_faces, &internal_edges); + + // Now remove all the obsolete faces. + // TODO(hongkai.dai@tri.global): currently we need to loop through each face + // in visible_faces, and then do a linear search in the list pt->faces to + // delete `face`. It would be better if we only loop through the list + // polytope->faces for once. Same for the edges. + for (const auto& f : visible_faces) { + ccdPtDelFace(polytope, f); + } - // expand triangle to tetrahedron - v[3] = ccdPtAddVertex(pt, newv); - e[3] = ccdPtAddEdge(pt, v[3], v[0]); - e[4] = ccdPtAddEdge(pt, v[3], v[1]); - e[5] = ccdPtAddEdge(pt, v[3], v[2]); + // Now remove all the obsolete edges. + for (const auto& e : internal_edges) { + ccdPtDelEdge(polytope, e); + } - if (ccdPtAddFace(pt, e[3], e[4], e[0]) == NULL - || ccdPtAddFace(pt, e[4], e[5], e[1]) == NULL - || ccdPtAddFace(pt, e[5], e[3], e[2]) == NULL){ - return -2; - } + // A vertex cannot be obsolete, since a vertex is always on the boundary of + // the Minkowski difference A ⊖ B. + // TODO(hongkai.dai@tri.global): as a sanity check, we should make sure that + // all vertices has at least one face/edge invisible from the new vertex + // `newv`. + + // Now add the new vertex. + ccd_pt_vertex_t* new_vertex = ccdPtAddVertex(polytope, newv); + + // Now add the new edges and faces, by connecting the new vertex with vertices + // on border_edges. map_vertex_to_new_edge maps a vertex on the silhouette + // edges to a new edge, with one end being the new vertex, and the other end + // being that vertex on the silhouette edges. + std::unordered_map map_vertex_to_new_edge; + for (const auto& border_edge : border_edges) { + ccd_pt_edge_t* e[2]; // The two new edges added by connecting new_vertex + // to the two vertices on border_edge. + for (int i = 0; i < 2; ++i) { + auto it = map_vertex_to_new_edge.find(border_edge->vertex[i]); + if (it == map_vertex_to_new_edge.end()) { + // This edge has not been added yet. + e[i] = ccdPtAddEdge(polytope, new_vertex, border_edge->vertex[i]); + map_vertex_to_new_edge.emplace_hint(it, border_edge->vertex[i], + e[i]); + } else { + e[i] = it->second; + } } + // Now add the face. + ccdPtAddFace(polytope, border_edge, e[0], e[1]); + } - return 0; + return 0; +} + +/** In each iteration of EPA algorithm, given the nearest point on the polytope + * boundary to the origin, a support direction will be computed, to find the + * support of the Minkowski difference A ⊖ B along that direction, so as to + * expand the polytope. + * @param polytope The polytope contained in A ⊖ B. + * @param nearest_feature The feature containing the nearest point on the + * boundary of the polytope to the origin. + * @retval dir The support direction along which to expand the polytope. Notice + * that dir is a normalized vector. + */ +static ccd_vec3_t supportEPADirection(const ccd_pt_t* polytope, + const ccd_pt_el_t* nearest_feature) { + /* + If we denote the nearest point as v, when v is not the origin, then the + support direction is v. If v is the origin, then v should be an interior + point on a face, and the support direction is the normal of that face, + pointing outward from the polytope. + */ + ccd_vec3_t dir; + if (ccdIsZero(nearest_feature->dist)) { + // nearest point is the origin. + switch (nearest_feature->type) { + case CCD_PT_VERTEX: { + throw std::logic_error( + "FCL supportEPADirection(): The nearest point to the origin is a " + "vertex of the polytope. This should be identified as a touching " + "contact, before calling function supportEPADirection()"); + } + case CCD_PT_EDGE: { + // When the nearest point is on an edge, the origin must be on that + // edge. The support direction could be in the range between + // edge.faces[0].normal and edge.faces[1].normal, where the face normals + // point outward from the polytope. In this implementation, we + // arbitrarily choose faces[0] normal. + const ccd_pt_edge_t* edge = + reinterpret_cast(nearest_feature); + dir = faceNormalPointingOutward(polytope, edge->faces[0]); + break; + } + case CCD_PT_FACE: { + // If origin is an interior point of a face, then choose the normal of + // that face as the sample direction. + const ccd_pt_face_t* face = + reinterpret_cast(nearest_feature); + dir = faceNormalPointingOutward(polytope, face); + break; + } + } + } else { + ccdVec3Copy(&dir, &(nearest_feature->witness)); + } + ccdVec3Normalize(&dir); + return dir; } /** Finds next support point (and stores it in out argument). - * Returns 0 on success, -1 otherwise */ -static int nextSupport(const void *obj1, const void *obj2, const ccd_t *ccd, - const ccd_pt_el_t *el, - ccd_support_t *out) -{ - ccd_vec3_t *a, *b, *c; - ccd_real_t dist; + * @param[in] polytope The current polytope contained inside the Minkowski + * difference A ⊖ B. + * @param[in] obj1 Geometric object A. + * @param[in] obj2 Geometric object B. + * @param[in] ccd The libccd solver. + * @param[in] el The polytope boundary feature that contains the point nearest + * to the origin. + * @param[out] out The next support point. + * @retval status If the next support point is found, then returns 0; otherwise + * returns -1. There are several reasons why the next support point is not + * found: + * 1. If the nearest point on the boundary of the polytope to the origin is a + * vertex of the polytope. Then we know the two objects A and B are in touching + * contact. + * 2. If the difference between the upper bound and lower bound of the distance + * is below sqrt(ccd->epa_tolerance), then we converge to a distance whose + * difference from the real distance is less than sqrt(ccd->epa_tolerance). + */ +static int nextSupport(const ccd_pt_t* polytope, const void* obj1, + const void* obj2, const ccd_t* ccd, + const ccd_pt_el_t* el, ccd_support_t* out) { + ccd_vec3_t *a, *b, *c; + ccd_real_t dist; - if (el->type == CCD_PT_VERTEX) - return -1; + if (el->type == CCD_PT_VERTEX) return -1; - // touch contact - if (ccdIsZero(el->dist)) - return -1; + const ccd_vec3_t dir = supportEPADirection(polytope, el); - __ccdSupport(obj1, obj2, &el->witness, ccd, out); + __ccdSupport(obj1, obj2, &dir, ccd, out); - // Compute dist of support point along element witness point direction - // so we can determine whether we expanded a polytope surrounding the - // origin a bit. - dist = ccdVec3Dot(&out->v, &el->witness); + // Compute distance of support point in the support direction, so we can + // determine whether we expanded a polytope surrounding the origin a bit. + dist = ccdVec3Dot(&out->v, &dir); - if (dist - el->dist < ccd->epa_tolerance) - return -1; + // el->dist is the squared distance from the feature "el" to the origin.. + // dist is an upper bound on the distance from the boundary of the Minkowski + // difference to the origin, and sqrt(el->dist) is a lower bound of that + // distance. + if (dist - std::sqrt(el->dist) < ccd->epa_tolerance) return -1; - if (el->type == CCD_PT_EDGE){ - // fetch end points of edge - ccdPtEdgeVec3((ccd_pt_edge_t *)el, &a, &b); + if (el->type == CCD_PT_EDGE) { + // fetch end points of edge + ccdPtEdgeVec3(reinterpret_cast(el), &a, &b); - // get distance from segment - dist = ccdVec3PointSegmentDist2(&out->v, a, b, NULL); - }else{ // el->type == CCD_PT_FACE - // fetch vertices of triangle face - ccdPtFaceVec3((ccd_pt_face_t *)el, &a, &b, &c); + // get distance from segment + dist = ccdVec3PointSegmentDist2(&out->v, a, b, NULL); + } else { // el->type == CCD_PT_FACE + // fetch vertices of triangle face + ccdPtFaceVec3(reinterpret_cast(el), &a, &b, &c); - // check if new point can significantly expand polytope - dist = ccdVec3PointTriDist2(&out->v, a, b, c, NULL); - } + // check if new point can significantly expand polytope + dist = ccdVec3PointTriDist2(&out->v, a, b, c, NULL); + } - if (dist < ccd->epa_tolerance) - return -1; + if (std::sqrt(dist) < ccd->epa_tolerance) return -1; - return 0; + return 0; } @@ -975,8 +1332,9 @@ static int __ccdEPA(const void *obj1, const void *obj2, size = ccdSimplexSize(simplex); if (size == 4){ ret = simplexToPolytope4(obj1, obj2, ccd, simplex, polytope, nearest); - }else if (size == 3){ - ret = simplexToPolytope3(obj1, obj2, ccd, simplex, polytope, nearest); + } else if (size == 3) { + ret = convert2SimplexToTetrahedron(obj1, obj2, ccd, simplex, polytope, + nearest); }else{ // size == 2 ret = simplexToPolytope2(obj1, obj2, ccd, simplex, polytope, nearest); } @@ -994,7 +1352,7 @@ static int __ccdEPA(const void *obj1, const void *obj2, *nearest = ccdPtNearest(polytope); // get next support point - if (nextSupport(obj1, obj2, ccd, *nearest, &supp) != 0) + if (nextSupport(polytope, obj1, obj2, ccd, *nearest, &supp) != 0) break; // expand nearest triangle using new point - supp @@ -1379,52 +1737,72 @@ static inline ccd_real_t _ccdDist(const void *obj1, const void *obj2, return -CCD_REAL(1.); } -static int penEPAPosCmp(const void *a, const void *b) -{ - ccd_pt_vertex_t *v1, *v2; - v1 = *(ccd_pt_vertex_t **)a; - v2 = *(ccd_pt_vertex_t **)b; - - if (ccdEq(v1->dist, v2->dist)){ - return 0; - }else if (v1->dist < v2->dist){ - return -1; - }else{ - return 1; - } -} - -static int penEPAPosClosest(const ccd_pt_t *pt, const ccd_pt_el_t *nearest, - ccd_vec3_t *p1, ccd_vec3_t* p2) -{ - FCL_UNUSED(nearest); - - ccd_pt_vertex_t *v; - ccd_pt_vertex_t **vs; - size_t i, len; - // compute median - len = 0; - ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){ - len++; - } - - vs = CCD_ALLOC_ARR(ccd_pt_vertex_t*, len); - if (vs == NULL) - return -1; - - i = 0; - ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){ - vs[i++] = v; +/** + * Given the nearest point on the polytope inside the Minkowski difference + * A ⊖ B, returns the point p1 on geometric object A and p2 on geometric object + * B, such that p1 is the deepest penetration point on A, and p2 is the deepest + * penetration point on B. + * @param[in] nearest The feature with the point that is nearest to the origin + * on the boundary of the polytope. + * @param[out] p1 the deepest penetration point on A, measured and expressed in + * the world frame. + * @param[out] p2 the deepest penetration point on B, measured and expressed in + * the world frame. + * @retval status Return 0 on success, and -1 on failure. + */ +static int penEPAPosClosest(const ccd_pt_el_t* nearest, ccd_vec3_t* p1, + ccd_vec3_t* p2) { + // We reconstruct the simplex on which the nearest point lives, and then + // compute the deepest penetration point on each geometric objects. Note that + // the reconstructed simplex has size up to 3 (at most 3 vertices). + if (nearest->type == CCD_PT_VERTEX) { + ccd_pt_vertex_t* v = (ccd_pt_vertex_t*)nearest; + ccdVec3Copy(p1, &v->v.v1); + ccdVec3Copy(p2, &v->v.v2); + return 0; + } else { + ccd_simplex_t s; + ccdSimplexInit(&s); + if (nearest->type == CCD_PT_EDGE) { + const ccd_pt_edge_t* e = reinterpret_cast(nearest); + ccdSimplexAdd(&s, &(e->vertex[0]->v)); + ccdSimplexAdd(&s, &(e->vertex[1]->v)); + } else if (nearest->type == CCD_PT_FACE) { + const ccd_pt_face_t* f = reinterpret_cast(nearest); + // The face triangle has three edges, each edge consists of two end + // points, so there are 6 end points in total, each vertex of the triangle + // appears twice among the 6 end points. We need to choose the three + // distinctive vertices out of these 6 end points. + // First we pick edge0, the two end points of edge0 are distinct. + ccdSimplexAdd(&s, &(f->edge[0]->vertex[0]->v)); + ccdSimplexAdd(&s, &(f->edge[0]->vertex[1]->v)); + // Next we pick edge1, one of the two end points on edge1 is distinct from + // the end points in edge0, we will add this distinct vertex to the + // simplex. + for (int i = 0; i < 2; ++i) { + ccd_pt_vertex_t* third_vertex = f->edge[1]->vertex[i]; + if (third_vertex != f->edge[0]->vertex[0] && + third_vertex != f->edge[0]->vertex[1]) { + ccdSimplexAdd(&s, &(third_vertex->v)); + break; + } + } + } else { + throw std::logic_error( + "FCL penEPAPosClosest(): Unsupported feature type. The closest point " + "should be either a vertex, on an edge, or on a face."); } - - qsort(vs, len, sizeof(ccd_pt_vertex_t*), penEPAPosCmp); - - ccdVec3Copy(p1, &vs[0]->v.v1); - ccdVec3Copy(p2, &vs[0]->v.v2); - - free(vs); - + // Now compute the closest point in the simplex. + // TODO(hongkai.dai@tri.global): we do not need to compute the closest point + // on the simplex, as that is already given in `nearest`. We only need to + // extract the deepest penetration points on each geometric object. + // Sean.Curtis@tri.global and I will refactor this code in the future, to + // avoid calling extractClosestPoints. + ccd_vec3_t p; + ccdVec3Copy(&p, &(nearest->witness)); + extractClosestPoints(&s, p1, p2, &p); return 0; + } } static inline ccd_real_t ccdGJKSignedDist(const void* obj1, const void* obj2, const ccd_t* ccd, ccd_vec3_t* p1, ccd_vec3_t* p2) @@ -1444,7 +1822,7 @@ static inline ccd_real_t ccdGJKSignedDist(const void* obj1, const void* obj2, co depth = -CCD_SQRT(nearest->dist); ccd_vec3_t pos1, pos2; - penEPAPosClosest(&polytope, nearest, &pos1, &pos2); + penEPAPosClosest(nearest, &pos1, &pos2); if (p1) *p1 = pos1; if (p2) *p2 = pos2; diff --git a/test/narrowphase/detail/convexity_based_algorithm/CMakeLists.txt b/test/narrowphase/detail/convexity_based_algorithm/CMakeLists.txt index 82f645866..f3a9465c3 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.cpp + test_gjk_libccd-inl_epa.cpp test_gjk_libccd-inl_extractClosestPoints.cpp + test_gjk_libccd-inl_signed_distance.cpp ) # Build all the tests diff --git a/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp deleted file mode 100644 index 6bc7bbdb3..000000000 --- a/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp +++ /dev/null @@ -1,163 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018. 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 Hongkai Dai*/ -#include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h" - -#include -#include - -#include "fcl/narrowphase/detail/gjk_solver_libccd.h" - -namespace fcl { -namespace detail { - -// Given two spheres, sphere 1 has radius1, and centered at point A, whose -// position is p_FA measured and expressed in frame F; sphere 2 has radius2, -// and centered at point B, whose position is p_FB measured and expressed in -// frame F. Computes the signed distance between the two spheres, together with -// the two closest points Na on sphere 1 and Nb on sphere 2, returns the -// position of Na and Nb expressed in frame F. -// We use the monogram notation on spatial vectors. The monogram notation is -// explained in -// http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html -template -S ComputeSphereSphereDistance(S radius1, S radius2, const Vector3& p_FA, - const Vector3& p_FB, Vector3* p_FNa, - Vector3* p_FNb) { - S min_distance = (p_FA - p_FB).norm() - radius1 - radius2; - const Vector3 p_AB_F = - p_FB - p_FA; // The vector AB measured and expressed - // in frame F. - *p_FNa = p_FA + p_AB_F.normalized() * radius1; - *p_FNb = p_FB - p_AB_F.normalized() * radius2; - return min_distance; -} - -template -void TestSphereToSphereGJKSignedDistance(S radius1, S radius2, - const Vector3& p_FA, - const Vector3& p_FB, S tol, - S min_distance_expected, - const Vector3& p_FNa_expected, - const Vector3& p_FNb_expected) { - // Test if GJKSignedDistance computes the right distance. Here we used sphere - // to sphere as the geometries. The distance between sphere and sphere should - // be computed using distance between primitives, instead of the GJK - // algorithm. But here we choose spheres for simplicity. - - fcl::Sphere s1(radius1); - fcl::Sphere s2(radius2); - fcl::Transform3 tf1, tf2; - tf1.setIdentity(); - tf2.setIdentity(); - tf1.translation() = p_FA; - tf2.translation() = p_FB; - void* o1 = GJKInitializer>::createGJKObject(s1, tf1); - void* o2 = GJKInitializer>::createGJKObject(s2, tf2); - - S dist; - Vector3 p1, p2; - GJKSolver_libccd gjkSolver; - bool res = GJKSignedDistance( - o1, detail::GJKInitializer>::getSupportFunction(), o2, - detail::GJKInitializer>::getSupportFunction(), - gjkSolver.max_distance_iterations, gjkSolver.distance_tolerance, &dist, - &p1, &p2); - - EXPECT_EQ(res, min_distance_expected >= 0); - - EXPECT_NEAR(dist, min_distance_expected, tol); - EXPECT_TRUE(p1.isApprox(p_FNa_expected, tol)); - EXPECT_TRUE(p2.isApprox(p_FNb_expected, tol)); - - GJKInitializer>::deleteGJKObject(o1); - GJKInitializer>::deleteGJKObject(o2); -} - -template -struct SphereSpecification { - SphereSpecification(S radius_, const Vector3& center_) - : radius{radius_}, center{center_} {} - S radius; - Vector3 center; -}; - -template -void TestNonCollidingSphereGJKSignedDistance(S tol) { - std::vector> spheres; - spheres.emplace_back(0.5, Vector3(0, 0, -1.2)); - spheres.emplace_back(0.5, Vector3(1.25, 0, 0)); - spheres.emplace_back(0.3, Vector3(-0.2, 0, 0)); - spheres.emplace_back(0.4, Vector3(-0.2, 0, 1.1)); - for (int i = 0; i < static_cast(spheres.size()); ++i) { - for (int j = i + 1; j < static_cast(spheres.size()); ++j) { - if ((spheres[i].center - spheres[j].center).norm() > - spheres[i].radius + spheres[j].radius) { - // Not in collision. - Vector3 p_FNa, p_FNb; - const S min_distance_expected = ComputeSphereSphereDistance( - spheres[i].radius, spheres[j].radius, spheres[i].center, - spheres[j].center, &p_FNa, &p_FNb); - TestSphereToSphereGJKSignedDistance( - spheres[i].radius, spheres[j].radius, spheres[i].center, - spheres[j].center, tol, min_distance_expected, p_FNa, p_FNb); - } else { - GTEST_FAIL() << "The two spheres collide." - << "\nSpheres[" << i << "] with radius " - << spheres[i].radius << ", centered at " - << spheres[i].center.transpose() << "\nSpheres[" << j - << "] with radius " << spheres[j].radius - << ", centered at " << spheres[j].center.transpose() - << "\n"; - } - } - } -} - -GTEST_TEST(FCL_GJKSignedDistance, sphere_sphere) { - // TODO(hongkai.dai@tri.global): By setting gjkSolver.distance_tolerance to - // the default value (1E-6), the tolerance we get on the closest points are - // only up to 1E-3. Should investigate why there is such a big difference. - TestNonCollidingSphereGJKSignedDistance(1E-3); - TestNonCollidingSphereGJKSignedDistance(1E-3); -} -} // namespace detail -} // namespace fcl - -//============================================================================== -int main(int argc, char* argv[]) { - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp new file mode 100644 index 000000000..6a9afcba0 --- /dev/null +++ b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp @@ -0,0 +1,1279 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. 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 Hongkai Dai (hongkai.dai@tri.global) */ + +/** Tests the Expanded Polytope Algorithm (EPA) implementation inside FCL. EPA + * computes the penetration depth and the points with the deepest penetration + * between two convex objects. + */ + +#include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h" + +#include + +#include + +#include "fcl/narrowphase/detail/convexity_based_algorithm/polytope.h" + +namespace fcl { +namespace detail { + +class Polytope { + public: + Polytope() { + polytope_ = new ccd_pt_t; + ccdPtInit(polytope_); + } + + ~Polytope() { + // ccdPtDestroy() destroys the vertices, edges, and faces, contained in the + // polytope, allowing the polytope itself to be subsequently deleted. + ccdPtDestroy(polytope_); + delete polytope_; + } + + ccd_pt_vertex_t& v(int i) { return *v_[i]; } + + ccd_pt_edge_t& e(int i) { return *e_[i]; } + + ccd_pt_face_t& f(int i) { return *f_[i]; } + + ccd_pt_t& polytope() { return *polytope_; } + + const ccd_pt_vertex_t& v(int i) const { return *v_[i]; } + + const ccd_pt_edge_t& e(int i) const { return *e_[i]; } + + const ccd_pt_face_t& f(int i) const { return *f_[i]; } + + const ccd_pt_t& polytope() const { return *polytope_; } + + protected: + std::vector& v() { return v_; } + std::vector& e() { return e_; } + std::vector& f() { return f_; } + + private: + std::vector v_; + std::vector e_; + std::vector f_; + ccd_pt_t* polytope_; +}; + +/** + Simple equilateral tetrahedron. + +Geometrically, its edge lengths are the given length (default to unit length). +Its "bottom" face is parallel with the z = 0 plane. It's default configuration +places the bottom face *on* the z = 0 plane with the origin contained in the +bottom face. + +In representation, the edge ordering is arbitrary (i.e., an edge can be defined +as (vᵢ, vⱼ) or (vⱼ, vᵢ). However, given an arbitrary definition of edges, the +*faces* have been defined to have a specific winding which causes e₀ × e₁ to +point inwards or outwards for that face. This allows us to explicitly fully +exercise the functionality for computing an outward normal. + - face 0: points outward + - face 1: points inward (requires flipping) + - face 2: points inward (requires flipping) + - face 3: points outward + +All property accessors are *mutable*. +*/ +class EquilateralTetrahedron : public Polytope { + public: + EquilateralTetrahedron(ccd_real_t bottom_center_x = 0, + ccd_real_t bottom_center_y = 0, + ccd_real_t bottom_center_z = 0, + ccd_real_t edge_length = 1) + : Polytope() { + v().resize(4); + e().resize(6); + f().resize(4); + auto AddTetrahedronVertex = [bottom_center_x, bottom_center_y, + bottom_center_z, edge_length, this]( + ccd_real_t x, ccd_real_t y, ccd_real_t z) { + return ccdPtAddVertexCoords( + &this->polytope(), x * edge_length + bottom_center_x, + y * edge_length + bottom_center_y, z * edge_length + bottom_center_z); + }; + v()[0] = AddTetrahedronVertex(0.5, -0.5 / std::sqrt(3), 0); + v()[1] = AddTetrahedronVertex(-0.5, -0.5 / std::sqrt(3), 0); + v()[2] = AddTetrahedronVertex(0, 1 / std::sqrt(3), 0); + v()[3] = AddTetrahedronVertex(0, 0, std::sqrt(2.0 / 3.0)); + e()[0] = ccdPtAddEdge(&polytope(), &v(0), &v(1)); + e()[1] = ccdPtAddEdge(&polytope(), &v(1), &v(2)); + e()[2] = ccdPtAddEdge(&polytope(), &v(2), &v(0)); + e()[3] = ccdPtAddEdge(&polytope(), &v(0), &v(3)); + e()[4] = ccdPtAddEdge(&polytope(), &v(1), &v(3)); + e()[5] = ccdPtAddEdge(&polytope(), &v(2), &v(3)); + f()[0] = ccdPtAddFace(&polytope(), &e(0), &e(1), &e(2)); + f()[1] = ccdPtAddFace(&polytope(), &e(0), &e(3), &e(4)); + f()[2] = ccdPtAddFace(&polytope(), &e(1), &e(4), &e(5)); + f()[3] = ccdPtAddFace(&polytope(), &e(3), &e(5), &e(2)); + } +}; + +GTEST_TEST(FCL_GJK_EPA, faceNormalPointingOutward) { + // Construct a equilateral tetrahedron, compute the normal on each face. + auto CheckTetrahedronFaceNormal = [](const EquilateralTetrahedron& p) { + for (int i = 0; i < 4; ++i) { + const ccd_vec3_t n = + libccd_extension::faceNormalPointingOutward(&p.polytope(), &p.f(i)); + for (int j = 0; j < 4; ++j) { + EXPECT_LE(ccdVec3Dot(&n, &p.v(j).v.v), + ccdVec3Dot(&n, &p.f(i).edge[0]->vertex[0]->v.v) + + constants::eps_34()); + } + } + }; + /* + p1-p4: The tetrahedron is positioned so that the origin is placed on each + face (some requiring flipping, some not) + p5: Origin is well within + p6: Origin on the bottom face, but the tetrahedron is too small; it must + evaluate all vertices and do a min/max comparison. + p7: Small tetrahedron with origin properly inside. + p8: Origin on the side face. + We do not test the case that the origin is on a vertex of the polytope. When + the origin coincides with a vertex, the two objects are touching, and we do + not need to call faceNormalPointOutward function to compute the direction + along which the polytope is expanded. + */ + EquilateralTetrahedron p1; + CheckTetrahedronFaceNormal(p1); + // Origin on the plane, and requires flipping the direction. + EquilateralTetrahedron p2(1.0 / 6, -1.0 / (6 * std::sqrt(3)), + -std::sqrt(6) / 9); + CheckTetrahedronFaceNormal(p2); + EquilateralTetrahedron p3(0, 1.0 / (3 * std::sqrt(3)), -std::sqrt(6) / 9); + CheckTetrahedronFaceNormal(p3); + EquilateralTetrahedron p4(-1.0 / 6, -1.0 / (6 * std::sqrt(3)), + -std::sqrt(6) / 9); + CheckTetrahedronFaceNormal(p4); + + // Check when the origin is within the polytope. + EquilateralTetrahedron p5(0, 0, -0.1); + CheckTetrahedronFaceNormal(p5); + + // Small tetrahedrons. + EquilateralTetrahedron p6(0, 0, 0, 0.01); + CheckTetrahedronFaceNormal(p6); + EquilateralTetrahedron p7(0, 0, -0.002, 0.01); + CheckTetrahedronFaceNormal(p7); + EquilateralTetrahedron p8(0, 0.01 / (3 * std::sqrt(3)), + -0.01 * std::sqrt(6) / 9, 0.01); + CheckTetrahedronFaceNormal(p8); +} + +GTEST_TEST(FCL_GJK_EPA, supportEPADirection) { + auto CheckSupportEPADirection = []( + const ccd_pt_t* polytope, const ccd_pt_el_t* nearest_pt, + const Eigen::Ref>& dir_expected, + ccd_real_t tol) { + const ccd_vec3_t dir = + libccd_extension::supportEPADirection(polytope, nearest_pt); + for (int i = 0; i < 3; ++i) { + EXPECT_NEAR(dir.v[i], dir_expected(i), tol); + } + }; + + // Nearest point is on the bottom triangle. + // The sampled direction should be -z unit vector. + EquilateralTetrahedron p1(0, 0, -0.1); + // The computation on Mac is very imprecise, thus the tolerance is big. + // TODO(hongkai.dai@tri.global): this tolerance should be cranked up once + // #291 is resolved. + const ccd_real_t tol = 3E-5; + CheckSupportEPADirection(&p1.polytope(), + reinterpret_cast(&p1.f(0)), + Vector3(0, 0, -1), tol); + // Nearest point is on an edge, as the origin is on an edge. + EquilateralTetrahedron p2(0, 0.5 / std::sqrt(3), 0); + // e(0) has two neighbouring faces, f(0) and f(1). The support direction could + // be the normal direction of either face. + if (p2.e(0).faces[0] == &p2.f(0)) { + // Check the support direction, should be the normal direction of f(0). + CheckSupportEPADirection(&p2.polytope(), + reinterpret_cast(&p2.e(0)), + Vector3(0, 0, -1), tol); + } else { + // The support direction should be the normal direction of f(1) + CheckSupportEPADirection( + &p2.polytope(), reinterpret_cast(&p2.e(0)), + Vector3(0, -2 * std::sqrt(2) / 3, 1.0 / 3), tol); + } + // Nearest point is on a vertex, should throw an error. + EquilateralTetrahedron p3(-0.5, 0.5 / std::sqrt(3), 0); + EXPECT_THROW( + libccd_extension::supportEPADirection( + &p3.polytope(), reinterpret_cast(&p3.v(0))), + std::logic_error); + + // Origin is an internal point of the bottom triangle + EquilateralTetrahedron p4(0, 0, 0); + CheckSupportEPADirection(&p4.polytope(), + reinterpret_cast(&p4.f(0)), + Vector3(0, 0, -1), tol); + + // Nearest point is on face(1) + EquilateralTetrahedron p5(0, 1 / (3 * std::sqrt(3)), + -std::sqrt(6) / 9 + 0.01); + CheckSupportEPADirection( + &p5.polytope(), reinterpret_cast(&p5.f(1)), + Vector3(0, -2 * std::sqrt(2) / 3, 1.0 / 3), tol); +} + +GTEST_TEST(FCL_GJK_EPA, isOutsidePolytopeFace) { + EquilateralTetrahedron p; + + auto CheckPointOutsidePolytopeFace = [&p](ccd_real_t x, ccd_real_t y, + ccd_real_t z, int face_index, + bool is_outside_expected) { + ccd_vec3_t pt; + pt.v[0] = x; + pt.v[1] = y; + pt.v[2] = z; + EXPECT_EQ(libccd_extension::isOutsidePolytopeFace(&p.polytope(), + &p.f(face_index), &pt), + is_outside_expected); + }; + + const bool expect_inside = false; + const bool expect_outside = true; + // point (0, 0, 0.1) is inside the tetrahedron. + CheckPointOutsidePolytopeFace(0, 0, 0.1, 0, expect_inside); + CheckPointOutsidePolytopeFace(0, 0, 0.1, 1, expect_inside); + CheckPointOutsidePolytopeFace(0, 0, 0.1, 2, expect_inside); + CheckPointOutsidePolytopeFace(0, 0, 0.1, 3, expect_inside); + + // point(0, 0, 2) is outside the tetrahedron. But it is on the "inner" side + // of the bottom face. + CheckPointOutsidePolytopeFace(0, 0, 2, 0, expect_inside); + CheckPointOutsidePolytopeFace(0, 0, 2, 1, expect_outside); + CheckPointOutsidePolytopeFace(0, 0, 2, 2, expect_outside); + CheckPointOutsidePolytopeFace(0, 0, 2, 3, expect_outside); + + // point (0, 0, 0) is right on the bottom face. + CheckPointOutsidePolytopeFace(0, 0, 0, 0, expect_inside); + CheckPointOutsidePolytopeFace(0, 0, 0, 1, expect_inside); + CheckPointOutsidePolytopeFace(0, 0, 0, 2, expect_inside); + CheckPointOutsidePolytopeFace(0, 0, 0, 3, expect_inside); +} + +// Construct a polytope with the following shape, namely an equilateral triangle +// on the top, and an equilateral triangle of the same size, but rotate by 60 +// degrees on the bottom. We will then connect the vertices of the equilateral +// triangles to form a convex polytope. +// v₄ +// v₃__╱╲__v₅ +// ╲╱ ╲╱ +// ╱____╲ +// v₂ ╲╱ v₀ +// v₁ +// The edges are in this order connected in a counter-clockwise order. +// e(0) connects v(0) and v(2). +// e(1) connects v(2) and v(4). +// e(2) connects v(4) and v(0). +// e(3) connects v(1) and v(3). +// e(4) connects v(3) and v(5). +// e(5) connects v(5) and v(1). +// e(6 + i) connects v(i) and v(i + 1). +// f(0) is the upper triangle. +// f(1) is the lower triangle. +// f(2 + i) is the triangle that connects v(i), v(i + 1) and v(i + 2), namely +// the triangles on the side. +// For each face, the edge e(0).cross(e(1)) has the following direction +// f(0) points inward. +// f(1) points outward. +// f(2) points inward. +// f(3) points outward. +// f(4) points inward. +// f(5) points outward. +// f(6) points inward +// f(7) points outward. +class Hexagram : public Polytope { + public: + Hexagram(ccd_real_t bottom_center_x = 0, ccd_real_t bottom_center_y = 0, + ccd_real_t bottom_center_z = 0) + : Polytope() { + v().resize(6); + e().resize(12); + f().resize(8); + auto AddHexagramVertex = [bottom_center_x, bottom_center_y, bottom_center_z, + this](ccd_real_t x, ccd_real_t y, ccd_real_t z) { + return ccdPtAddVertexCoords(&this->polytope(), x + bottom_center_x, + y + bottom_center_y, z + bottom_center_z); + }; + // right corner of upper triangle + v()[0] = AddHexagramVertex(0.5, -1 / std::sqrt(3), 1); + // bottom corner of lower triangle + v()[1] = AddHexagramVertex(0, -2 / std::sqrt(3), 0); + // left corner of upper triangle + v()[2] = AddHexagramVertex(-0.5, -1 / std::sqrt(3), 1); + // left corner of lower triangle + v()[3] = AddHexagramVertex(-0.5, 1 / std::sqrt(3), 0); + // top corner of upper triangle + v()[4] = AddHexagramVertex(0, 2 / std::sqrt(3), 1); + // right corner of lower triangle + v()[5] = AddHexagramVertex(0.5, 1 / std::sqrt(3), 0); + + // edges on the upper triangle + e()[0] = ccdPtAddEdge(&polytope(), &v(0), &v(2)); + e()[1] = ccdPtAddEdge(&polytope(), &v(2), &v(4)); + e()[2] = ccdPtAddEdge(&polytope(), &v(4), &v(0)); + // edges on the lower triangle + e()[3] = ccdPtAddEdge(&polytope(), &v(1), &v(3)); + e()[4] = ccdPtAddEdge(&polytope(), &v(3), &v(5)); + e()[5] = ccdPtAddEdge(&polytope(), &v(5), &v(1)); + // edges connecting the upper triangle to the lower triangle + for (int i = 0; i < 6; ++i) { + e()[6 + i] = ccdPtAddEdge(&polytope(), &v(i), &v((i + 1) % 6)); + } + + // upper triangle + f()[0] = ccdPtAddFace(&polytope(), &e(0), &e(1), &e(2)); + // lower triangle + f()[1] = ccdPtAddFace(&polytope(), &e(3), &e(4), &e(5)); + // triangles on the side + f()[2] = ccdPtAddFace(&polytope(), &e(0), &e(7), &e(6)); + f()[3] = ccdPtAddFace(&polytope(), &e(7), &e(8), &e(3)); + f()[4] = ccdPtAddFace(&polytope(), &e(8), &e(9), &e(1)); + f()[5] = ccdPtAddFace(&polytope(), &e(9), &e(10), &e(4)); + f()[6] = ccdPtAddFace(&polytope(), &e(10), &e(11), &e(2)); + f()[7] = ccdPtAddFace(&polytope(), &e(11), &e(6), &e(5)); + } +}; + +template +bool IsElementInSet(const std::unordered_set& S, const T* element) { + return S.count(const_cast(element)) > 0; +} + +// @param border_edge_indices_expected +// polytope.e(border_edge_indices_expected(i)) is a border edge. Similarly for +// visible_face_indices_expected and internal_edges_indices_expected. +void CheckComputeVisiblePatchCommon( + const Polytope& polytope, + const std::unordered_set& border_edges, + const std::unordered_set& visible_faces, + const std::unordered_set internal_edges, + const std::unordered_set& border_edge_indices_expected, + const std::unordered_set& visible_face_indices_expected, + const std::unordered_set internal_edges_indices_expected) { + // Check border_edges + EXPECT_EQ(border_edges.size(), border_edge_indices_expected.size()); + for (const int edge_index : border_edge_indices_expected) { + EXPECT_TRUE(IsElementInSet(border_edges, &polytope.e(edge_index))); + } + // Check visible_faces + EXPECT_EQ(visible_faces.size(), visible_face_indices_expected.size()); + for (const int face_index : visible_face_indices_expected) { + EXPECT_TRUE(IsElementInSet(visible_faces, &polytope.f(face_index))); + } + // Check internal_edges + EXPECT_EQ(internal_edges.size(), internal_edges_indices_expected.size()); + for (const auto edge_index : internal_edges_indices_expected) { + EXPECT_TRUE(IsElementInSet(internal_edges, &polytope.e(edge_index))); + } +} + +// @param edge_indices we will call ComputeVisiblePatchRecursive(polytope, face, +// edge_index, new_vertex, ...) for each edge_index in edge_indices. Namely we +// will compute the visible patches, starting from face.e(edge_index). +void CheckComputeVisiblePatchRecursive( + const Polytope& polytope, ccd_pt_face_t& face, + const std::vector& edge_indices, const ccd_vec3_t& new_vertex, + const std::unordered_set& border_edge_indices_expected, + const std::unordered_set& visible_face_indices_expected, + const std::unordered_set& internal_edges_indices_expected) { + std::unordered_set border_edges; + std::unordered_set visible_faces; + visible_faces.insert(&face); + std::unordered_set internal_edges; + for (const int edge_index : edge_indices) { + libccd_extension::ComputeVisiblePatchRecursive( + polytope.polytope(), face, edge_index, new_vertex, &border_edges, + &visible_faces, &internal_edges); + } + CheckComputeVisiblePatchCommon(polytope, border_edges, visible_faces, + internal_edges, border_edge_indices_expected, + visible_face_indices_expected, + internal_edges_indices_expected); +} + +void CheckComputeVisiblePatch( + const Polytope& polytope, ccd_pt_face_t& face, const ccd_vec3_t& new_vertex, + const std::unordered_set& border_edge_indices_expected, + const std::unordered_set& visible_face_indices_expected, + const std::unordered_set& internal_edges_indices_expected) { + std::unordered_set border_edges; + std::unordered_set visible_faces; + std::unordered_set internal_edges; + libccd_extension::ComputeVisiblePatch(polytope.polytope(), face, new_vertex, + &border_edges, &visible_faces, + &internal_edges); + + CheckComputeVisiblePatchCommon(polytope, border_edges, visible_faces, + internal_edges, border_edge_indices_expected, + visible_face_indices_expected, + internal_edges_indices_expected); +} + +GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatch_TopFaceVisible) { + // 1 visible face. + Hexagram hex; + // Point P is just slightly above the top triangle. Only the top triangle can + // be seen from point P. + ccd_vec3_t p; + p.v[0] = 0; + p.v[1] = 0; + p.v[2] = 1.1; + const std::unordered_set empty_set; + // Test recursive implementation. + CheckComputeVisiblePatchRecursive(hex, hex.f(0), {0}, p, {0}, {0}, empty_set); + + // Test ComputeVisiblePatch. + CheckComputeVisiblePatch(hex, hex.f(0), p, {0, 1, 2}, {0}, empty_set); +} + +GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatch_4FacesVisible) { + // 4 visible faces. + Hexagram hex; + // Point P is just above the top triangle by a certain height, such that it + // can see the triangles on the side, which connects two vertices on the upper + // triangle, and one vertex on the lower triangle. + ccd_vec3_t p; + p.v[0] = 0; + p.v[1] = 0; + p.v[2] = 2.1; + // Test recursive implementation. + CheckComputeVisiblePatchRecursive(hex, hex.f(0), {0}, p, {6, 7}, {0, 2}, {0}); + + // Test ComputeVisiblePatch. + CheckComputeVisiblePatch(hex, hex.f(0), p, {6, 7, 8, 9, 10, 11}, {0, 2, 4, 6}, + {0, 1, 2}); +} + +GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatch_TopAndSideFacesVisible) { + // 2 visible faces. + Hexagram hex; + // Point P is just outside the upper triangle (face0) and the triangle face2, + // it can see both face0 and face2, but not the other triangles. + ccd_vec3_t p; + p.v[0] = 0; + p.v[1] = -1 / std::sqrt(3) - 0.1; + p.v[2] = 1.1; + CheckComputeVisiblePatchRecursive(hex, hex.f(0), {0}, p, {6, 7}, {0, 2}, {0}); + + CheckComputeVisiblePatch(hex, hex.f(0), p, {1, 2, 6, 7}, {0, 2}, {0}); +} + +GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatch_2FacesVisible) { + // Test with the equilateral tetrahedron. + // Point P is outside of an edge on the bottom triangle. It can see both faces + // neighbouring that edge. + + EquilateralTetrahedron tetrahedron(0, 0, -0.1); + ccd_vec3_t p; + p.v[0] = 0; + p.v[1] = -1 / std::sqrt(3) - 0.1; + p.v[2] = -0.2; + + // Start with from face 0. + CheckComputeVisiblePatch(tetrahedron, tetrahedron.f(0), p, {1, 2, 3, 4}, + {0, 1}, {0}); + + // Start with from face 1. + CheckComputeVisiblePatch(tetrahedron, tetrahedron.f(1), p, {1, 2, 3, 4}, + {0, 1}, {0}); +} + +// Returns true if the the distance difference between the two vertices are +// below tol. +bool VertexPositionCoincide(const ccd_pt_vertex_t* v1, + const ccd_pt_vertex_t* v2, ccd_real_t tol) { + return ccdVec3Dist2(&v1->v.v, &v2->v.v) < tol * tol; +} + +// Return true, if the vertices in e1 are all mapped to the vertices in e2, +// according to the mapping @p map_v1_to_v2. +bool EdgeMatch(const ccd_pt_edge_t* e1, const ccd_pt_edge_t* e2, + const std::unordered_map& + map_v1_to_v2) { + ccd_pt_vertex_t* v2_expected[2]; + for (int i = 0; i < 2; ++i) { + auto it = map_v1_to_v2.find(e1->vertex[i]); + if (it == map_v1_to_v2.end()) { + throw std::logic_error("vertex[" + std::to_string(i) + + "] in e1 is not found in map_v1_to_v2"); + } + v2_expected[i] = it->second; + } + return (v2_expected[0] == e2->vertex[0] && v2_expected[1] == e2->vertex[1]) || + (v2_expected[0] == e2->vertex[1] && v2_expected[1] == e2->vertex[0]); +} + +// Return true, if the edges in f1 are all mapped to the edges in f2, according +// to the mapping @p map_e1_to_e2. +bool TriangleMatch( + const ccd_pt_face_t* f1, const ccd_pt_face_t* f2, + const std::unordered_map& map_e1_to_e2) { + std::unordered_set e2_expected; + for (int i = 0; i < 3; ++i) { + auto it = map_e1_to_e2.find(f1->edge[i]); + if (it == map_e1_to_e2.end()) { + throw std::logic_error("edge[" + std::to_string(i) + + "] in f1 is not found in map_e1_to_e2"); + } + e2_expected.insert(it->second); + } + // The edges in f1 have to be distinct. + EXPECT_EQ(e2_expected.size(), 3u); + for (int i = 0; i < 3; ++i) { + auto it = e2_expected.find(f2->edge[i]); + if (it == e2_expected.end()) { + return false; + } + } + return true; +} + +// Construct the mapping from feature1_list to feature2_list. There should be a +// one-to-one correspondence between feature1_list and feature2_list. +// @param feature1_list[in] A list of features to be mapped from. +// @param feature2_list[in] A list of features to be mapped to. +// @param cmp_feature[in] Returns true if two features are identical, otherwise +// returns false. +// @param feature1[out] The set of features in feature1_list. +// @param feature2[out] The set of features in feature2_list. +// @param map_feature1_to_feature2[out] Maps a feature in feature1_list to +// a feature in feature2_list. +// @note The features in feature1_list should be unique, so are in +// feature2_list. +template +void MapFeature1ToFeature2( + const ccd_list_t* feature1_list, const ccd_list_t* feature2_list, + std::function cmp_feature, + std::unordered_set* feature1, std::unordered_set* feature2, + std::unordered_map* map_feature1_to_feature2) { + feature1->clear(); + feature2->clear(); + map_feature1_to_feature2->clear(); + T* f; + ccdListForEachEntry(feature1_list, f, T, list) { + auto it = feature1->find(f); + assert(it == feature1->end()); + feature1->emplace_hint(it, f); + } + ccdListForEachEntry(feature2_list, f, T, list) { + auto it = feature2->find(f); + assert(it == feature2->end()); + feature2->emplace_hint(it, f); + } + EXPECT_EQ(feature1->size(), feature2->size()); + for (const auto& f1 : *feature1) { + bool found_match = false; + for (const auto& f2 : *feature2) { + if (cmp_feature(f1, f2)) { + if (!found_match) { + map_feature1_to_feature2->emplace_hint( + map_feature1_to_feature2->end(), f1, f2); + found_match = true; + } else { + GTEST_FAIL() << "There should be only one element in feature2_list " + "that matches with an element in feature1_list."; + } + } + } + EXPECT_TRUE(found_match); + } + // Every feature in feature1_list should be matched to a feature in + // feature2_list. + EXPECT_EQ(map_feature1_to_feature2->size(), feature1->size()); +} + +void ComparePolytope(const ccd_pt_t* polytope1, const ccd_pt_t* polytope2, + ccd_real_t tol) { + // Build the mapping between the vertices in polytope1 to the vertices in + // polytope2. + std::unordered_set v1_set, v2_set; + std::unordered_map map_v1_to_v2; + MapFeature1ToFeature2( + &polytope1->vertices, &polytope2->vertices, + [tol](const ccd_pt_vertex_t* v1, const ccd_pt_vertex_t* v2) { + return VertexPositionCoincide(v1, v2, tol); + }, + &v1_set, &v2_set, &map_v1_to_v2); + + // Build the mapping between the edges in polytope1 to the edges in polytope2. + std::unordered_set e1_set, e2_set; + std::unordered_map map_e1_to_e2; + MapFeature1ToFeature2( + &polytope1->edges, &polytope2->edges, + [map_v1_to_v2](const ccd_pt_edge_t* e1, const ccd_pt_edge_t* e2) { + return EdgeMatch(e1, e2, map_v1_to_v2); + }, + &e1_set, &e2_set, &map_e1_to_e2); + + // Build the mapping between the faces in polytope1 to the faces in polytope2. + std::unordered_set f1_set, f2_set; + std::unordered_map map_f1_to_f2; + MapFeature1ToFeature2( + &polytope1->faces, &polytope2->faces, + [map_e1_to_e2](const ccd_pt_face_t* f1, const ccd_pt_face_t* f2) { + return TriangleMatch(f1, f2, map_e1_to_e2); + }, + &f1_set, &f2_set, &map_f1_to_f2); + + /* TODO(hongkai.dai@tri.global): enable the following check, when issue + https://github.com/danfis/libccd/issues/46 has been fixed. Currently + ccd_pt_vertex_t.edges are garbage. + // Now make sure that the edges connected to a vertex in polytope 1, are the + // same edges connected to the corresponding vertex in polytope 2. + for (const auto& v1 : v1_set) { + auto v2 = map_v1_to_v2[v1]; + std::unordered_set v1_edges, v2_edges; + ccd_pt_edge_t* e; + ccdListForEachEntry(&v1->edges, e, ccd_pt_edge_t, list) { + v1_edges.insert(e); + } + ccdListForEachEntry(&v2->edges, e, ccd_pt_edge_t, list) { + v2_edges.insert(e); + } + EXPECT_EQ(v1_edges.size(), v2_edges.size()); + // Now check for each edge connecting to v1, the corresponding edge is + // connected to v2. + for (const auto& v1_e : v1_edges) { + auto it = map_e1_to_e2.find(v1_e); + EXPECT_NE(it, map_e1_to_e2.end()) { + auto v2_e = it->second; + EXPECT_NE(v2_edges.find(v2_e), v2_edges.end()); + } + }*/ + + // Make sure that the faces connected to each edge in polytope 1, are the same + // face connected to the corresponding face in polytope 2. + for (const auto& e1 : e1_set) { + auto e2 = map_e1_to_e2[e1]; + ccd_pt_face_t* f2_expected[2]; + for (int i = 0; i < 2; ++i) { + f2_expected[i] = map_f1_to_f2[e1->faces[i]]; + } + EXPECT_TRUE( + (f2_expected[0] == e2->faces[0] && f2_expected[1] == e2->faces[1]) || + (f2_expected[0] == e2->faces[1] && f2_expected[1] == e2->faces[0])); + } +} + +GTEST_TEST(FCL_GJK_EPA, expandPolytope_tetrahedron1) { + // Expand the equilateral tetrahedron by adding a point just outside one of + // the triangle face. That nearest triangle face will be deleted, and the + // three new faces will be added, by connecting the new vertex with the three + // vertices on the removed face. + EquilateralTetrahedron polytope(0, 0, -0.1); + // nearest point is on the bottom triangle + ccd_support_t newv; + newv.v.v[0] = 0; + newv.v.v[1] = 0; + newv.v.v[2] = -0.2; + + const int result = libccd_extension::expandPolytope( + &polytope.polytope(), reinterpret_cast(&polytope.f(0)), + &newv); + EXPECT_EQ(result, 0); + + // Construct the expanded polytope manually. + EquilateralTetrahedron tetrahedron(0, 0, -0.1); + ccd_pt_t& polytope_expected = tetrahedron.polytope(); + // The bottom face is removed. + ccdPtDelFace(&polytope_expected, &tetrahedron.f(0)); + // Insert the vertex. + ccd_pt_vertex_t* new_vertex = + ccdPtAddVertexCoords(&polytope_expected, 0, 0, -0.2); + // Add new edges. + ccd_pt_edge_t* new_edges[3]; + for (int i = 0; i < 3; ++i) { + new_edges[i] = + ccdPtAddEdge(&polytope_expected, new_vertex, &tetrahedron.v(i)); + } + // Add new faces. + ccdPtAddFace(&polytope_expected, &tetrahedron.e(0), new_edges[0], + new_edges[1]); + ccdPtAddFace(&polytope_expected, &tetrahedron.e(1), new_edges[1], + new_edges[2]); + ccdPtAddFace(&polytope_expected, &tetrahedron.e(2), new_edges[2], + new_edges[0]); + + ComparePolytope(&polytope.polytope(), &polytope_expected, + constants::eps_34()); +} + +GTEST_TEST(FCL_GJK_EPA, expandPolytope_tetrahedron_2visible_faces) { + // Expand the equilateral tetrahedron by adding a point just outside one edge. + // The two neighbouring faces of that edge will be deleted. Four new faces + // will be added, by connecting the new vertex with the remaining vertex on + // the two removed faces, that is opposite to the removed edge. + EquilateralTetrahedron polytope(0, 0, -0.1); + // nearest point is on the bottom triangle + ccd_support_t newv; + newv.v.v[0] = 0; + newv.v.v[1] = -0.5 / std::sqrt(3) - 0.1; + newv.v.v[2] = -0.2; + + const int result = libccd_extension::expandPolytope( + &polytope.polytope(), reinterpret_cast(&polytope.e(0)), + &newv); + EXPECT_EQ(result, 0); + + // Construct the expanded polytope manually. + EquilateralTetrahedron tetrahedron(0, 0, -0.1); + ccd_pt_t& polytope_expected = tetrahedron.polytope(); + // The bottom face is removed. + ccdPtDelFace(&polytope_expected, &tetrahedron.f(0)); + // The other face that neighbours with f(0) is removed. + ccdPtDelFace(&polytope_expected, &tetrahedron.f(1)); + // The nearest edge is removed. + ccdPtDelEdge(&polytope_expected, &tetrahedron.e(0)); + // Insert the vertex. + ccd_pt_vertex_t* new_vertex = ccdPtAddVertexCoords( + &polytope_expected, newv.v.v[0], newv.v.v[1], newv.v.v[2]); + // Add new edges. + ccd_pt_edge_t* new_edges[4]; + new_edges[0] = + ccdPtAddEdge(&polytope_expected, new_vertex, &tetrahedron.v(0)); + new_edges[1] = + ccdPtAddEdge(&polytope_expected, new_vertex, &tetrahedron.v(1)); + new_edges[2] = + ccdPtAddEdge(&polytope_expected, new_vertex, &tetrahedron.v(2)); + new_edges[3] = + ccdPtAddEdge(&polytope_expected, new_vertex, &tetrahedron.v(3)); + // Add new faces. + ccdPtAddFace(&polytope_expected, &tetrahedron.e(3), new_edges[0], + new_edges[3]); + ccdPtAddFace(&polytope_expected, &tetrahedron.e(2), new_edges[0], + new_edges[2]); + ccdPtAddFace(&polytope_expected, &tetrahedron.e(4), new_edges[1], + new_edges[3]); + ccdPtAddFace(&polytope_expected, &tetrahedron.e(1), new_edges[1], + new_edges[2]); + + ComparePolytope(&polytope.polytope(), &polytope_expected, + constants::eps_34()); +} + +GTEST_TEST(FCL_GJK_EPA, expandPolytope_hexagram_1visible_face) { + // Expand the Hexagram by adding a point just above the upper triangle. + // The upper triangle will be deleted. Three new faces will be added, by + // connecting the new vertex with the three vertices of the removed triangle. + Hexagram hex(0, 0, -0.9); + // nearest point is on the top triangle + ccd_support_t newv; + newv.v.v[0] = 0; + newv.v.v[1] = 0; + newv.v.v[2] = 0.2; + + const int result = libccd_extension::expandPolytope( + &hex.polytope(), reinterpret_cast(&hex.f(0)), &newv); + EXPECT_EQ(result, 0); + + // Construct the expanded polytope manually. + Hexagram hex_duplicate(0, 0, -0.9); + ccd_pt_t& polytope_expected = hex_duplicate.polytope(); + // Remove the upper triangle. + ccdPtDelFace(&polytope_expected, &hex_duplicate.f(0)); + + // Add the new vertex. + ccd_pt_vertex_t* new_vertex = ccdPtAddVertexCoords( + &polytope_expected, newv.v.v[0], newv.v.v[1], newv.v.v[2]); + // Add the new edges. + ccd_pt_edge_t* new_edges[3]; + new_edges[0] = + ccdPtAddEdge(&polytope_expected, new_vertex, &hex_duplicate.v(0)); + new_edges[1] = + ccdPtAddEdge(&polytope_expected, new_vertex, &hex_duplicate.v(2)); + new_edges[2] = + ccdPtAddEdge(&polytope_expected, new_vertex, &hex_duplicate.v(4)); + // Add the new faces. + ccdPtAddFace(&polytope_expected, new_edges[0], new_edges[1], + &hex_duplicate.e(0)); + ccdPtAddFace(&polytope_expected, new_edges[1], new_edges[2], + &hex_duplicate.e(1)); + ccdPtAddFace(&polytope_expected, new_edges[2], new_edges[0], + &hex_duplicate.e(2)); + + ComparePolytope(&hex.polytope(), &polytope_expected, + constants::eps_34()); +} + +GTEST_TEST(FCL_GJK_EPA, expandPolytope_hexagram_4_visible_faces) { + // Expand the Hexagram by adding a point above the upper triangle by a certain + // height, such that the new vertex can see the upper triangle, together with + // the three triangles on the side of the hexagram. All these four triangles + // will be removed. 6 new faces will be added by connecting the new vertex, + // with eah vertex in the old hexagram. + Hexagram hex(0, 0, -0.9); + // nearest point is on the top triangle + ccd_support_t newv; + newv.v.v[0] = 0; + newv.v.v[1] = 0; + newv.v.v[2] = 1.2; + + const int result = libccd_extension::expandPolytope( + &hex.polytope(), reinterpret_cast(&hex.f(0)), &newv); + EXPECT_EQ(result, 0); + + // Construct the expanded polytope manually. + Hexagram hex_duplicate(0, 0, -0.9); + ccd_pt_t& polytope_expected = hex_duplicate.polytope(); + // Remove the upper triangle. + ccdPtDelFace(&polytope_expected, &hex_duplicate.f(0)); + // Remove the triangles on the side, which consists of two vertices on the + // upper triangle, and one vertex on the lower triangle. + ccdPtDelFace(&polytope_expected, &hex_duplicate.f(2)); + ccdPtDelFace(&polytope_expected, &hex_duplicate.f(4)); + ccdPtDelFace(&polytope_expected, &hex_duplicate.f(6)); + // Remove the edges of the upper triangle. + ccdPtDelEdge(&polytope_expected, &hex_duplicate.e(0)); + ccdPtDelEdge(&polytope_expected, &hex_duplicate.e(1)); + ccdPtDelEdge(&polytope_expected, &hex_duplicate.e(2)); + + // Add the new vertex. + ccd_pt_vertex_t* new_vertex = ccdPtAddVertexCoords( + &polytope_expected, newv.v.v[0], newv.v.v[1], newv.v.v[2]); + // Add the new edges. + ccd_pt_edge_t* new_edges[6]; + for (int i = 0; i < 6; ++i) { + new_edges[i] = + ccdPtAddEdge(&polytope_expected, new_vertex, &hex_duplicate.v(i)); + } + // Add the new faces. + for (int i = 0; i < 6; ++i) { + ccdPtAddFace(&polytope_expected, new_edges[i % 6], new_edges[(i + 1) % 6], + &hex_duplicate.e(i + 6)); + } + ComparePolytope(&hex.polytope(), &polytope_expected, + constants::eps_34()); +} + +void CompareCcdVec3(const ccd_vec3_t& v, const ccd_vec3_t& v_expected, + ccd_real_t tol) { + for (int i = 0; i < 3; ++i) { + EXPECT_NEAR(v.v[i], v_expected.v[i], tol); + } +} + +GTEST_TEST(FCL_GJK_EPA, penEPAPosClosest_vertex) { + // The nearest point is a vertex on the polytope. + // tetrahedron.v(0) is the origin. + EquilateralTetrahedron tetrahedron(-0.5, 0.5 / std::sqrt(3), 0); + // Make sure that v1 - v2 = v. + tetrahedron.v(0).v.v1.v[0] = 1; + tetrahedron.v(0).v.v1.v[1] = 2; + tetrahedron.v(0).v.v1.v[2] = 3; + for (int i = 0; i < 3; ++i) { + tetrahedron.v(0).v.v2.v[i] = tetrahedron.v(0).v.v1.v[i]; + } + ccd_vec3_t p1, p2; + EXPECT_EQ( + libccd_extension::penEPAPosClosest( + reinterpret_cast(&tetrahedron.v(0)), &p1, &p2), + 0); + CompareCcdVec3(p1, tetrahedron.v(0).v.v1, constants::eps_78()); + CompareCcdVec3(p2, tetrahedron.v(0).v.v2, constants::eps_78()); +} + +GTEST_TEST(FCL_GJK_EPA, penEPAPosClosest_edge) { + // The nearest point is on an edge of the polytope. + // tetrahedron.e(1) contains the origin. + EquilateralTetrahedron tetrahedron(0.25, -0.25 / std::sqrt(3), 0); + // e(1) connects two vertices v(1) and v(2), make sure that v(1).v1 - v(1).v2 + // = v(1).v, also v(2).v1 - v(2).v2 = v(2).v + ccdVec3Set(&tetrahedron.v(1).v.v1, 1, 2, 3); + ccdVec3Set(&tetrahedron.v(2).v.v1, 4, 5, 6); + for (int i = 1; i <= 2; ++i) { + for (int j = 0; j < 3; ++j) { + tetrahedron.v(i).v.v2.v[j] = + tetrahedron.v(i).v.v1.v[j] - tetrahedron.v(i).v.v.v[j]; + } + } + // Notice that origin = 0.5*v(1).v + 0.5*v(2).v + // So p1 = 0.5*v(1).v1 + 0.5*v(2).v1 + // p2 = 0.5*v(1).v2 + 0.5*v(2).v2 + ccd_vec3_t p1, p2; + EXPECT_EQ( + libccd_extension::penEPAPosClosest( + reinterpret_cast(&tetrahedron.e(1)), &p1, &p2), + 0); + ccd_vec3_t p1_expected, p2_expected; + ccdVec3Copy(&p1_expected, &tetrahedron.v(1).v.v1); + ccdVec3Add(&p1_expected, &tetrahedron.v(2).v.v1); + ccdVec3Scale(&p1_expected, ccd_real_t(0.5)); + ccdVec3Copy(&p2_expected, &tetrahedron.v(1).v.v2); + ccdVec3Add(&p2_expected, &tetrahedron.v(2).v.v2); + ccdVec3Scale(&p2_expected, ccd_real_t(0.5)); + + CompareCcdVec3(p1, p1_expected, constants::eps_78()); + CompareCcdVec3(p2, p2_expected, constants::eps_78()); +} + +GTEST_TEST(FCL_GJK_EPA, penEPAPosClosest_face) { + // The nearest point is on a face of the polytope, It is the center of + // tetrahedron.f(1). + const Vector3 bottom_center_pos = + Vector3(0, 1.0 / (3 * std::sqrt(3)), -std::sqrt(6) / 9) + + 0.01 * Vector3(0, -2 * std::sqrt(2) / 3, 1.0 / 3); + EquilateralTetrahedron tetrahedron(bottom_center_pos(0), bottom_center_pos(1), + bottom_center_pos(2)); + // Assign v(i).v1 and v(i).v2 for i = 0, 1, 3, such that + // v(i).v = v(i).v1 - v(i).v2 + tetrahedron.v(0).v.v1.v[0] = 1; + tetrahedron.v(0).v.v1.v[1] = 2; + tetrahedron.v(0).v.v1.v[2] = 3; + tetrahedron.v(1).v.v1.v[0] = 4; + tetrahedron.v(1).v.v1.v[1] = 5; + tetrahedron.v(1).v.v1.v[2] = 6; + tetrahedron.v(3).v.v1.v[0] = 7; + tetrahedron.v(3).v.v1.v[1] = 8; + tetrahedron.v(3).v.v1.v[2] = 9; + for (int i : {0, 1, 3}) { + for (int j = 0; j < 3; ++j) { + tetrahedron.v(i).v.v2.v[j] = + tetrahedron.v(i).v.v1.v[j] - tetrahedron.v(i).v.v.v[j]; + } + } + + ccd_vec3_t p1, p2; + EXPECT_EQ( + libccd_extension::penEPAPosClosest( + reinterpret_cast(&tetrahedron.f(1)), &p1, &p2), + 0); + + // Notice that the nearest point = 1/3 * v(0).v + 1/3 * v(1).v + 1/3 * v(3).v + // So p1 = 1/3 * (v(0).v1 + v(1).v1 + v(3).v1) + // p2 = 1/3 * (v(0).v2 + v(1).v2 + v(3).v2) + ccd_vec3_t p1_expected, p2_expected; + ccdVec3Copy(&p1_expected, &tetrahedron.v(0).v.v1); + ccdVec3Add(&p1_expected, &tetrahedron.v(1).v.v1); + ccdVec3Add(&p1_expected, &tetrahedron.v(3).v.v1); + ccdVec3Scale(&p1_expected, ccd_real_t(1.0 / 3)); + ccdVec3Copy(&p2_expected, &tetrahedron.v(0).v.v2); + ccdVec3Add(&p2_expected, &tetrahedron.v(1).v.v2); + ccdVec3Add(&p2_expected, &tetrahedron.v(3).v.v2); + ccdVec3Scale(&p2_expected, ccd_real_t(1.0 / 3)); + + CompareCcdVec3(p1, p1_expected, constants::eps_78()); + CompareCcdVec3(p2, p2_expected, constants::eps_78()); +} + +// Test convert2SimplexToTetrahedron function. +// We construct a test scenario that two boxes are on the xy plane of frame F. +// The two boxes penetrate to each other, as shown in the bottom plot. +// y +// ┏━━━│━━━┓Box1 +// ┏┃━━┓ ┃ +// ───┃┃──┃O───┃─────x +// box2┗┃━━┛│ ┃ +// ┗━━━│━━━┛ +// +// @param[in] X_WF The pose of the frame F measured and expressed in the world +// frame W. +// @param[out] o1 Box1 +// @param[out] o2 Box2 +// @param[out] ccd The ccd solver info. +// @param[out] X_FB1 The pose of box 1 frame measured and expressed in frame F. +// @param[out] X_FB2 The pose of box 2 frame measured and expressed in frame F. +template +void SetUpBoxToBox(const Transform3& X_WF, void** o1, void** o2, ccd_t* ccd, + fcl::Transform3* X_FB1, fcl::Transform3* X_FB2) { + const fcl::Vector3 box1_size(2, 2, 2); + const fcl::Vector3 box2_size(1, 1, 2); + // Box 1 is fixed. + X_FB1->setIdentity(); + X_FB1->translation() << 0, 0, 1; + X_FB2->setIdentity(); + X_FB2->translation() << -0.6, 0, 1; + + const fcl::Transform3 X_WB1 = X_WF * (*X_FB1); + const fcl::Transform3 X_WB2 = X_WF * (*X_FB2); + fcl::Box box1(box1_size); + fcl::Box box2(box2_size); + *o1 = GJKInitializer>::createGJKObject(box1, X_WB1); + *o2 = GJKInitializer>::createGJKObject(box1, X_WB2); + + // Set up ccd solver. + CCD_INIT(ccd); + ccd->support1 = detail::GJKInitializer>::getSupportFunction(); + ccd->support2 = detail::GJKInitializer>::getSupportFunction(); + ccd->max_iterations = 1000; + ccd->dist_tolerance = 1E-6; +} + +template +Vector3 ToEigenVector(const ccd_vec3_t& v) { + return (Vector3() << v.v[0], v.v[1], v.v[2]).finished(); +} + +template +ccd_vec3_t ToCcdVec3(const Eigen::Ref>& v) { + ccd_vec3_t u; + u.v[0] = v(0); + u.v[1] = v(1); + u.v[2] = v(2); + return u; +} + +template +void TestSimplexToPolytope3InGivenFrame(const Transform3& X_WF) { + void* o1 = NULL; + void* o2 = NULL; + ccd_t ccd; + fcl::Transform3 X_FB1, X_FB2; + SetUpBoxToBox(X_WF, &o1, &o2, &ccd, &X_FB1, &X_FB2); + + // Construct a 2-simplex that contains the origin. The vertices of this + // 2-simplex are on the boundary of the Minkowski difference. + ccd_simplex_t simplex; + ccdSimplexInit(&simplex); + ccd_support_t pts[3]; + // We find three points Pa1, Pb1, Pc1 on box 1, and three points Pa2, Pb2, Pc2 + // on box 2, such that the 2-simplex with vertices (Pa1 - Pa2, Pb1 - Pb2, + // Pc1 - Pc2) contains the origin. + const Vector3 p_FPa1(-1, -1, -1); + const Vector3 p_FPa2(-0.1, 0.5, -1); + pts[0].v = ToCcdVec3(p_FPa1 - p_FPa2); + pts[0].v1 = ToCcdVec3(p_FPa1); + pts[0].v2 = ToCcdVec3(p_FPa2); + + const Vector3 p_FPb1(-1, 1, -1); + const Vector3 p_FPb2(-0.1, 0.5, -1); + pts[1].v = ToCcdVec3(p_FPb1 - p_FPb2); + pts[1].v1 = ToCcdVec3(p_FPb1); + pts[1].v2 = ToCcdVec3(p_FPb2); + + const Vector3 p_FPc1(1, 1, -1); + const Vector3 p_FPc2(-0.1, 0.5, -1); + pts[2].v = ToCcdVec3(p_FPc1 - p_FPc2); + pts[2].v1 = ToCcdVec3(p_FPc1); + pts[2].v2 = ToCcdVec3(p_FPc2); + for (int i = 0; i < 3; ++i) { + ccdSimplexAdd(&simplex, &pts[i]); + } + // Make sure that the origin is on the triangle. + const Vector3 a = ToEigenVector(pts[0].v); + const Vector3 b = ToEigenVector(pts[1].v); + const Vector3 c = ToEigenVector(pts[2].v); + // We first check if the origin is co-planar with vertices a, b, and c. + // If a, b, c and origin are co-planar, then aᵀ · (b × c)) = 0 + EXPECT_NEAR(a.dot(b.cross(c)), 0, 1E-10); + // Now check if origin is within the triangle, by checking the condition + // (a × b)ᵀ · (b × c) ≥ 0 + // (b × c)ᵀ · (c × a) ≥ 0 + // (c × a)ᵀ · (a × b) ≥ 0 + // Namely the cross product a × b, b × c, c × a all point to the same + // direction. + // Note the check above is valid when either a, b, c is a zero vector, or + // they are co-linear. + EXPECT_GE(a.cross(b).dot(b.cross(c)), 0); + EXPECT_GE(b.cross(c).dot(c.cross(a)), 0); + EXPECT_GE(c.cross(a).dot(a.cross(b)), 0); + + ccd_pt_t polytope; + ccdPtInit(&polytope); + ccd_pt_el_t* nearest{}; + libccd_extension::convert2SimplexToTetrahedron(o1, o2, &ccd, &simplex, + &polytope, &nearest); + // Box1 and Box2 are not touching, so nearest is set to null. + EXPECT_EQ(nearest, nullptr); + + // Check the polytope + // The polytope should have 4 vertices, with three of them being the vertices + // of the 2-simplex, and another vertex that has the maximal support along + // the normal directions of the 2-simplex. + // We first construct the set containing the polytope vertices. + std::unordered_set polytope_vertices; + { + ccd_pt_vertex_t* v; + ccdListForEachEntry(&polytope.vertices, v, ccd_pt_vertex_t, list) { + const auto it = polytope_vertices.find(v); + EXPECT_EQ(it, polytope_vertices.end()); + polytope_vertices.emplace_hint(it, v); + } + } + EXPECT_EQ(polytope_vertices.size(), 4u); + // We need to find out the vertex on the polytope, that is not the vertex + // of the simplex. + ccd_pt_vertex_t* non_simplex_vertex{nullptr}; + // A simplex vertex matches with a polytope vertex if they coincide. + int num_matched_vertices = 0; + for (const auto& v : polytope_vertices) { + bool found_match = false; + for (int i = 0; i < 3; ++i) { + if (ccdVec3Dist2(&v->v.v, &pts[i].v) < constants::eps_78()) { + num_matched_vertices++; + found_match = true; + break; + } + } + if (!found_match) { + non_simplex_vertex = v; + } + } + EXPECT_EQ(num_matched_vertices, 3); + EXPECT_NE(non_simplex_vertex, nullptr); + // Make sure that the non-simplex vertex has the maximal support along the + // simplex normal direction. + // Find the two normal directions of the 2-simplex. + Vector3 dir1, dir2; + Vector3 ab, ac; + ab = ToEigenVector(pts[1].v) - ToEigenVector(pts[0].v); + ac = ToEigenVector(pts[2].v) - ToEigenVector(pts[0].v); + dir1 = ab.cross(ac); + dir2 = -dir1; + // Now make sure non_simplex_vertex has the largest support + // p_B1V1 are the position of the box 1 vertices in box1 frame B1. + // p_B2V1 are the position of the box 2 vertices in box2 frame B2. + const Eigen::Vector3d box1_size{2, 2, 2}; + const Eigen::Vector3d box2_size{1, 1, 2}; + Eigen::Matrix p_B1V1, p_B2V2; + p_B1V1 << -1, -1, -1, -1, 1, 1, 1, 1, -1, -1, 1, 1, -1, -1, 1, 1, -1, 1, -1, + 1, -1, 1, -1, 1; + p_B2V2 << -1, -1, -1, -1, 1, 1, 1, 1, -1, -1, 1, 1, -1, -1, 1, 1, -1, 1, -1, + 1, -1, 1, -1, 1; + for (int i = 0; i < 3; ++i) { + p_B1V1.row(i) *= box1_size[i] / 2; + p_B2V2.row(i) *= box2_size[i] / 2; + } + + const Eigen::Matrix p_FV1 = X_FB1 * p_B1V1; + const Eigen::Matrix p_FV2 = X_FB2 * p_B2V2; + // The support of the Minkowski difference along direction dir1. + const S max_support1 = (dir1.transpose() * p_FV1).maxCoeff() - + (dir1.transpose() * p_FV2).minCoeff(); + // The support of the Minkowski difference along direction dir2. + const S max_support2 = (dir2.transpose() * p_FV1).maxCoeff() - + (dir2.transpose() * p_FV2).minCoeff(); + + const double expected_max_support = std::max(max_support1, max_support2); + const double non_simplex_vertex_support1 = + ToEigenVector(non_simplex_vertex->v.v).dot(dir1); + const double non_simplex_vertex_support2 = + ToEigenVector(non_simplex_vertex->v.v).dot(dir2); + EXPECT_NEAR( + std::max(non_simplex_vertex_support1, non_simplex_vertex_support2), + expected_max_support, constants::eps_78()); + + // Also make sure the non_simplex_vertex actually is inside the Minkowski + // difference. + // Call the non-simplex vertex as D. This vertex equals to the difference + // between a point Dv1 in box1, and a point Dv2 in box2. + const Vector3 p_B1Dv1 = + (X_WF * X_FB1).inverse() * ToEigenVector(non_simplex_vertex->v.v1); + const Vector3 p_B2Dv2 = + (X_WF * X_FB2).inverse() * ToEigenVector(non_simplex_vertex->v.v2); + // Now check if p_B1Dv1 is in box1, and p_B2Dv2 is in box2. + for (int i = 0; i < 3; ++i) { + EXPECT_LE(p_B1Dv1(i), box1_size(i) / 2 + constants::eps_78()); + EXPECT_LE(p_B2Dv2(i), box2_size(i) / 2 + constants::eps_78()); + EXPECT_GE(p_B1Dv1(i), -box1_size(i) / 2 - constants::eps_78()); + EXPECT_GE(p_B2Dv2(i), -box2_size(i) / 2 - constants::eps_78()); + } + // Now that we make sure the vertices of the polytope is correct, we will + // check the edges and faces of the polytope. We do so by constructing an + // expected polytope, and compare it with the polytope obtained from + // convert2SimplexToTetrahedron(). + ccd_pt_t polytope_expected; + ccdPtInit(&polytope_expected); + + ccd_pt_vertex_t* vertices_expected[4]; + int v_count = 0; + for (const auto& v : polytope_vertices) { + vertices_expected[v_count++] = ccdPtAddVertex(&polytope_expected, &v->v); + } + ccd_pt_edge_t* edges_expected[6]; + edges_expected[0] = ccdPtAddEdge(&polytope_expected, vertices_expected[0], + vertices_expected[1]); + edges_expected[1] = ccdPtAddEdge(&polytope_expected, vertices_expected[1], + vertices_expected[2]); + edges_expected[2] = ccdPtAddEdge(&polytope_expected, vertices_expected[2], + vertices_expected[0]); + edges_expected[3] = ccdPtAddEdge(&polytope_expected, vertices_expected[3], + vertices_expected[0]); + edges_expected[4] = ccdPtAddEdge(&polytope_expected, vertices_expected[3], + vertices_expected[1]); + edges_expected[5] = ccdPtAddEdge(&polytope_expected, vertices_expected[3], + vertices_expected[2]); + + ccdPtAddFace(&polytope_expected, edges_expected[0], edges_expected[3], + edges_expected[4]); + ccdPtAddFace(&polytope_expected, edges_expected[1], edges_expected[4], + edges_expected[5]); + ccdPtAddFace(&polytope_expected, edges_expected[2], edges_expected[3], + edges_expected[5]); + ccdPtAddFace(&polytope_expected, edges_expected[0], edges_expected[1], + edges_expected[2]); + + ComparePolytope(&polytope, &polytope_expected, constants::eps_34()); + + ccdPtDestroy(&polytope_expected); + ccdPtDestroy(&polytope); +} + +template +void TestSimplexToPolytope3() { + Transform3 X_WF; + X_WF.setIdentity(); + TestSimplexToPolytope3InGivenFrame(X_WF); + + X_WF.translation() << 0, 0, 1; + TestSimplexToPolytope3InGivenFrame(X_WF); + + X_WF.translation() << -0.2, 0.4, 0.1; + TestSimplexToPolytope3InGivenFrame(X_WF); +} + +GTEST_TEST(FCL_GJK_EPA, convert2SimplexToTetrahedron) { + TestSimplexToPolytope3(); + TestSimplexToPolytope3(); +} + +} // namespace detail +} // namespace fcl + +//============================================================================== +int main(int argc, char* argv[]) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_signed_distance.cpp b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_signed_distance.cpp new file mode 100644 index 000000000..6add8eb41 --- /dev/null +++ b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_signed_distance.cpp @@ -0,0 +1,342 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. 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 Hongkai Dai */ +/** + * Test the signed distance query between two convex objects, when calling with + * solver = GST_LIBCCD. + */ +#include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h" + +#include +#include + +#include "fcl/narrowphase/detail/gjk_solver_libccd.h" + +namespace fcl { +namespace detail { +// Given two spheres, sphere 1 has radius1, and centered at point A, whose +// position is p_FA measured and expressed in frame F; sphere 2 has radius2, +// and centered at point B, whose position is p_FB measured and expressed in +// frame F. Computes the signed distance between the two spheres, together with +// the two closest points Na on sphere 1 and Nb on sphere 2, returns the +// position of Na and Nb expressed in frame F. +// We use the monogram notation on spatial vectors. The monogram notation is +// explained in +// http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html +template +S ComputeSphereSphereDistance(S radius1, S radius2, const Vector3& p_FA, + const Vector3& p_FB, Vector3* p_FNa, + Vector3* p_FNb) { + S min_distance = (p_FA - p_FB).norm() - radius1 - radius2; + const Vector3 p_AB_F = + p_FB - p_FA; // The vector AB measured and expressed + // in frame F. + *p_FNa = p_FA + p_AB_F.normalized() * radius1; + *p_FNb = p_FB - p_AB_F.normalized() * radius2; + return min_distance; +} + +template +void TestSphereToSphereGJKSignedDistance(S radius1, S radius2, + const Vector3& p_FA, + const Vector3& p_FB, S tol, + S min_distance_expected, + const Vector3& p_FNa_expected, + const Vector3& p_FNb_expected) { + // Test if GJKSignedDistance computes the right distance. Here we used sphere + // to sphere as the geometries. The distance between sphere and sphere should + // be computed using distance between primitives, instead of the GJK + // algorithm. But here we choose spheres for simplicity. + + fcl::Sphere s1(radius1); + fcl::Sphere s2(radius2); + fcl::Transform3 tf1, tf2; + tf1.setIdentity(); + tf2.setIdentity(); + tf1.translation() = p_FA; + tf2.translation() = p_FB; + void* o1 = GJKInitializer>::createGJKObject(s1, tf1); + void* o2 = GJKInitializer>::createGJKObject(s2, tf2); + + S dist; + Vector3 p1, p2; + GJKSolver_libccd gjkSolver; + bool res = GJKSignedDistance( + o1, detail::GJKInitializer>::getSupportFunction(), o2, + detail::GJKInitializer>::getSupportFunction(), + gjkSolver.max_distance_iterations, gjkSolver.distance_tolerance, &dist, + &p1, &p2); + + EXPECT_EQ(res, min_distance_expected >= 0); + + EXPECT_NEAR(dist, min_distance_expected, tol); + EXPECT_TRUE(p1.isApprox(p_FNa_expected, tol)); + EXPECT_TRUE(p2.isApprox(p_FNb_expected, tol)); + + GJKInitializer>::deleteGJKObject(o1); + GJKInitializer>::deleteGJKObject(o2); +} + +template +struct SphereSpecification { + SphereSpecification(S radius_, const Vector3& center_) + : radius{radius_}, center{center_} {} + S radius; + Vector3 center; +}; + +template +void TestNonCollidingSphereGJKSignedDistance(S tol) { + std::vector> spheres; + spheres.emplace_back(0.5, Vector3(0, 0, -1.2)); + spheres.emplace_back(0.5, Vector3(1.25, 0, 0)); + spheres.emplace_back(0.3, Vector3(-0.2, 0, 0)); + spheres.emplace_back(0.4, Vector3(-0.2, 0, 1.1)); + for (int i = 0; i < static_cast(spheres.size()); ++i) { + for (int j = i + 1; j < static_cast(spheres.size()); ++j) { + if ((spheres[i].center - spheres[j].center).norm() > + spheres[i].radius + spheres[j].radius) { + // Not in collision. + Vector3 p_FNa, p_FNb; + const S min_distance_expected = ComputeSphereSphereDistance( + spheres[i].radius, spheres[j].radius, spheres[i].center, + spheres[j].center, &p_FNa, &p_FNb); + TestSphereToSphereGJKSignedDistance( + spheres[i].radius, spheres[j].radius, spheres[i].center, + spheres[j].center, tol, min_distance_expected, p_FNa, p_FNb); + } else { + GTEST_FAIL() << "The two spheres collide." + << "\nSpheres[" << i << "] with radius " + << spheres[i].radius << ", centered at " + << spheres[i].center.transpose() << "\nSpheres[" << j + << "] with radius " << spheres[j].radius + << ", centered at " << spheres[j].center.transpose() + << "\n"; + } + } + } +} + +GTEST_TEST(FCL_GJKSignedDistance, sphere_sphere) { + // By setting gjkSolver.distance_tolerance to the default value (1E-6), the + // tolerance we get on the closest points are only up to the square root of + // 1E-6, namely 1E-3. + TestNonCollidingSphereGJKSignedDistance(1E-3); + TestNonCollidingSphereGJKSignedDistance(1E-3); +} + +//---------------------------------------------------------------------------- +// Box test +// Given two boxes, we can perturb the pose of one box so the boxes are +// penetrating, touching or separated. +template +struct BoxSpecification { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + BoxSpecification(const fcl::Vector3& m_size) : size(m_size) { + X_FB.setIdentity(); + } + fcl::Vector3 size; + fcl::Transform3 X_FB; +}; + +template +void TestBoxesInFrameF(S tol, const Transform3& X_WF) { + const fcl::Vector3 box1_size(1, 1, 1); + const fcl::Vector3 box2_size(0.6, 0.8, 1); + // Put the two boxes on the xy plane of frame F. + // B1 is the frame rigidly attached to box 1, B2 is the frame rigidly attached + // to box 2. W is the world frame. F is a frame fixed to the world. X_FB1 is + // the pose of box 1 expressed and measured in frame F, X_FB2 is the pose of + // box 2 expressed and measured in frame F. + fcl::Transform3 X_FB1, X_FB2; + // Box 1 is fixed. + X_FB1.setIdentity(); + X_FB1.translation() << 0, 0, 0.5; + + // First fix the orientation of box 2, such that one of its diagonal (the one + // connecting the vertex (0.3, -0.4, 1) and (-0.3, 0.4, 1) is parallel to the + // x axis in frame F. If we move the position of box 2, we get different + // signed distance. + X_FB2.setIdentity(); + X_FB2.linear() << 0.6, -0.8, 0, 0.8, 0.6, 0, 0, 0, 1; + + // p_xy_FNa is the xy position of point Na (the deepest penetration point on + // box 1) measured and expressed in the frame F. + // p_xy_FNb is the xy position of point Nb (the deepest penetration point on + // box 2) measured and expressed in the frame F. + auto CheckDistance = [&box1_size, &box2_size, &X_FB1, &X_WF]( + const Transform3& X_FB2, S distance_expected, + const Vector2& p_xy_FNa_expected, const Vector2& p_xy_FNb_expected, + S tol) { + const fcl::Transform3 X_WB1 = X_WF * X_FB1; + const fcl::Transform3 X_WB2 = X_WF * X_FB2; + fcl::Box box1(box1_size); + fcl::Box box2(box2_size); + void* o1 = GJKInitializer>::createGJKObject(box1, X_WB1); + void* o2 = GJKInitializer>::createGJKObject(box2, X_WB2); + S dist; + Vector3 p_WNa, p_WNb; + GJKSolver_libccd gjkSolver; + bool res = GJKSignedDistance( + o1, detail::GJKInitializer>::getSupportFunction(), o2, + detail::GJKInitializer>::getSupportFunction(), + gjkSolver.max_distance_iterations, gjkSolver.distance_tolerance, &dist, + &p_WNa, &p_WNb); + + // It is unclear how FCL handles touching contact. It could return either + // true or false for touching contact. So, we ignore the condition where + // expected distance is zero. + if (distance_expected < 0) { + EXPECT_FALSE(res); + } else if (distance_expected > 0) { + EXPECT_TRUE(res); + } + + EXPECT_NEAR(dist, distance_expected, tol); + const Vector3 p_FNa = + X_WF.linear().transpose() * (p_WNa - X_WF.translation()); + const Vector3 p_FNb = + X_WF.linear().transpose() * (p_WNb - X_WF.translation()); + + EXPECT_TRUE(p_FNa.template head<2>().isApprox(p_xy_FNa_expected, tol)); + EXPECT_TRUE(p_FNb.template head<2>().isApprox(p_xy_FNb_expected, tol)); + // The z height of the closest points should be the same. + EXPECT_NEAR(p_FNa(2), p_FNb(2), tol); + // The closest point is within object A/B, so the z height should be within + // [0, 1] + EXPECT_GE(p_FNa(2), 0); + EXPECT_GE(p_FNb(2), 0); + EXPECT_LE(p_FNa(2), 1); + EXPECT_LE(p_FNb(2), 1); + + GJKInitializer>::deleteGJKObject(o1); + GJKInitializer>::deleteGJKObject(o2); + }; + + auto CheckBoxEdgeBoxFaceDistance = [&CheckDistance]( + const Transform3& X_FB2, S tol) { + const double expected_distance = -X_FB2.translation()(0) - 1; + CheckDistance( + X_FB2, expected_distance, Vector2(-0.5, X_FB2.translation()(1)), + Vector2(X_FB2.translation()(0) + 0.5, X_FB2.translation()(1)), tol); + }; + //--------------------------------------------------------------- + // Touching contact + // An edge of box 2 is touching a face of box 1 + X_FB2.translation() << -1, 0, 0.5; + CheckBoxEdgeBoxFaceDistance(X_FB2, tol); + + // The touching face on box 1 is parallel to the y axis, so shifting box 2 on + // y axis still gives touching contact. Shift box 2 on y axis by 0.1m. + X_FB2.translation() << -1, 0.1, 0.5; + CheckBoxEdgeBoxFaceDistance(X_FB2, tol); + + // Shift box 2 on y axis by -0.1m. + X_FB2.translation() << -1, -0.1, 0.5; + CheckBoxEdgeBoxFaceDistance(X_FB2, tol); + // TODO(hongkai.dai@tri.global): Add other touching contact cases, including + // face-face, face-vertex, edge-edge, edge-vertex and vertex-vertex. + + //-------------------------------------------------------------- + // Penetrating contact + // An edge of box 2 penetrates into a face of box 1 + X_FB2.translation() << -0.9, 0, 0.5; + CheckBoxEdgeBoxFaceDistance(X_FB2, tol); + + // Shift box 2 on y axis by 0.1m. + X_FB2.translation() << -0.9, 0.1, 0.5; + CheckBoxEdgeBoxFaceDistance(X_FB2, tol); + + // Shift box 2 on y axis by -0.05m. + X_FB2.translation() << -0.9, -0.05, 0.5; + CheckBoxEdgeBoxFaceDistance(X_FB2, tol); + + // Shift box 2 on y axis by -0.1m. + X_FB2.translation() << -0.9, -0.1, 0.5; + CheckBoxEdgeBoxFaceDistance(X_FB2, tol); +} + +template +void TestBoxes(S tol) { + // Frame F coincides with the world frame W. + Transform3 X_WF; + X_WF.setIdentity(); + TestBoxesInFrameF(tol, X_WF); + + // Frame F is shifted from the world frame W. + X_WF.translation() << 0, 0, 1; + TestBoxesInFrameF(tol, X_WF); + + X_WF.translation() << 0, 1, 0; + TestBoxesInFrameF(tol, X_WF); + + X_WF.translation() << 1, 0, 0; + TestBoxesInFrameF(tol, X_WF); + + // Frame F is rotated from the world frame W. + X_WF.setIdentity(); + const S kPi = fcl::constants::pi(); + X_WF.linear() = + Eigen::AngleAxis(0.1 * kPi, Vector3::UnitZ()).toRotationMatrix(); + TestBoxesInFrameF(tol, X_WF); + + // TODO(hongkai.dai): This test exposes an error in simplexToPolytope4, that + // the initial simplex can be degenerate. Should add the special case on + // degenerate simplex in simplexToPolytope4. + /*X_WF.translation() << 0, 0, 0; + X_WF.linear() = + Eigen::AngleAxis(0.1 * kPi, Vector3(1.0 / 3, 2.0 / 3, -2.0 / 3)) + .toRotationMatrix(); + TestBoxesInFrameF(tol, X_WF);*/ + + // Frame F is rotated and shifted from the world frame W. + X_WF.translation() << 0.1, 0.2, 0.3; + TestBoxesInFrameF(tol, X_WF); +} + +GTEST_TEST(FCL_GJKSignedDistance, box_box) { + // By setting gjkSolver.distance_tolerance to the default value (1E-6), the + // tolerance we get on the closest points are only up to 1E-3 + TestBoxes(1E-3); + TestBoxes(1E-3); +} +} // namespace detail +} // namespace fcl + +//============================================================================== +int main(int argc, char* argv[]) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/test_fcl_signed_distance.cpp b/test/test_fcl_signed_distance.cpp index 1d6296352..c67eb9f89 100644 --- a/test/test_fcl_signed_distance.cpp +++ b/test/test_fcl_signed_distance.cpp @@ -78,16 +78,21 @@ void test_distance_spheresphere(GJKSolverType solver_type) res = distance(&s1, tf1, &s2, tf2, request, result); EXPECT_TRUE(res); - EXPECT_TRUE(std::abs(result.min_distance - (-5)) < 1.5e-1); - // TODO(JS): The negative distance computation using libccd requires - // unnecessarily high error tolerance. + // request.distance_tolerance is actually the square of the distance + // tolerance, namely the difference between distance returned from FCL's EPA + // implementation and the actual distance, is less than + // sqrt(request.distance_tolerance). + const S distance_tolerance = std::sqrt(request.distance_tolerance); + EXPECT_NEAR(result.min_distance, -5, distance_tolerance); // TODO(JS): Only GST_LIBCCD can compute the pair of nearest points on the // surface of the spheres. if (solver_type == GST_LIBCCD) { - EXPECT_TRUE(result.nearest_points[0].isApprox(Vector3(20, 0, 0))); - EXPECT_TRUE(result.nearest_points[1].isApprox(Vector3(-10, 0, 0))); + EXPECT_TRUE(result.nearest_points[0].isApprox(Vector3(20, 0, 0), + distance_tolerance)); + EXPECT_TRUE(result.nearest_points[1].isApprox(Vector3(-10, 0, 0), + distance_tolerance)); } } From 2462bb1d06488cb5fa155363be4f53ed366b942b Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Wed, 11 Jul 2018 16:53:03 +0200 Subject: [PATCH 03/34] Add GenerateBVHSubModel variants (#308) This commit modifies the generateBVHModel() functions to use an additional argument to chose whether to delay construction of the BVH to permit more geometry to be added first. --- .../geometric_shape_to_BVH_model-inl.h | 136 +++---- .../geometry/geometric_shape_to_BVH_model.h | 121 +++++-- test/CMakeLists.txt | 1 + ...l_generate_bvh_model_deferred_finalize.cpp | 331 ++++++++++++++++++ 4 files changed, 494 insertions(+), 95 deletions(-) create mode 100644 test/test_fcl_generate_bvh_model_deferred_finalize.cpp diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h index bafdfea73..7fe43860c 100644 --- a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h +++ b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h @@ -44,8 +44,29 @@ namespace fcl { //============================================================================== +// Local helper function to ease conditional adding of triangles to a BVHModel template -void generateBVHModel(BVHModel& model, const Box& shape, const Transform3& pose) +int addTriangles(BVHModel& model, const std::vector>& points, const std::vector& tri_indices, FinalizeModel finalize_model) +{ + int retval = BVH_OK; + if(model.build_state == BVH_BUILD_STATE_EMPTY){ + retval = model.beginModel(); + } + + if(retval == BVH_OK){ + retval = model.addSubModel(points, tri_indices); + } + + if(retval == BVH_OK && finalize_model == FinalizeModel::DO){ + retval = model.endModel(); + model.computeLocalAABB(); + } + return retval; +} + +//============================================================================== +template +int generateBVHModel(BVHModel& model, const Box& shape, const Transform3& pose, FinalizeModel finalize_model) { using S = typename BV::S; @@ -81,15 +102,13 @@ void generateBVHModel(BVHModel& model, const Box& shape, con points[i] = pose * points[i]; } - model.beginModel(); - model.addSubModel(points, tri_indices); - model.endModel(); - model.computeLocalAABB(); + return addTriangles(model, points, tri_indices, finalize_model); } + //============================================================================== template -void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3& pose, unsigned int seg, unsigned int ring) +int generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model) { using S = typename BV::S; @@ -148,15 +167,12 @@ void generateBVHModel(BVHModel& model, const Sphere& shape, points[i] = pose * points[i]; } - model.beginModel(); - model.addSubModel(points, tri_indices); - model.endModel(); - model.computeLocalAABB(); + return addTriangles(model, points, tri_indices, finalize_model); } //============================================================================== template -void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3& pose, unsigned int n_faces_for_unit_sphere) +int generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3& pose, unsigned int n_faces_for_unit_sphere, FinalizeModel finalize_model) { using S = typename BV::S; @@ -165,12 +181,12 @@ void generateBVHModel(BVHModel& model, const Sphere& shape, unsigned int ring = ceil(n_low_bound); unsigned int seg = ceil(n_low_bound); - generateBVHModel(model, shape, pose, seg, ring); + return generateBVHModel(model, shape, pose, seg, ring, finalize_model); } //============================================================================== template -void generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3& pose, unsigned int seg, unsigned int ring) +int generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model) { using S = typename BV::S; @@ -232,15 +248,12 @@ void generateBVHModel(BVHModel& model, const Ellipsoid& shap points[i] = pose * points[i]; } - model.beginModel(); - model.addSubModel(points, tri_indices); - model.endModel(); - model.computeLocalAABB(); + return addTriangles(model, points, tri_indices, finalize_model); } //============================================================================== template -void generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3& pose, unsigned int n_faces_for_unit_ellipsoid) +int generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3& pose, unsigned int n_faces_for_unit_ellipsoid, FinalizeModel finalize_model) { using S = typename BV::S; @@ -256,12 +269,12 @@ void generateBVHModel(BVHModel& model, const Ellipsoid& shap const unsigned int ring = std::ceil(n_low_bound); const unsigned int seg = std::ceil(n_low_bound); - generateBVHModel(model, shape, pose, seg, ring); + return generateBVHModel(model, shape, pose, seg, ring, finalize_model); } //============================================================================== template -void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3& pose, unsigned int tot, unsigned int h_num) +int generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model) { using S = typename BV::S; @@ -272,45 +285,45 @@ void generateBVHModel(BVHModel& model, const Cylinder& shape S h = shape.lz; S phi, phid; const S pi = constants::pi(); - phid = pi * 2 / tot; + phid = pi * 2 / circle_split_tot; phi = 0; S hd = h / h_num; - for(unsigned int i = 0; i < tot; ++i) + for(unsigned int i = 0; i < circle_split_tot; ++i) points.emplace_back(r * cos(phi + phid * i), r * sin(phi + phid * i), h / 2); for(unsigned int i = 0; i < h_num - 1; ++i) { - for(unsigned int j = 0; j < tot; ++j) + for(unsigned int j = 0; j < circle_split_tot; ++j) { points.emplace_back(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd); } } - for(unsigned int i = 0; i < tot; ++i) + for(unsigned int i = 0; i < circle_split_tot; ++i) points.emplace_back(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2); points.emplace_back(0, 0, h / 2); points.emplace_back(0, 0, -h / 2); - for(unsigned int i = 0; i < tot; ++i) - tri_indices.emplace_back((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1))); + for(unsigned int i = 0; i < circle_split_tot; ++i) + tri_indices.emplace_back((h_num + 1) * circle_split_tot, i, ((i == circle_split_tot - 1) ? 0 : (i + 1))); - for(unsigned int i = 0; i < tot; ++i) - tri_indices.emplace_back((h_num + 1) * tot + 1, h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i); + for(unsigned int i = 0; i < circle_split_tot; ++i) + tri_indices.emplace_back((h_num + 1) * circle_split_tot + 1, h_num * circle_split_tot + ((i == circle_split_tot - 1) ? 0 : (i + 1)), h_num * circle_split_tot + i); for(unsigned int i = 0; i < h_num; ++i) { - for(unsigned int j = 0; j < tot; ++j) + for(unsigned int j = 0; j < circle_split_tot; ++j) { int a, b, c, d; a = j; - b = (j == tot - 1) ? 0 : (j + 1); - c = j + tot; - d = (j == tot - 1) ? tot : (j + 1 + tot); + b = (j == circle_split_tot - 1) ? 0 : (j + 1); + c = j + circle_split_tot; + d = (j == circle_split_tot - 1) ? circle_split_tot : (j + 1 + circle_split_tot); - int start = i * tot; + int start = i * circle_split_tot; tri_indices.emplace_back(start + b, start + a, start + c); tri_indices.emplace_back(start + b, start + c, start + d); } @@ -321,15 +334,12 @@ void generateBVHModel(BVHModel& model, const Cylinder& shape points[i] = pose * points[i]; } - model.beginModel(); - model.addSubModel(points, tri_indices); - model.endModel(); - model.computeLocalAABB(); + return addTriangles(model, points, tri_indices, finalize_model); } //============================================================================== template -void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3& pose, unsigned int tot_for_unit_cylinder) +int generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3& pose, unsigned int circle_split_tot_for_unit_cylinder, FinalizeModel finalize_model) { using S = typename BV::S; @@ -337,19 +347,18 @@ void generateBVHModel(BVHModel& model, const Cylinder& shape S h = shape.lz; const S pi = constants::pi(); - unsigned int tot = tot_for_unit_cylinder * r; - S phid = pi * 2 / tot; + unsigned int circle_split_tot = circle_split_tot_for_unit_cylinder * r; + S phid = pi * 2 / circle_split_tot; S circle_edge = phid * r; unsigned int h_num = ceil(h / circle_edge); - generateBVHModel(model, shape, pose, tot, h_num); + return generateBVHModel(model, shape, pose, circle_split_tot, h_num, finalize_model); } - //============================================================================== template -void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3& pose, unsigned int tot, unsigned int h_num) +int generateBVHModel(BVHModel& model, const Cone& shape, const Transform3& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model) { using S = typename BV::S; @@ -361,7 +370,7 @@ void generateBVHModel(BVHModel& model, const Cone& shape, co S phi, phid; const S pi = constants::pi(); - phid = pi * 2 / tot; + phid = pi * 2 / circle_split_tot; phi = 0; S hd = h / h_num; @@ -370,35 +379,35 @@ void generateBVHModel(BVHModel& model, const Cone& shape, co { S h_i = h / 2 - (i + 1) * hd; S rh = r * (0.5 - h_i / h); - for(unsigned int j = 0; j < tot; ++j) + for(unsigned int j = 0; j < circle_split_tot; ++j) { points.emplace_back(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i); } } - for(unsigned int i = 0; i < tot; ++i) + for(unsigned int i = 0; i < circle_split_tot; ++i) points.emplace_back(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2); points.emplace_back(0, 0, h / 2); points.emplace_back(0, 0, -h / 2); - for(unsigned int i = 0; i < tot; ++i) - tri_indices.emplace_back(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1)); + for(unsigned int i = 0; i < circle_split_tot; ++i) + tri_indices.emplace_back(h_num * circle_split_tot, i, (i == circle_split_tot - 1) ? 0 : (i + 1)); - for(unsigned int i = 0; i < tot; ++i) - tri_indices.emplace_back(h_num * tot + 1, (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)), (h_num - 1) * tot + i); + for(unsigned int i = 0; i < circle_split_tot; ++i) + tri_indices.emplace_back(h_num * circle_split_tot + 1, (h_num - 1) * circle_split_tot + ((i == circle_split_tot - 1) ? 0 : (i + 1)), (h_num - 1) * circle_split_tot + i); for(unsigned int i = 0; i < h_num - 1; ++i) { - for(unsigned int j = 0; j < tot; ++j) + for(unsigned int j = 0; j < circle_split_tot; ++j) { int a, b, c, d; a = j; - b = (j == tot - 1) ? 0 : (j + 1); - c = j + tot; - d = (j == tot - 1) ? tot : (j + 1 + tot); + b = (j == circle_split_tot - 1) ? 0 : (j + 1); + c = j + circle_split_tot; + d = (j == circle_split_tot - 1) ? circle_split_tot : (j + 1 + circle_split_tot); - int start = i * tot; + int start = i * circle_split_tot; tri_indices.emplace_back(start + b, start + a, start + c); tri_indices.emplace_back(start + b, start + c, start + d); } @@ -409,31 +418,30 @@ void generateBVHModel(BVHModel& model, const Cone& shape, co points[i] = pose * points[i]; } - model.beginModel(); - model.addSubModel(points, tri_indices); - model.endModel(); - model.computeLocalAABB(); + return addTriangles(model, points, tri_indices, finalize_model); + } //============================================================================== template -void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3& pose, unsigned int tot_for_unit_cone) +int generateBVHModel(BVHModel& model, const Cone& shape, const Transform3& pose, unsigned int circle_split_tot_for_unit_cone, FinalizeModel finalize_model) { using S = typename BV::S; + S r = shape.radius; S h = shape.lz; const S pi = constants::pi(); - unsigned int tot = tot_for_unit_cone * r; - S phid = pi * 2 / tot; + unsigned int circle_split_tot = circle_split_tot_for_unit_cone * r; + S phid = pi * 2 / circle_split_tot; S circle_edge = phid * r; unsigned int h_num = ceil(h / circle_edge); - generateBVHModel(model, shape, pose, tot, h_num); + return generateBVHModel(model, shape, pose, circle_split_tot, h_num, finalize_model); } } // namespace fcl -#endif +#endif \ No newline at end of file diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model.h b/include/fcl/geometry/geometric_shape_to_BVH_model.h index c18825d07..d44350d38 100644 --- a/include/fcl/geometry/geometric_shape_to_BVH_model.h +++ b/include/fcl/geometry/geometric_shape_to_BVH_model.h @@ -52,54 +52,113 @@ namespace fcl { +/** +@brief enum class used to indicate whether we simply want to add more primitives to the model + or finalize it. +*/ +enum class FinalizeModel{ + DO, + DONT +}; + + +/** +@defgroup generateBVHModel +@brief Create a BVHModel using geometric primitives +@details The functions in this group can be used to add geometric primitives (Box, Sphere, Ellipsoid, Cylinder, Cone) + to a BVHModel. It can either close off the model or leave it unfinalized in order to add more primitives later. +@note All functions in this group have a common sub-set of parameters (listed below). In addition, each has unique + parameters related to the geometry type being added and how it should be tessellated. These additional parameters + are documented with their corresponding function +@warning If this function is used to create a BVHModel containing multiple geometric primitives, the BVHModel inherently + represents the *union* of those primitives. The BVHModel structure does not retain any notion of the original + geometric primitive. +@param[out] model The BVHModel to be generated or added to +@param[in] shape The geometric object to be added to the BVHModel +@param[in] pose The pose of the geometric object +@param[in] finalize_model an enum indicating whether the model is final or more submodels can be added later +@return Return code (as defined by BVHReturnCode) indicating the success of the operation +@{ +*/ /// @brief Generate BVH model from box template -void generateBVHModel(BVHModel& model, const Box& shape, const Transform3& pose); +int generateBVHModel(BVHModel& model, const Box& shape, const Transform3& pose, FinalizeModel finalize_model = FinalizeModel::DO); -/// @brief Generate BVH model from sphere, given the number of segments along longitude and number of rings along latitude. +/** +@brief Generate BVH model from sphere +@param[in] seg The number of segments along longitude +@param[in] ring The number of rings along latitude +**/ template -void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3& pose, unsigned int seg, unsigned int ring); - -/// @brief Generate BVH model from sphere -/// The difference between generateBVHModel is that it gives the number of triangles faces N for a sphere with unit radius. For sphere of radius r, -/// then the number of triangles is r * r * N so that the area represented by a single triangle is approximately the same.s +int generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model = FinalizeModel::DO); + +/** +@brief Generate BVH model from sphere +@details The difference between generateBVHModel is that it gives the number of triangles faces N for a sphere with unit radius. For sphere of radius r, + then the number of triangles is r * r * N so that the area represented by a single triangle is approximately the same. +@param[in] n_faces_for_unit_sphere The number of triangles for a unit-sized sphere +**/ template -void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3& pose, unsigned int n_faces_for_unit_sphere); +int generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3& pose, unsigned int n_faces_for_unit_sphere, FinalizeModel finalize_model = FinalizeModel::DO); -/// @brief Generate BVH model from ellipsoid, given the number of segments along longitude and number of rings along latitude. +/** +@brief Generate BVH model from ellipsoid +@param[in] seg The number of segments along longitude +@param[in] ring The number of rings along latitude +**/ template -void generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3& pose, unsigned int seg, unsigned int ring); - -/// @brief Generate BVH model from ellipsoid -/// The difference between generateBVHModel is that it gives the number of triangles faces N for an ellipsoid with unit radii (1, 1, 1). For ellipsoid of radii (a, b, c), -/// then the number of triangles is ((a^p * b^p + b^p * c^p + c^p * a^p)/3)^(1/p) * N, where p is 1.6075, so that the area represented by a single triangle is approximately the same. -/// Reference: https://en.wikipedia.org/wiki/Ellipsoid#Approximate_formula +int generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model = FinalizeModel::DO); + +/** +@brief Generate BVH model from ellipsoid +@details The difference between generateBVHModel is that it gives the number of triangles faces N for an ellipsoid with unit radii (1, 1, 1). For ellipsoid of radii (a, b, c), + then the number of triangles is ((a^p * b^p + b^p * c^p + c^p * a^p)/3)^(1/p) * N, where p is 1.6075, so that the area represented by a single triangle is approximately the same. + Reference: https://en.wikipedia.org/wiki/Ellipsoid#Approximate_formula +@param[in] n_faces_for_unit_ellipsoid The number of faces a unit ellipsoid would have +**/ template -void generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3& pose, unsigned int n_faces_for_unit_ellipsoid); +int generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3& pose, unsigned int n_faces_for_unit_ellipsoid, FinalizeModel finalize_model = FinalizeModel::DO); -/// @brief Generate BVH model from cylinder, given the number of segments along circle and the number of segments along axis. +/** +@brief Generate BVH model from cylinder, given the number of segments along circle and the number of segments along axis. +@param[in] circle_split_tot The number of segments the bottom plate of the cylinder is split into +@param[in] h_num The number of segments along the axis the cylinder is split into +**/ template -void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3& pose, unsigned int tot, unsigned int h_num); - -/// @brief Generate BVH model from cylinder -/// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cylinder with -/// larger radius, the number of circle split number is r * tot. +int generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model = FinalizeModel::DO); + +/** +@brief Generate BVH model from cylinder +@details Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cylinder with + larger radius, the number of circle split number is r * tot. +@param[in] circle_split_tot_for_unit_cylinder The number of segments the bottom plate of an equivalent unit-sized cylinder would be split into +**/ template -void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3& pose, unsigned int tot_for_unit_cylinder); +int generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3& pose, unsigned int circle_split_tot_for_unit_cylinder, FinalizeModel finalize_model = FinalizeModel::DO); -/// @brief Generate BVH model from cone, given the number of segments along circle and the number of segments along axis. -template -void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3& pose, unsigned int tot, unsigned int h_num); -/// @brief Generate BVH model from cone -/// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cone with -/// larger radius, the number of circle split number is r * tot. +/** +@brief Generate BVH model from cone +@param[in] circle_split_tot The number of segments the bottom plate is split into +@param[in] h_num an The number of segments along the axis the cone is split into +**/ +template +int generateBVHModel(BVHModel& model, const Cone& shape, const Transform3& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model = FinalizeModel::DO); + +/** +@brief Generate BVH model from cone +@details Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cone with + larger radius, the number of circle split number is r * tot. +@param[in] circle_split_tot_for_unit_cone The number of segments the bottom plate of an equivalent unit-sized cone would be split into +**/ template -void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3& pose, unsigned int tot_for_unit_cone); +int generateBVHModel(BVHModel& model, const Cone& shape, const Transform3& pose, unsigned int circle_split_tot_for_unit_cone, FinalizeModel finalize_model = FinalizeModel::DO); + +/**@} */ // end of doxygen group generateBVHModel } // namespace fcl #include "fcl/geometry/geometric_shape_to_BVH_model-inl.h" -#endif +#endif \ No newline at end of file diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index cc599b137..b21c05adb 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -52,6 +52,7 @@ set(tests test_fcl_distance.cpp test_fcl_frontlist.cpp test_fcl_general.cpp + test_fcl_generate_bvh_model_deferred_finalize.cpp test_fcl_geometric_shapes.cpp test_fcl_math.cpp test_fcl_profiler.cpp diff --git a/test/test_fcl_generate_bvh_model_deferred_finalize.cpp b/test/test_fcl_generate_bvh_model_deferred_finalize.cpp new file mode 100644 index 000000000..11e9a0dc3 --- /dev/null +++ b/test/test_fcl_generate_bvh_model_deferred_finalize.cpp @@ -0,0 +1,331 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, Open Source Robotics Foundation + * 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 Open Source Robotics Foundation 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 Nico van Duijn */ + +#include + +#include "fcl/config.h" +#include "fcl/geometry/geometric_shape_to_BVH_model.h" +#include "test_fcl_utility.h" +#include + +using namespace fcl; + +/** +@description This file tests functionality in generateBVHModel(). Specifically, + it tests that a BVHModel can be either finalized after adding a + geometric primitive, or left "open" in order to add further + shapes at a later time. This functionality is tested without any + regard to proper functionality or special cases in the conversion from + geometric primitive to BVHModel. +**/ + + +/** +@details This function tests adding geometric primitives to an empty model. + It checks proper functionality of those simply by + verifying the return value, the number of vertices, triangles and the state of the model. + In the process, the provided model will always be BVH_BUILD_STATE_BEGUN afterwards +**/ +template +void checkAddToEmptyModel(BVHModel& model, const ShapeType& shape) +{ + using S = typename BV::S; + uint8_t n = 32; // Hard-coded mesh-resolution. Not testing corner cases where n=0 or such + int ret; + + // Make sure we are given an empty model + GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_EMPTY); + uint8_t num_vertices = model.num_vertices; + uint8_t num_tris = model.num_tris; + GTEST_ASSERT_EQ(num_vertices, 0); + GTEST_ASSERT_EQ(num_tris, 0); + + // Add the shape to the model and count vertices and triangles to make sure it has been created + ret = generateBVHModel(model, shape, Transform3::Identity(), n, FinalizeModel::DONT); + GTEST_ASSERT_EQ(ret, BVH_OK); + EXPECT_GT(model.num_vertices, num_vertices); + EXPECT_GT(model.num_tris, num_tris); + EXPECT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN); +} + +// Specialization for boxes +template +void checkAddToEmptyModel(BVHModel& model, const Box& shape) +{ + using S = typename BV::S; + int ret; + + // Make sure we are given an empty model + GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_EMPTY); + GTEST_ASSERT_EQ(model.num_vertices, 0); + GTEST_ASSERT_EQ(model.num_tris, 0); + + // Add the shape to the model and count vertices and triangles to make sure it has been created + ret = generateBVHModel(model, shape, Transform3::Identity(), FinalizeModel::DONT); + GTEST_ASSERT_EQ(ret, BVH_OK); + EXPECT_EQ(model.num_vertices, 8); + EXPECT_EQ(model.num_tris, 12); + EXPECT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN); +} + + +/** +@details This function tests adding geometric primitives to an unfinalized model. + It checks proper functionality by verifying the return value, + the number of vertices, triangles and the state of the model. + After execution, the provided model will always be BVH_BUILD_STATE_BEGUN. +**/ +template +void checkAddToUnfinalizedModel(BVHModel& model, const ShapeType& shape) +{ + using S = typename BV::S; + const uint8_t n = 32; + int ret; + + // Make sure we are given a model that is already begun + GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN); + uint8_t num_vertices = model.num_vertices; + uint8_t num_tris = model.num_tris; + + // Add the shape to the model and count vertices and triangles to make sure it has been created + ret = generateBVHModel(model, shape, Transform3(Translation3(Vector3(2.0, 2.0, 2.0))), n, FinalizeModel::DONT); + GTEST_ASSERT_EQ(ret, BVH_OK); + EXPECT_GT(model.num_vertices, num_vertices); + EXPECT_GT(model.num_tris, num_tris); + EXPECT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN); +} + +// Specialization for boxes +template +void checkAddToUnfinalizedModel(BVHModel& model, const Box& shape) +{ + using S = typename BV::S; + int ret; + + // Make sure we are given a model that is already begun + GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN); + uint8_t num_vertices = model.num_vertices; + uint8_t num_tris = model.num_tris; + + // Add the shape to the model and count vertices and triangles to make sure it has been created + ret = generateBVHModel(model, shape, Transform3(Translation3(Vector3(2.0, 2.0, 2.0))), FinalizeModel::DONT); + GTEST_ASSERT_EQ(ret, BVH_OK); + EXPECT_EQ(model.num_vertices, num_vertices + 8); + EXPECT_EQ(model.num_tris, num_tris + 12); + EXPECT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN); +} + +/** +@details This function tests adding primitives to a previously begun model + It checks proper functionality by checking the return value, + the number of vertices and triangles and the state of the model + after execution. After this call, the model is finalized. + +**/ +template +void checkAddAndFinalizeModel(BVHModel& model, const ShapeType& shape){ + using S = typename BV::S; + const uint8_t n = 32; + int ret; + + // Make sure we are given a model that is already begun + GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN); + uint8_t num_vertices = model.num_vertices; + uint8_t num_tris = model.num_tris; + + // Add another instance of the shape and make sure it was added to the model by counting vertices and tris + ret = generateBVHModel(model, shape, Transform3(Translation3(Vector3(2.0, 2.0, 2.0))), n, FinalizeModel::DO); + GTEST_ASSERT_EQ(ret, BVH_OK); + EXPECT_GT(model.num_vertices, num_vertices); + EXPECT_GT(model.num_tris, num_tris); + EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED); +} + +// Specialization for boxes +template +void checkAddAndFinalizeModel(BVHModel& model, const Box& shape){ + using S = typename BV::S; + int ret; + + // Make sure we are given a model that is already begun + GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN); + uint8_t num_vertices = model.num_vertices; + uint8_t num_tris = model.num_tris; + + // Add another instance of the shape and make sure it was added to the model by counting vertices and tris + ret = generateBVHModel(model, shape, Transform3(Translation3(Vector3(3.0, 3.0, 3.0))), FinalizeModel::DO); + GTEST_ASSERT_EQ(ret, BVH_OK); + EXPECT_EQ(model.num_vertices, num_vertices + 8); + EXPECT_EQ(model.num_tris, num_tris + 12); + EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED); +} + + +/** +@details This function tests that adding geometric primitives to a finalized model indeed + returns the BVH error we would expect. +**/ +template +void checkAddToFinalizedModel(BVHModel& model, const ShapeType& shape) +{ + using S = typename BV::S; + const uint8_t n = 32; + + GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED); + auto ret = generateBVHModel(model, shape, Transform3::Identity(), n, FinalizeModel::DONT); + EXPECT_EQ(ret, BVH_ERR_BUILD_OUT_OF_SEQUENCE); +} + +// Specialization for boxes +template +void checkAddToFinalizedModel(BVHModel& model, const Box& shape) +{ + using S = typename BV::S; + + GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED); + auto ret = generateBVHModel(model, shape, Transform3::Identity(), FinalizeModel::DONT); + EXPECT_EQ(ret, BVH_ERR_BUILD_OUT_OF_SEQUENCE); +} + +template +void testBVHModelFromBox() +{ + using S = typename BV::S; + const S a = 1.0; + const S b = 1.0; + const S c = 1.0; + + std::shared_ptr> model(new BVHModel); + Box box(a, b, c); + + checkAddToEmptyModel(*model, box); + checkAddToUnfinalizedModel(*model, box); + checkAddAndFinalizeModel(*model, box); + checkAddToFinalizedModel(*model, box); + +} + +template +void testBVHModelFromSphere() +{ + using S = typename BV::S; + const S r = 1.0; + + Sphere sphere(r); + std::shared_ptr> model(new BVHModel); + checkAddToEmptyModel(*model, sphere); + checkAddToUnfinalizedModel(*model, sphere); + checkAddAndFinalizeModel(*model, sphere); + checkAddToFinalizedModel(*model, sphere); +} + +template +void testBVHModelFromEllipsoid() +{ + using S = typename BV::S; + const S a = 1.0; + const S b = 1.0; + const S c = 1.0; + + Ellipsoid ellipsoid(a, b, c); + std::shared_ptr> model(new BVHModel); + + checkAddToEmptyModel(*model, ellipsoid); + checkAddToUnfinalizedModel(*model, ellipsoid); + checkAddAndFinalizeModel(*model, ellipsoid); + checkAddToFinalizedModel(*model, ellipsoid); +} + +template +void testBVHModelFromCylinder() +{ + using S = typename BV::S; + const S r = 1.0; + const S h = 1.0; + + Cylinder cylinder(r, h); + std::shared_ptr> model(new BVHModel); + + checkAddToEmptyModel(*model, cylinder); + checkAddToUnfinalizedModel(*model, cylinder); + checkAddAndFinalizeModel(*model, cylinder); + checkAddToFinalizedModel(*model, cylinder); +} + +template +void testBVHModelFromCone() +{ + using S = typename BV::S; + const S r = 1.0; + const S h = 1.0; + + Cone cone(r, h); + std::shared_ptr> model(new BVHModel); + + checkAddToEmptyModel(*model, cone); + checkAddToUnfinalizedModel(*model, cone); + checkAddAndFinalizeModel(*model, cone); + checkAddToFinalizedModel(*model, cone); +} + +template +void testBVHModelFromPrimitives() +{ + testBVHModelFromBox(); + testBVHModelFromSphere(); + testBVHModelFromEllipsoid(); + testBVHModelFromCylinder(); + testBVHModelFromCone(); +} + +GTEST_TEST(FCL_GENERATE_BVH_MODELS, generating_bvh_models_from_primitives) +{ + testBVHModelFromPrimitives>(); + testBVHModelFromPrimitives>(); + testBVHModelFromPrimitives>(); + testBVHModelFromPrimitives>(); + testBVHModelFromPrimitives>(); + testBVHModelFromPrimitives >(); + testBVHModelFromPrimitives >(); + testBVHModelFromPrimitives >(); +} + +//============================================================================== +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 7641edc78f9ecf747cdf7c763ef35f9e0666d70e Mon Sep 17 00:00:00 2001 From: Hongkai Dai Date: Tue, 17 Jul 2018 13:20:27 -0700 Subject: [PATCH 04/34] Update EPA test tolerance. (#314) Update EPA test tolerance, and adds a missing set of the EPA tolerance for signed distance query. --- .../gjk_libccd-inl.h | 18 +- .../test_gjk_libccd-inl_signed_distance.cpp | 334 ++++++++++++------ 2 files changed, 240 insertions(+), 112 deletions(-) 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 572455826..47f7aab4d 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 @@ -1215,8 +1215,8 @@ static ccd_vec3_t supportEPADirection(const ccd_pt_t* polytope, * vertex of the polytope. Then we know the two objects A and B are in touching * contact. * 2. If the difference between the upper bound and lower bound of the distance - * is below sqrt(ccd->epa_tolerance), then we converge to a distance whose - * difference from the real distance is less than sqrt(ccd->epa_tolerance). + * is below ccd->epa_tolerance, then we converge to a distance whose + * difference from the real distance is less than ccd->epa_tolerance. */ static int nextSupport(const ccd_pt_t* polytope, const void* obj1, const void* obj2, const ccd_t* ccd, @@ -2253,6 +2253,7 @@ bool GJKDistanceImpl(void* obj1, ccd_support_fn supp1, void* obj2, ccd.max_iterations = max_iterations; ccd.dist_tolerance = tolerance; + ccd.epa_tolerance = tolerance; ccd_vec3_t p1_, p2_; // NOTE(JS): p1_ and p2_ are set to zeros in order to suppress uninitialized @@ -2282,6 +2283,19 @@ bool GJKDistance(void* obj1, ccd_support_fn supp1, p1, p2); } +/** + * Compute the signed distance between two mesh objects. When the objects are + * separating, the signed distance is > 0. When the objects are touching or + * penetrating, the signed distance is <= 0. + * @param tolerance. When the objects are separating, the GJK algorithm + * terminates when the change of distance between iterations is smaller than + * this tolerance. Note that this does NOT necessarily mean that the computed + * distance is within @p tolerance to the actual distance. On the other hand, + * when the objects are penetrating, the EPA algorithm terminates when the + * difference between the upper bound and the lower bound of the penetration + * depth is smaller than @p tolerance. Hence the computed penetration depth is + * within @p tolerance to the true depth. + */ template bool GJKSignedDistance(void* obj1, ccd_support_fn supp1, void* obj2, ccd_support_fn supp2, diff --git a/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_signed_distance.cpp b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_signed_distance.cpp index 6add8eb41..26b65f775 100644 --- a/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_signed_distance.cpp +++ b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_signed_distance.cpp @@ -47,63 +47,94 @@ namespace fcl { namespace detail { // Given two spheres, sphere 1 has radius1, and centered at point A, whose -// position is p_FA measured and expressed in frame F; sphere 2 has radius2, -// and centered at point B, whose position is p_FB measured and expressed in -// frame F. Computes the signed distance between the two spheres, together with -// the two closest points Na on sphere 1 and Nb on sphere 2, returns the -// position of Na and Nb expressed in frame F. +// position is p_F1 measured and expressed in frame F; sphere 2 has radius2, +// and centered at point B, whose position is p_F2 measured and expressed in +// frame F. Computes the signed distance between the two spheres. // We use the monogram notation on spatial vectors. The monogram notation is // explained in // http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html template -S ComputeSphereSphereDistance(S radius1, S radius2, const Vector3& p_FA, - const Vector3& p_FB, Vector3* p_FNa, - Vector3* p_FNb) { - S min_distance = (p_FA - p_FB).norm() - radius1 - radius2; - const Vector3 p_AB_F = - p_FB - p_FA; // The vector AB measured and expressed - // in frame F. - *p_FNa = p_FA + p_AB_F.normalized() * radius1; - *p_FNb = p_FB - p_AB_F.normalized() * radius2; +S ComputeSphereSphereDistance(S radius1, S radius2, const Vector3& p_F1, + const Vector3& p_F2) { + S min_distance = (p_F1 - p_F2).norm() - radius1 - radius2; return min_distance; } template void TestSphereToSphereGJKSignedDistance(S radius1, S radius2, - const Vector3& p_FA, - const Vector3& p_FB, S tol, - S min_distance_expected, - const Vector3& p_FNa_expected, - const Vector3& p_FNb_expected) { + const Vector3& p_F1, + const Vector3& p_F2, + S solver_tolerance, S test_tol, + S min_distance_expected) { // Test if GJKSignedDistance computes the right distance. Here we used sphere // to sphere as the geometries. The distance between sphere and sphere should // be computed using distance between primitives, instead of the GJK // algorithm. But here we choose spheres for simplicity. - + // + // There are two tolerances: the solver_tolerance, and the test_tol. + // solver_tolerance is used to determine when the algorithm terminates. If + // the objects are separated, the GJK algorithm terminates when the change of + // distance is below solver_tolerance, which does NOT mean that the separation + // distance computed by GJK is within solver_tolerance to the true distance, + // so we use test_tol as a separate tolerance to check the accuracy of the + // computed distance. + // If the objects penetrate, the EPA algorithm terminates when the difference + // between the upper bound of penetration depth and the lower bound + // is below solver_tolerance, which means that the EPA computed penetration + // depth is within solver_tolerance to the true depth. + // The tolerance is explained in GJKSignedDistance() in gjk_libccd-inl.h fcl::Sphere s1(radius1); fcl::Sphere s2(radius2); fcl::Transform3 tf1, tf2; tf1.setIdentity(); tf2.setIdentity(); - tf1.translation() = p_FA; - tf2.translation() = p_FB; + tf1.translation() = p_F1; + tf2.translation() = p_F2; void* o1 = GJKInitializer>::createGJKObject(s1, tf1); void* o2 = GJKInitializer>::createGJKObject(s2, tf2); S dist; - Vector3 p1, p2; + // N1 and N2 are the witness points on sphere 1 and 2 respectively. + Vector3 p_FN1, p_FN2; GJKSolver_libccd gjkSolver; + gjkSolver.distance_tolerance = solver_tolerance; bool res = GJKSignedDistance( o1, detail::GJKInitializer>::getSupportFunction(), o2, detail::GJKInitializer>::getSupportFunction(), gjkSolver.max_distance_iterations, gjkSolver.distance_tolerance, &dist, - &p1, &p2); + &p_FN1, &p_FN2); EXPECT_EQ(res, min_distance_expected >= 0); - EXPECT_NEAR(dist, min_distance_expected, tol); - EXPECT_TRUE(p1.isApprox(p_FNa_expected, tol)); - EXPECT_TRUE(p2.isApprox(p_FNb_expected, tol)); + EXPECT_NEAR(dist, min_distance_expected, test_tol); + // Now check if the distance between N1 and N2 matches with dist, they should + // match independent of what solver_tolerance we choose. + EXPECT_NEAR((p_FN1 - p_FN2).norm(), std::abs(dist), + fcl::constants::eps_78()); + // Check if p1 is on the boundary of sphere 1, and p2 is on the boundary of + // sphere 2. + EXPECT_NEAR((p_FN1 - p_F1).norm(), radius1, test_tol); + EXPECT_NEAR((p_FN2 - p_F2).norm(), radius2, test_tol); + // The witness points N1 and N2 are defined as by shifting geometry B with + // the vector N1 - N2, the shifted geometry B' and A are touching. Hence + // if we shift sphere 1 by p_FN2 - p_FN1, then the two spheres should be + // touching. The shifted sphere is centered at p_F1 + p_FN2 - p_FN1. + EXPECT_NEAR((p_F1 + p_FN2 - p_FN1 - p_F2).norm(), radius1 + radius2, + test_tol); + // Note that we do not check the computed witness points to the true witness + // points. There are two reasons + // 1. Generally, the witness points are NOT guaranteed to be unique (consider + // plane-to-plane contact). + // 2. Even if there are unique witness points, it is hard to infer the bounds + // on the computed witness points, based on the tolerance of the solver. This + // bounds depend on the curvature of the geometries near the contact region, + // and we do not have a general approach to compute the bound for generic + // geometries. + // On the other hand, for sphere-sphere contact, it is possible to compute + // the bounds, since the witness points are unique, and the curvature is + // constant. For the moment, we are satisfied with the test above. If the + // future maintainer wants to improve this test, he/she might compute these + // bounds for the sphere-sphere case. GJKInitializer>::deleteGJKObject(o1); GJKInitializer>::deleteGJKObject(o2); @@ -118,7 +149,7 @@ struct SphereSpecification { }; template -void TestNonCollidingSphereGJKSignedDistance(S tol) { +void TestNonCollidingSphereGJKSignedDistance() { std::vector> spheres; spheres.emplace_back(0.5, Vector3(0, 0, -1.2)); spheres.emplace_back(0.5, Vector3(1.25, 0, 0)); @@ -129,13 +160,19 @@ void TestNonCollidingSphereGJKSignedDistance(S tol) { if ((spheres[i].center - spheres[j].center).norm() > spheres[i].radius + spheres[j].radius) { // Not in collision. - Vector3 p_FNa, p_FNb; - const S min_distance_expected = ComputeSphereSphereDistance( - spheres[i].radius, spheres[j].radius, spheres[i].center, - spheres[j].center, &p_FNa, &p_FNb); - TestSphereToSphereGJKSignedDistance( - spheres[i].radius, spheres[j].radius, spheres[i].center, - spheres[j].center, tol, min_distance_expected, p_FNa, p_FNb); + for (const S solver_tolerance : {S(1e-4), S(1e-5), S(1e-6)}) { + const S min_distance_expected = + ComputeSphereSphereDistance(spheres[i].radius, spheres[j].radius, + spheres[i].center, spheres[j].center); + // When the change of distance is below solver_tolerances[k], it does + // not mean that the error in separating distance is below + // solver_tolerances[k]. Empirically we find the error is less than 10 + // * solver_tolerances[k], but there is no proof. + TestSphereToSphereGJKSignedDistance( + spheres[i].radius, spheres[j].radius, spheres[i].center, + spheres[j].center, solver_tolerance, 10 * solver_tolerance, + min_distance_expected); + } } else { GTEST_FAIL() << "The two spheres collide." << "\nSpheres[" << i << "] with radius " @@ -149,12 +186,47 @@ void TestNonCollidingSphereGJKSignedDistance(S tol) { } } +template +void TestCollidingSphereGJKSignedDistance() { + std::vector> spheres; + spheres.emplace_back(0.5, Vector3(0, 0, 0)); + spheres.emplace_back(0.5, Vector3(0.75, 0, 0)); + spheres.emplace_back(0.3, Vector3(0.2, 0, 0)); + spheres.emplace_back(0.4, Vector3(0.2, 0, 0.4)); + for (int i = 0; i < static_cast(spheres.size()); ++i) { + for (int j = i + 1; j < static_cast(spheres.size()); ++j) { + if ((spheres[i].center - spheres[j].center).norm() < + spheres[i].radius + spheres[j].radius) { + // colliding + const S min_distance_expected = + ComputeSphereSphereDistance(spheres[i].radius, spheres[j].radius, + spheres[i].center, spheres[j].center); + for (const S solver_tolerance : {S(1E-4), S(1E-5), S(1E-6)}) { + // For colliding spheres, the solver_tolerance is the bound on the + // error relative to the true answer. + TestSphereToSphereGJKSignedDistance( + spheres[i].radius, spheres[j].radius, spheres[i].center, + spheres[j].center, solver_tolerance, solver_tolerance, + min_distance_expected); + } + } else { + GTEST_FAIL() << "The two spheres failed to collide." + << "\nSpheres[" << i << "] with radius " + << spheres[i].radius << ", centered at " + << spheres[i].center.transpose() << "\nSpheres[" << j + << "] with radius " << spheres[j].radius + << ", centered at " << spheres[j].center.transpose() + << "\n"; + } + } + } +} + GTEST_TEST(FCL_GJKSignedDistance, sphere_sphere) { - // By setting gjkSolver.distance_tolerance to the default value (1E-6), the - // tolerance we get on the closest points are only up to the square root of - // 1E-6, namely 1E-3. - TestNonCollidingSphereGJKSignedDistance(1E-3); - TestNonCollidingSphereGJKSignedDistance(1E-3); + TestNonCollidingSphereGJKSignedDistance(); + TestNonCollidingSphereGJKSignedDistance(); + TestCollidingSphereGJKSignedDistance(); + TestCollidingSphereGJKSignedDistance(); } //---------------------------------------------------------------------------- @@ -172,34 +244,35 @@ struct BoxSpecification { }; template -void TestBoxesInFrameF(S tol, const Transform3& X_WF) { +void TestBoxesInFrameF(const Transform3& X_WF) { const fcl::Vector3 box1_size(1, 1, 1); const fcl::Vector3 box2_size(0.6, 0.8, 1); // Put the two boxes on the xy plane of frame F. - // B1 is the frame rigidly attached to box 1, B2 is the frame rigidly attached - // to box 2. W is the world frame. F is a frame fixed to the world. X_FB1 is - // the pose of box 1 expressed and measured in frame F, X_FB2 is the pose of - // box 2 expressed and measured in frame F. + // B1 is the frame rigidly attached to box 1, B2 is the frame rigidly + // attached to box 2. W is the world frame. F is a frame fixed to the world. + // X_FB1 is the pose of box 1 expressed and measured in frame F, X_FB2 is the + // pose of box 2 expressed and measured in frame F. fcl::Transform3 X_FB1, X_FB2; // Box 1 is fixed. X_FB1.setIdentity(); X_FB1.translation() << 0, 0, 0.5; - // First fix the orientation of box 2, such that one of its diagonal (the one - // connecting the vertex (0.3, -0.4, 1) and (-0.3, 0.4, 1) is parallel to the - // x axis in frame F. If we move the position of box 2, we get different + // First fix the orientation of box 2, such that one of its diagonal (the + // one connecting the vertex (0.3, -0.4, 1) and (-0.3, 0.4, 1) is parallel to + // the x axis in frame F. If we move the position of box 2, we get different // signed distance. X_FB2.setIdentity(); X_FB2.linear() << 0.6, -0.8, 0, 0.8, 0.6, 0, 0, 0, 1; - // p_xy_FNa is the xy position of point Na (the deepest penetration point on + // p_xy_FN1 is the xy position of point N1 (the deepest penetration point on // box 1) measured and expressed in the frame F. - // p_xy_FNb is the xy position of point Nb (the deepest penetration point on + // p_xy_FN2 is the xy position of point N2 (the deepest penetration point on // box 2) measured and expressed in the frame F. auto CheckDistance = [&box1_size, &box2_size, &X_FB1, &X_WF]( const Transform3& X_FB2, S distance_expected, - const Vector2& p_xy_FNa_expected, const Vector2& p_xy_FNb_expected, - S tol) { + const Vector2& p_xy_FN1_expected, const Vector2& p_xy_FN2_expected, + S solver_distance_tolerance, S test_distance_tolerance, + S test_witness_tolerance) { const fcl::Transform3 X_WB1 = X_WF * X_FB1; const fcl::Transform3 X_WB2 = X_WF * X_FB2; fcl::Box box1(box1_size); @@ -207,13 +280,14 @@ void TestBoxesInFrameF(S tol, const Transform3& X_WF) { void* o1 = GJKInitializer>::createGJKObject(box1, X_WB1); void* o2 = GJKInitializer>::createGJKObject(box2, X_WB2); S dist; - Vector3 p_WNa, p_WNb; + Vector3 p_WN1, p_WN2; GJKSolver_libccd gjkSolver; + gjkSolver.distance_tolerance = solver_distance_tolerance; bool res = GJKSignedDistance( o1, detail::GJKInitializer>::getSupportFunction(), o2, detail::GJKInitializer>::getSupportFunction(), gjkSolver.max_distance_iterations, gjkSolver.distance_tolerance, &dist, - &p_WNa, &p_WNb); + &p_WN1, &p_WN2); // It is unclear how FCL handles touching contact. It could return either // true or false for touching contact. So, we ignore the condition where @@ -224,93 +298,135 @@ void TestBoxesInFrameF(S tol, const Transform3& X_WF) { EXPECT_TRUE(res); } - EXPECT_NEAR(dist, distance_expected, tol); - const Vector3 p_FNa = - X_WF.linear().transpose() * (p_WNa - X_WF.translation()); - const Vector3 p_FNb = - X_WF.linear().transpose() * (p_WNb - X_WF.translation()); + EXPECT_NEAR(dist, distance_expected, test_distance_tolerance); + const Vector3 p_FN1 = + X_WF.linear().transpose() * (p_WN1 - X_WF.translation()); + const Vector3 p_FN2 = + X_WF.linear().transpose() * (p_WN2 - X_WF.translation()); - EXPECT_TRUE(p_FNa.template head<2>().isApprox(p_xy_FNa_expected, tol)); - EXPECT_TRUE(p_FNb.template head<2>().isApprox(p_xy_FNb_expected, tol)); + EXPECT_TRUE(p_FN1.template head<2>().isApprox(p_xy_FN1_expected, + test_witness_tolerance)); + EXPECT_TRUE(p_FN2.template head<2>().isApprox(p_xy_FN2_expected, + test_witness_tolerance)); // The z height of the closest points should be the same. - EXPECT_NEAR(p_FNa(2), p_FNb(2), tol); - // The closest point is within object A/B, so the z height should be within - // [0, 1] - EXPECT_GE(p_FNa(2), 0); - EXPECT_GE(p_FNb(2), 0); - EXPECT_LE(p_FNa(2), 1); - EXPECT_LE(p_FNb(2), 1); + EXPECT_NEAR(p_FN1(2), p_FN2(2), test_witness_tolerance); + // The closest point is within object A/B, so the z height should be + // within [0, 1] + EXPECT_GE(p_FN1(2), 0); + EXPECT_GE(p_FN2(2), 0); + EXPECT_LE(p_FN1(2), 1); + EXPECT_LE(p_FN2(2), 1); GJKInitializer>::deleteGJKObject(o1); GJKInitializer>::deleteGJKObject(o2); }; auto CheckBoxEdgeBoxFaceDistance = [&CheckDistance]( - const Transform3& X_FB2, S tol) { + const Transform3& X_FB2, S solver_distance_tolerance, + S test_distance_tolerance, S test_witness_tolerance) { const double expected_distance = -X_FB2.translation()(0) - 1; CheckDistance( X_FB2, expected_distance, Vector2(-0.5, X_FB2.translation()(1)), - Vector2(X_FB2.translation()(0) + 0.5, X_FB2.translation()(1)), tol); + Vector2(X_FB2.translation()(0) + 0.5, X_FB2.translation()(1)), + solver_distance_tolerance, test_distance_tolerance, + test_witness_tolerance); }; + //--------------------------------------------------------------- // Touching contact - // An edge of box 2 is touching a face of box 1 - X_FB2.translation() << -1, 0, 0.5; - CheckBoxEdgeBoxFaceDistance(X_FB2, tol); - - // The touching face on box 1 is parallel to the y axis, so shifting box 2 on - // y axis still gives touching contact. Shift box 2 on y axis by 0.1m. - X_FB2.translation() << -1, 0.1, 0.5; - CheckBoxEdgeBoxFaceDistance(X_FB2, tol); - - // Shift box 2 on y axis by -0.1m. - X_FB2.translation() << -1, -0.1, 0.5; - CheckBoxEdgeBoxFaceDistance(X_FB2, tol); - // TODO(hongkai.dai@tri.global): Add other touching contact cases, including - // face-face, face-vertex, edge-edge, edge-vertex and vertex-vertex. + // Test with different solver distance tolerances. + std::vector solver_tolerances = {S(1E-4), fcl::constants::eps_12(), + fcl::constants::eps_34(), + fcl::constants::eps_78()}; + for (int i = 0; i < static_cast(solver_tolerances.size()); ++i) { + const S solver_distance_tolerance = solver_tolerances[i]; + // For touching contact, FCL might call either GJK or EPA algorithm. When it + // calls GJK algorithm, there is no theoretical guarantee, on how the + // distance error is related to solver's distance_tolerance. + // Empirically we find 10x is reasonable. + const S test_distance_tolerance = 10 * solver_distance_tolerance; + const S test_witness_tolerance = test_distance_tolerance; + // An edge of box 2 is touching a face of box 1 + X_FB2.translation() << -1, 0, 0.5; + CheckBoxEdgeBoxFaceDistance(X_FB2, solver_distance_tolerance, + test_distance_tolerance, + test_witness_tolerance); + + // The touching face on box 1 is parallel to the y axis, so shifting box 2 + // on y axis still gives touching contact. Shift box 2 on y axis by 0.1m. + X_FB2.translation() << -1, 0.1, 0.5; + CheckBoxEdgeBoxFaceDistance(X_FB2, solver_distance_tolerance, + test_distance_tolerance, + test_witness_tolerance); + + // Shift box 2 on y axis by -0.1m. + X_FB2.translation() << -1, -0.1, 0.5; + CheckBoxEdgeBoxFaceDistance(X_FB2, solver_distance_tolerance, + test_distance_tolerance, + test_witness_tolerance); + // TODO(hongkai.dai@tri.global): Add other touching contact cases, including + // face-face, face-vertex, edge-edge, edge-vertex and vertex-vertex. + } //-------------------------------------------------------------- // Penetrating contact // An edge of box 2 penetrates into a face of box 1 - X_FB2.translation() << -0.9, 0, 0.5; - CheckBoxEdgeBoxFaceDistance(X_FB2, tol); - - // Shift box 2 on y axis by 0.1m. - X_FB2.translation() << -0.9, 0.1, 0.5; - CheckBoxEdgeBoxFaceDistance(X_FB2, tol); - - // Shift box 2 on y axis by -0.05m. - X_FB2.translation() << -0.9, -0.05, 0.5; - CheckBoxEdgeBoxFaceDistance(X_FB2, tol); - - // Shift box 2 on y axis by -0.1m. - X_FB2.translation() << -0.9, -0.1, 0.5; - CheckBoxEdgeBoxFaceDistance(X_FB2, tol); + for (int i = 0; i < static_cast(solver_tolerances.size()); ++i) { + const S solver_distance_tolerance = solver_tolerances[i]; + // For penetrating contact, FCL calls EPA algorithm. When the solver + // terminates, the computed distance should be within + // solver.distance_tolerance to the actual distance. + const S test_distance_tolerance = solver_distance_tolerance; + const S test_witness_tolerance = test_distance_tolerance; + + X_FB2.translation() << -0.9, 0, 0.5; + CheckBoxEdgeBoxFaceDistance(X_FB2, solver_distance_tolerance, + test_distance_tolerance, + test_witness_tolerance); + + // Shift box 2 on y axis by 0.1m. + X_FB2.translation() << -0.9, 0.1, 0.5; + CheckBoxEdgeBoxFaceDistance(X_FB2, solver_distance_tolerance, + test_distance_tolerance, + test_witness_tolerance); + + // Shift box 2 on y axis by -0.05m. + X_FB2.translation() << -0.9, -0.05, 0.5; + CheckBoxEdgeBoxFaceDistance(X_FB2, solver_distance_tolerance, + test_distance_tolerance, + test_witness_tolerance); + + // Shift box 2 on y axis by -0.1m. + X_FB2.translation() << -0.9, -0.1, 0.5; + CheckBoxEdgeBoxFaceDistance(X_FB2, solver_distance_tolerance, + test_distance_tolerance, + test_witness_tolerance); + } } template -void TestBoxes(S tol) { +void TestBoxes() { // Frame F coincides with the world frame W. Transform3 X_WF; X_WF.setIdentity(); - TestBoxesInFrameF(tol, X_WF); + TestBoxesInFrameF(X_WF); // Frame F is shifted from the world frame W. X_WF.translation() << 0, 0, 1; - TestBoxesInFrameF(tol, X_WF); + TestBoxesInFrameF(X_WF); X_WF.translation() << 0, 1, 0; - TestBoxesInFrameF(tol, X_WF); + TestBoxesInFrameF(X_WF); X_WF.translation() << 1, 0, 0; - TestBoxesInFrameF(tol, X_WF); + TestBoxesInFrameF(X_WF); // Frame F is rotated from the world frame W. X_WF.setIdentity(); const S kPi = fcl::constants::pi(); X_WF.linear() = Eigen::AngleAxis(0.1 * kPi, Vector3::UnitZ()).toRotationMatrix(); - TestBoxesInFrameF(tol, X_WF); + TestBoxesInFrameF(X_WF); // TODO(hongkai.dai): This test exposes an error in simplexToPolytope4, that // the initial simplex can be degenerate. Should add the special case on @@ -319,18 +435,16 @@ void TestBoxes(S tol) { X_WF.linear() = Eigen::AngleAxis(0.1 * kPi, Vector3(1.0 / 3, 2.0 / 3, -2.0 / 3)) .toRotationMatrix(); - TestBoxesInFrameF(tol, X_WF);*/ + TestBoxesInFrameF(X_WF);*/ // Frame F is rotated and shifted from the world frame W. X_WF.translation() << 0.1, 0.2, 0.3; - TestBoxesInFrameF(tol, X_WF); + TestBoxesInFrameF(X_WF); } GTEST_TEST(FCL_GJKSignedDistance, box_box) { - // By setting gjkSolver.distance_tolerance to the default value (1E-6), the - // tolerance we get on the closest points are only up to 1E-3 - TestBoxes(1E-3); - TestBoxes(1E-3); + TestBoxes(); + TestBoxes(); } } // namespace detail } // namespace fcl From 157f34fbebc8cde0d554a3d4cf87f610a711b4df Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Tue, 24 Jul 2018 09:54:48 -0700 Subject: [PATCH 05/34] Correct error in box-box primitive to allow for float-compilation Literal values were defined in box-box primitve and being interpreted as doubles. This caused errors when compiling with S = float. This fix addresses that issue. --- .../narrowphase/detail/primitive_shape_algorithm/box_box-inl.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box-inl.h index 28a32b8be..91ffeca65 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box-inl.h @@ -393,7 +393,7 @@ int boxBox2( // For small boxes (dimensions all less than 1), limit the scale factor to // be no smaller than 10 * eps. This assumes all dimensions are strictly // non-negative. - S scale_factor = max(max(A.maxCoeff(), B.maxCoeff()), 1.0) * 10 * eps; + S scale_factor = max(max(A.maxCoeff(), B.maxCoeff()), S(1.0)) * S(10) * eps; Q.array() += scale_factor; } From 8fb2ce0e3463ab7e431d4e5afee05724d45c2eeb Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Tue, 31 Jul 2018 10:46:04 -0700 Subject: [PATCH 06/34] Add custom sphere-box collision and distance tests By default, the GJK solver was being used for performing distance queries between box's and spheres. For small features, the answer was being dominated by the iterative tolerance and producing wildly problematic values. The logical thing to do is to perform sphere-box collisions using knowledge of the primitives. This commit adds the following: - A new test illustrating the error of GJK is used (see test_fcl_sphere_box.cpp) - Borrows the CompareMatrices functionality from Drake and adds it to FCL. - Adds the custom sphere-box collision (sphere_box.h and sphere_box-inl.h) - Adds *extensive* unit tests on the custom algorithm. - Ties the custom algorithm into the libccd and indep GJK solvers. - Remove a useless conflicting test from test_fcl_collision (it's only useless in retrospect). And its formulation can't help but become corrupt. - Update *other* tests that otherwise depend on box-sphere collision. --- CHANGELOG.md | 5 + .../narrowphase/detail/gjk_solver_indep-inl.h | 43 +- .../detail/gjk_solver_libccd-inl.h | 43 +- .../sphere_box-inl.h | 231 ++++++ .../primitive_shape_algorithm/sphere_box.h | 142 ++++ .../primitive_shape_algorithm/sphere_box.cpp | 60 ++ test/CMakeLists.txt | 1 + test/eigen_matrix_compare.h | 150 ++++ test/narrowphase/detail/CMakeLists.txt | 1 + .../primitive_shape_algorithm/CMakeLists.txt | 8 + .../test_sphere_box.cpp | 749 ++++++++++++++++++ test/test_fcl_collision.cpp | 102 --- test/test_fcl_geometric_shapes.cpp | 33 +- test/test_fcl_sphere_box.cpp | 216 +++++ 14 files changed, 1670 insertions(+), 114 deletions(-) create mode 100644 include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h create mode 100644 include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h create mode 100644 src/narrowphase/detail/primitive_shape_algorithm/sphere_box.cpp create mode 100644 test/eigen_matrix_compare.h create mode 100644 test/narrowphase/detail/primitive_shape_algorithm/CMakeLists.txt create mode 100644 test/narrowphase/detail/primitive_shape_algorithm/test_sphere_box.cpp create mode 100644 test/test_fcl_sphere_box.cpp diff --git a/CHANGELOG.md b/CHANGELOG.md index 7f2bebd6d..710400e79 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,10 @@ * Fixed redundant pair checking of SpatialHashingCollisionManager: [#156](https://github.com/flexible-collision-library/fcl/pull/156) +* Narrowphase + + * Add custom sphere-box collision and distance algorithms for both solvers: [#316](https://github.com/flexible-collision-library/fcl/pull/316) + * Distance * Added distance request option for computing exact negative distance: [#172](https://github.com/flexible-collision-library/fcl/pull/172) @@ -26,6 +30,7 @@ * Added CMake targets for generating API documentation: [#174](https://github.com/flexible-collision-library/fcl/pull/174) * Enabled build with SSE option by default: [#159](https://github.com/flexible-collision-library/fcl/pull/159) * Added missing copyright headers: [#149](https://github.com/flexible-collision-library/fcl/pull/149) + * Added test utility for performing equality between Eigen matrix-types (`CompareMatrices` in `test/eign_matrix_compare.h`): [#316](https://github.com/flexible-collision-library/fcl/pull/316) ### FCL 0.5.0 (2016-07-19) diff --git a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h b/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h index e75236202..4cd8482c5 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h +++ b/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h @@ -49,6 +49,7 @@ #include "fcl/narrowphase/detail/convexity_based_algorithm/gjk.h" #include "fcl/narrowphase/detail/convexity_based_algorithm/epa.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h" +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.h" @@ -181,7 +182,7 @@ bool GJKSolver_indep::shapeIntersect( // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | box | O | | | | | | O | O | | +// | box | O | O | | | | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | sphere |/////| O | | O | | | O | O | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ @@ -246,6 +247,8 @@ FCL_GJK_INDEP_SHAPE_INTERSECT(Box, detail::boxBoxIntersect) FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Capsule, detail::sphereCapsuleIntersect) +FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Box, detail::sphereBoxIntersect) + FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Halfspace, detail::sphereHalfspaceIntersect) FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Ellipsoid, Halfspace, detail::ellipsoidHalfspaceIntersect) FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Box, Halfspace, detail::boxHalfspaceIntersect) @@ -670,7 +673,7 @@ bool GJKSolver_indep::shapeSignedDistance( // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | box | | | | | | | | | | +// | box | | O | | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | sphere |/////| O | | O | | | | | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ @@ -689,6 +692,42 @@ bool GJKSolver_indep::shapeSignedDistance( // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +//============================================================================== +template +struct ShapeDistanceIndepImpl, Box> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Sphere& s1, + const Transform3& tf1, + const Box& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereBoxDistance(s1, tf1, s2, tf2, dist, p1, p2); + } +}; + +//============================================================================== +template +struct ShapeDistanceIndepImpl, Sphere> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Box& s1, + const Transform3& tf1, + const Sphere& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereBoxDistance(s2, tf2, s1, tf1, dist, p2, p1); + } +}; + //============================================================================== template struct ShapeDistanceIndepImpl, Capsule> diff --git a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h b/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h index 7f0a60b82..745583ef7 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h +++ b/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h @@ -46,6 +46,7 @@ #include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h" +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.h" @@ -177,7 +178,7 @@ bool GJKSolver_libccd::shapeIntersect( // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | box | O | | | | | | O | O | | +// | box | O | O | | | | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | sphere |/////| O | | O | | | O | O | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ @@ -242,6 +243,8 @@ FCL_GJK_LIBCCD_SHAPE_INTERSECT(Box, detail::boxBoxIntersect) FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Capsule, detail::sphereCapsuleIntersect) +FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Box, detail::sphereBoxIntersect) + FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Halfspace, detail::sphereHalfspaceIntersect) FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Ellipsoid, Halfspace, detail::ellipsoidHalfspaceIntersect) FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Box, Halfspace, detail::boxHalfspaceIntersect) @@ -651,7 +654,7 @@ bool GJKSolver_libccd::shapeDistance( // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | box | | | | | | | | | | +// | box | | O | | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | sphere |/////| O | | O | | | | | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ @@ -670,6 +673,42 @@ bool GJKSolver_libccd::shapeDistance( // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +//============================================================================== +template +struct ShapeDistanceLibccdImpl, Box> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Sphere& s1, + const Transform3& tf1, + const Box& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereBoxDistance(s1, tf1, s2, tf2, dist, p1, p2); + } +}; + +//============================================================================== +template +struct ShapeDistanceLibccdImpl, Sphere> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Box& s1, + const Transform3& tf1, + const Sphere& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereBoxDistance(s2, tf2, s1, tf1, dist, p2, p1); + } +}; + //============================================================================== template struct ShapeDistanceLibccdImpl, Capsule> diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h new file mode 100644 index 000000000..53cdace4e --- /dev/null +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h @@ -0,0 +1,231 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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 Open Source Robotics Foundation 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 (2018) */ + +#ifndef FCL_NARROWPHASE_DETAIL_SPHEREBOX_INL_H +#define FCL_NARROWPHASE_DETAIL_SPHEREBOX_INL_H + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h" + +namespace fcl { +namespace detail { + +extern template FCL_EXPORT bool +sphereBoxIntersect(const Sphere& sphere, const Transform3& X_FS, + const Box& box, const Transform3& X_FB, + std::vector>* contacts); + +//============================================================================== + +extern template FCL_EXPORT bool +sphereBoxDistance(const Sphere& sphere, const Transform3& X_FS, + const Box& box, const Transform3& X_FB, + double* distance, Vector3* p_FSb, + Vector3* p_FBs); + +//============================================================================== + +// Helper function for box-sphere queries. Given a box defined in its canonical +// frame B (i.e., aligned to the axes and centered on the origin) and a query +// point Q, determines point N, the point *inside* the box nearest to Q. Note: +// this is *not* the nearest point on the surface of the box; if Q is inside +// the box, then the nearest point is Q itself. +// @param size The size of the box to query against. +// @param p_BQ The position vector from frame B's origin to the query +// point Q measured and expressed in frame B. +// @param[out] p_BN_ptr A position vector from frame B's origin to the point N +// measured and expressed in frame B. +// @returns true if the nearest point is a different point than the query point. +// @pre P_BN_ptr must point to a valid Vector3 instance. +template +bool nearestPointInBox(const Vector3& size, const Vector3& p_BQ, + Vector3* p_BN_ptr) { + assert(p_BN_ptr != nullptr); + Vector3& p_BN = *p_BN_ptr; + // Clamp the point to the box. If we do *any* clamping we know the center was + // outside. If we did *no* clamping, the point is inside the box. + const Vector3 half_size = size / 2; + // The nearest point on the box (N) to Q measured in frame B. + bool clamped = false; + for (int i = 0; i < 3; ++i) { + p_BN(i) = p_BQ(i); + if (p_BQ(i) < -half_size(i)) { + clamped = true; + p_BN(i) = -half_size(i); + } + if (p_BQ(i) > half_size(i)) { + clamped = true; + p_BN(i) = half_size(i); + } + } + return clamped; +} +//============================================================================== + +template +FCL_EXPORT bool sphereBoxIntersect(const Sphere& sphere, + const Transform3& X_FS, const Box& box, + const Transform3& X_FB, + std::vector>* contacts) { + const S r = sphere.radius; + // Find the sphere center C in the box's frame. + const Transform3 X_BS = X_FB.inverse() * X_FS; + const Vector3 p_BC = X_BS.translation(); + + // Find N, the nearest point *inside* the box to the sphere center C (measured + // and expressed in frame B) + Vector3 p_BN; + bool N_is_not_C = nearestPointInBox(box.side, p_BC, &p_BN); + + // Compute the position vector from the nearest point N to the sphere center + // C in the common box frame B. If the center is inside the box, this will be + // the zero vector. + Vector3 p_CN_B = p_BN - p_BC; + S squared_distance = p_CN_B.squaredNorm(); + // The nearest point to the sphere is *farther* than radius, they are *not* + // penetrating. + if (squared_distance > r * r) + return false; + + // Now we know they are colliding. + + if (contacts != nullptr) { + // Return values have been requested. + S depth{0}; + Vector3 n_SB_B; // Normal pointing from sphere into box (in box's frame) + Vector3 p_BP; // Contact position (P) in the box frame. + // We want to make sure that differences exceed machine precision -- we + // don't want normal and contact position dominated by noise. However, + // because we apply an arbitrary rigid transform to the sphere's center, we + // lose bits of precision. For an arbitrary non-identity transform, 4 bits + // is the maximum possible precision loss. So, we only consider a point to + // be outside the box if it's distance is at least that epsilon. + // Furthermore, in finding the *near* face, a better candidate must be more + // than this epsilon closer to the sphere center (see the test in the + // else branch). + auto eps = 16 * constants::eps(); + if (N_is_not_C && squared_distance > eps * eps) { + // The center is on the outside. Normal direction is from C to N (computed + // above) and penetration depth is r - |p_BN - p_BC|. The contact position + // is 1/2 the penetration distance in the normal direction from p_BN. + S distance = sqrt(squared_distance); + n_SB_B = p_CN_B / distance; + depth = r - distance; + p_BP = p_BN + n_SB_B * (depth * 0.5); + } else { + // The center is inside. The sphere center projects onto all faces. The + // face that is closest defines the normal direction. The penetration + // depth is the distance to that face + radius. The position is the point + // midway between the projection point, and the point opposite the sphere + // center in the *negative* normal direction. + Vector3 half_size = box.side / 2; + S min_distance = + std::numeric_limits::Real>::infinity(); + int min_axis = -1; + for (int i = 0; i < 3; ++i) { + S dist = p_BC(i) >= 0 ? half_size(i) - p_BC(i) : p_BC(i) + half_size(i); + // To be closer, the face has to be more than epsilon closer. + if (dist + eps < min_distance) { + min_distance = dist; + min_axis = i; + } + } + n_SB_B << 0, 0, 0; + // NOTE: This sign *may* seem counter-intuitive. A center nearest the +z + // face produces a normal in the -z direction; this is because the normal + // points from the sphere and into the box; and the penetration is *into* + // the +z face (so points in the -z direction). The same logic applies to + // all other directions. + n_SB_B(min_axis) = p_BC(min_axis) >= 0 ? -1 : 1; + depth = min_distance + r; + p_BP = p_BC + n_SB_B * ((r - min_distance) / 2); + } + contacts->emplace_back(X_FB.linear() * n_SB_B, X_FB * p_BP, depth); + } + return true; +} + +//============================================================================== + +template +FCL_EXPORT bool sphereBoxDistance(const Sphere& sphere, + const Transform3& X_FS, const Box& box, + const Transform3& X_FB, S* distance, + Vector3* p_FSb, Vector3* p_FBs) { + // Find the sphere center C in the box's frame. + const Transform3 X_BS = X_FB.inverse() * X_FS; + const Vector3 p_BC = X_BS.translation(); + const S r = sphere.radius; + + // Find N, the nearest point *inside* the box to the sphere center C (measured + // and expressed in frame B) + Vector3 p_BN; + bool N_is_not_C = nearestPointInBox(box.side, p_BC, &p_BN); + + if (N_is_not_C) { + // If N is not C, we know the sphere center is *outside* the box (but we + // don't know yet if the they are completely separated). + + // Compute the position vector from the nearest point N to the sphere center + // C in the frame B. + Vector3 p_NC_B = p_BC - p_BN; + S squared_distance = p_NC_B.squaredNorm(); + if (squared_distance > r * r) { + // The distance to the nearest point is greater than the radius, we have + // proven separation. + S d{-1}; + if (distance || p_FBs || p_FSb) + d = sqrt(squared_distance); + if (distance != nullptr) + *distance = d - r; + if (p_FBs != nullptr) + *p_FBs = X_FB * p_BN; + if (p_FSb != nullptr) { + const Vector3 p_BSb = (p_NC_B / d) * (d - r) + p_BN; + *p_FSb = X_FB * p_BSb; + } + return true; + } + } + + // We didn't *prove* separation, so we must be in penetration. + if (distance != nullptr) *distance = -1; + return false; +} + +} // namespace detail +} // namespace fcl + +#endif // FCL_NARROWPHASE_DETAIL_SPHEREBOX_INL_H diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h new file mode 100644 index 000000000..87fabf8cd --- /dev/null +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h @@ -0,0 +1,142 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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 Open Source Robotics Foundation 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 (2018) */ + +#ifndef FCL_NARROWPHASE_DETAIL_SPHEREBOX_H +#define FCL_NARROWPHASE_DETAIL_SPHEREBOX_H + +#include "fcl/geometry/shape/box.h" +#include "fcl/geometry/shape/sphere.h" +#include "fcl/narrowphase/contact_point.h" + +namespace fcl { + +namespace detail { + +/** @name Custom box-sphere proximity algorithms + + These functions provide custom algorithms for analyzing the relationship + between a sphere and a box. + + These functions make use of the + [Drake monogram + notation](http://drake.mit.edu/doxygen_cxx/group__multibody__notation__basics.html) + to describe quantities (particularly the poses of shapes). + + Both shapes must be posed in a common frame (notated as F). This common frame + is typically the world frame W. Regardless, if the optional output data is + returned, it will be reported in this common frame F. + + The functions can be executed in one of two ways: to perform a strict boolean + query (is colliding/is separated) or to get data which characterizes the + colliding or separating state (e.g., penetration depth vs separating distance). + The difference in usage is defined by whether the optional out parameters + are null or non-null. In the documentation, these are labeled "(optional)". + + For these functions, if the sphere and box are detected to be *touching* this + is considered a collision. As such, a collision query would report true and + a separating distance query would report false. + */ + +// NOTE: the choice to consider touching contact as collision is predicated on +// the implementation in sphere-sphere contact. + +//@{ + +// NOTE: The handling of the discontinuities in normal and position was +// implicitly defined in the spherebox test in test_fcl_geometric_shapes.cpp. It +// provided backwards compatibility to that test. + +/** Detect collision between the sphere and box. If colliding, return + characterization of collision in the provided vector. + + The reported depth is guaranteed to be continuous with respect to the relative + pose of the two objects. The normal and contact position are guaranteed to be + continuous while the sphere center lies *outside* the box. However, if the + sphere center lies *inside* the box, there are regions of discontinuity in both + normal and contact position. This is due to the fact that there is not + necessarily a *unique* characterization of penetration depth (i.e., the sphere + center may be equidistant to multiple faces). In this case, the faces are + arbitrarily prioritized and the face with the highest priority is used to + compute normal and position. The priority order is +x, -x, +y, -y, +z, and -z. + For example: + + - If the center is near the edge between the -y and +z faces, the normal will + be defined w.r.t. to the -y face. + - If the center is near the corner of the +x, +y, & +z faces, the +x face + will be used. + + @param sphere The sphere geometry. + @param X_FS The pose of the sphere S in the common frame F. + @param box The box geometry. + @param X_FB The pose of the box B in the common frame F. + @param contacts[out] (optional) If the shapes collide, the contact point data + will be appended to the end of this vector. + @return True if the objects are colliding (including touching). + @tparam S The scalar parameter (must be a valid Eigen scalar). */ +template +FCL_EXPORT bool sphereBoxIntersect(const Sphere& sphere, + const Transform3& X_FS, const Box& box, + const Transform3& X_FB, + std::vector>* contacts); + +/** Evaluate the minimum separating distance between a sphere and box. If + separated, the nearest points on each shape will be returned in frame F. + @param sphere The sphere geometry. + @param X_FS The pose of the sphere S in the common frame F. + @param box The box geometry. + @param X_FB The pose of the box B in the common frame F. + @param distance[out] (optional) The separating distance between the box + and sphere. Set to -1 if the shapes are penetrating. + @param p_FSb[out] (optional) The closest point on the *sphere* to the box + measured and expressed in frame F. + @param p_FBs[out] (optional) The closest point on the *box* to the sphere + measured and expressed in frame F. + @return True if the objects are separated. + @tparam S The scalar parameter (must be a valid Eigen scalar). */ +template +FCL_EXPORT bool sphereBoxDistance(const Sphere& sphere, + const Transform3& X_FS, const Box& box, + const Transform3& X_FB, S* distance, + Vector3* p_FSb, Vector3* p_FBs); + +//@} + +} // namespace detail +} // namespace fcl + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h" + +#endif // FCL_NARROWPHASE_DETAIL_SPHEREBOX_H diff --git a/src/narrowphase/detail/primitive_shape_algorithm/sphere_box.cpp b/src/narrowphase/detail/primitive_shape_algorithm/sphere_box.cpp new file mode 100644 index 000000000..291ad3063 --- /dev/null +++ b/src/narrowphase/detail/primitive_shape_algorithm/sphere_box.cpp @@ -0,0 +1,60 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. 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) (2018) */ + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h" + +namespace fcl +{ + +namespace detail +{ + +//============================================================================== +template bool +sphereBoxIntersect(const Sphere& sphere, const Transform3& X_FS, + const Box& box, const Transform3& X_FB, + std::vector>* contacts); + +//============================================================================== + +template bool +sphereBoxDistance(const Sphere& sphere, const Transform3& X_FS, + const Box& box, const Transform3& X_FB, + double* distance, Vector3* p_FSb, + Vector3* p_FBs); + +} // namespace detail +} // namespace fcl diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index b21c05adb..36d435629 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -59,6 +59,7 @@ set(tests test_fcl_shape_mesh_consistency.cpp test_fcl_signed_distance.cpp test_fcl_simple.cpp + test_fcl_sphere_box.cpp test_fcl_sphere_capsule.cpp test_fcl_sphere_sphere.cpp ) diff --git a/test/eigen_matrix_compare.h b/test/eigen_matrix_compare.h new file mode 100644 index 000000000..11e88474a --- /dev/null +++ b/test/eigen_matrix_compare.h @@ -0,0 +1,150 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. 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 Open Source Robotics Foundation 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. + */ + +// This code was taken from Drake. +// https://github.com/RobotLocomotion/drake/blob/master/common/test_utilities/eigen_matrix_compare.h + +#ifndef FCL_EIGEN_MATRIX_COMPARE_H +#define FCL_EIGEN_MATRIX_COMPARE_H + +#include +#include +#include + +#include +#include + +namespace fcl { + +enum class MatrixCompareType { absolute, relative }; + +/** + * Compares two matrices to determine whether they are equal to within a certain + * threshold. + * + * @param m1 The first matrix to compare. + * @param m2 The second matrix to compare. + * @param tolerance The tolerance for determining equivalence. + * @param compare_type Whether the tolerance is absolute or relative. + * @return true if the two matrices are equal based on the specified tolerance. + */ +template +::testing::AssertionResult CompareMatrices( + const Eigen::MatrixBase& m1, + const Eigen::MatrixBase& m2, double tolerance = 0.0, + MatrixCompareType compare_type = MatrixCompareType::absolute) { + if (m1.rows() != m2.rows() || m1.cols() != m2.cols()) { + return ::testing::AssertionFailure() + << "Matrix size mismatch: (" << m1.rows() << " x " << m1.cols() + << " vs. " << m2.rows() << " x " << m2.cols() << ")"; + } + + for (int ii = 0; ii < m1.rows(); ii++) { + for (int jj = 0; jj < m1.cols(); jj++) { + // First handle the corner cases of positive infinity, negative infinity, + // and NaN + const auto both_positive_infinity = + m1(ii, jj) == std::numeric_limits::infinity() && + m2(ii, jj) == std::numeric_limits::infinity(); + + const auto both_negative_infinity = + m1(ii, jj) == -std::numeric_limits::infinity() && + m2(ii, jj) == -std::numeric_limits::infinity(); + + using std::isnan; + const auto both_nan = isnan(m1(ii, jj)) && isnan(m2(ii, jj)); + + if (both_positive_infinity || both_negative_infinity || both_nan) + continue; + + // Check for case where one value is NaN and the other is not + if ((isnan(m1(ii, jj)) && !isnan(m2(ii, jj))) || + (!isnan(m1(ii, jj)) && isnan(m2(ii, jj)))) { + return ::testing::AssertionFailure() << "NaN missmatch at (" << ii + << ", " << jj << "):\nm1 =\n" + << m1 << "\nm2 =\n" + << m2; + } + + // Determine whether the difference between the two matrices is less than + // the tolerance. + using std::abs; + const auto delta = abs(m1(ii, jj) - m2(ii, jj)); + + if (compare_type == MatrixCompareType::absolute) { + // Perform comparison using absolute tolerance. + + if (delta > tolerance) { + return ::testing::AssertionFailure() + << "Values at (" << ii << ", " << jj + << ") exceed tolerance: " << m1(ii, jj) << " vs. " + << m2(ii, jj) << ", diff = " << delta + << ", tolerance = " << tolerance << "\nm1 =\n" + << m1 << "\nm2 =\n" + << m2 << "\ndelta=\n" + << (m1 - m2); + } + } else { + // Perform comparison using relative tolerance, see: + // http://realtimecollisiondetection.net/blog/?p=89 + using std::max; + const auto max_value = max(abs(m1(ii, jj)), abs(m2(ii, jj))); + const auto relative_tolerance = + tolerance * max(decltype(max_value){1}, max_value); + + if (delta > relative_tolerance) { + return ::testing::AssertionFailure() + << "Values at (" << ii << ", " << jj + << ") exceed tolerance: " << m1(ii, jj) << " vs. " + << m2(ii, jj) << ", diff = " << delta + << ", tolerance = " << tolerance + << ", relative tolerance = " << relative_tolerance + << "\nm1 =\n" + << m1 << "\nm2 =\n" + << m2 << "\ndelta=\n" + << (m1 - m2); + } + } + } + } + + return ::testing::AssertionSuccess() << "m1 =\n" + << m1 + << "\nis approximately equal to m2 =\n" + << m2; +} + +} // namespace fcl + +#endif // FCL_EIGEN_MATRIX_COMPARE_H diff --git a/test/narrowphase/detail/CMakeLists.txt b/test/narrowphase/detail/CMakeLists.txt index c46f5f81e..8c6534c8e 100644 --- a/test/narrowphase/detail/CMakeLists.txt +++ b/test/narrowphase/detail/CMakeLists.txt @@ -1 +1,2 @@ add_subdirectory(convexity_based_algorithm) +add_subdirectory(primitive_shape_algorithm) diff --git a/test/narrowphase/detail/primitive_shape_algorithm/CMakeLists.txt b/test/narrowphase/detail/primitive_shape_algorithm/CMakeLists.txt new file mode 100644 index 000000000..8e908cf78 --- /dev/null +++ b/test/narrowphase/detail/primitive_shape_algorithm/CMakeLists.txt @@ -0,0 +1,8 @@ +set(tests + test_sphere_box.cpp +) + +# Build all the tests +foreach(test ${tests}) + add_fcl_test(${test}) +endforeach(test) diff --git a/test/narrowphase/detail/primitive_shape_algorithm/test_sphere_box.cpp b/test/narrowphase/detail/primitive_shape_algorithm/test_sphere_box.cpp new file mode 100644 index 000000000..a10d5dd67 --- /dev/null +++ b/test/narrowphase/detail/primitive_shape_algorithm/test_sphere_box.cpp @@ -0,0 +1,749 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. 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) (2018) */ + +// Tests the custom sphere-box tests: distance and collision. + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h" + +#include + +#include + +#include "eigen_matrix_compare.h" +#include "fcl/geometry/shape/box.h" +#include "fcl/geometry/shape/sphere.h" + +namespace fcl { +namespace detail { +namespace { + +// In the worst case (with arbitrary frame orientations) it seems like I'm +// losing about 4 bits of precision in the solution (compared to performing +// the equivalent query without any rotations). This encodes that bit loss to +// an epsilon value appropriate to the scalar type. +template +struct Eps { + using Real = typename constants::Real; + static Real value() { return 16 * constants::eps(); } +}; + +// NOTE: The version of Eigen in travis CI seems to be using code that when +// evaluating: X_FB.inverse() * X_FS ends up doing the equivalent of multiplying +// two 4x4 matrices (rather than exploiting the compact affine representation). +// As such, it leaks a slightly more error into the computation and this extra +// padding accounts for CI peculiarity. +template <> +struct Eps { + using Real = constants::Real; + static Real value() { return 20 * constants::eps(); } +}; + +// Utility function for evaluating points inside boxes. Tests various +// configurations of points and boxes. +template void NearestPointInBox() { + // Picking sizes that are *not* powers of two and *not* uniform in size. + Box box{S(0.6), S(1.2), S(3.6)}; + Vector3 p_BN; + Vector3 p_BQ; + + // Case: query point at origin. + p_BQ << 0, 0, 0; + bool N_is_not_C = nearestPointInBox(box.side, p_BQ, &p_BN); + EXPECT_FALSE(N_is_not_C); + EXPECT_TRUE(CompareMatrices(p_BN, p_BQ, 0, MatrixCompareType::absolute)); + + Vector3 half_size = box.side * 0.5; + // Per-octant tests: + for (S x : {-1, 1}) { + for (S y : {-1, 1}) { + for (S z : {-1, 1}) { + Vector3 quadrant{x, y, z}; + // Case: point inside (no clamped values). + p_BQ = quadrant.cwiseProduct(half_size * 0.5); + N_is_not_C = nearestPointInBox(box.side, p_BQ, &p_BN); + EXPECT_FALSE(N_is_not_C); + EXPECT_TRUE( + CompareMatrices(p_BN, p_BQ, 0, MatrixCompareType::absolute)); + + // For each axis: + for (int axis : {0, 1, 2}) { + // Case: one direction clamped. + Vector3 scale{0.5, 0.5, 0.5}; + scale(axis) = 1.5; + p_BQ = quadrant.cwiseProduct(half_size.cwiseProduct(scale)); + N_is_not_C = nearestPointInBox(box.side, p_BQ, &p_BN); + EXPECT_TRUE(N_is_not_C); + for (int i : {0, 1, 2}) { + if (i == axis) + EXPECT_EQ(p_BN(i), quadrant(i) * half_size(i)); + else + EXPECT_EQ(p_BN(i), p_BQ(i)); + } + + // Case: One direction unclamped. + scale << 1.5, 1.5, 1.5; + scale(axis) = 0.5; + p_BQ = quadrant.cwiseProduct(half_size.cwiseProduct(scale)); + N_is_not_C = nearestPointInBox(box.side, p_BQ, &p_BN); + EXPECT_TRUE(N_is_not_C); + for (int i : {0, 1, 2}) { + if (i == axis) + EXPECT_EQ(p_BN(i), p_BQ(i)); + else + EXPECT_EQ(p_BN(i), quadrant(i) * half_size(i)); + } + + // Case: query point on face in axis direction -- unclamped. + scale << 0.5, 0.5, 0.5; + scale(axis) = 1.0; + p_BQ = quadrant.cwiseProduct(half_size.cwiseProduct(scale)); + N_is_not_C = nearestPointInBox(box.side, p_BQ, &p_BN); + EXPECT_FALSE(N_is_not_C); + EXPECT_TRUE( + CompareMatrices(p_BN, p_BQ, 0, MatrixCompareType::absolute)); + } + + // Case: external point in Voronoi region of corner (all axes clamped). + p_BQ = quadrant.cwiseProduct(half_size * 1.5); + N_is_not_C = nearestPointInBox(box.side, p_BQ, &p_BN); + EXPECT_TRUE(N_is_not_C); + for (int i : {0, 1, 2}) + EXPECT_EQ(p_BN(i), quadrant(i) * half_size(i)); + } + } + } +} + +// Defines the test configuration for a single test. It includes the geometry +// and the pose of the sphere in the box's frame B. It also includes the +// expected answers in that same frame. It does not include those quantities +// that vary from test invocation to invocation (e.g., the pose of the box in +// the world frame or the *orientation* of the sphere). +// +// Collision and distance are complementary queries -- two objects in collision +// have no defined distance because they are *not* separated and vice versa. +// These configurations allow for the test of the complementarity property. +template +struct TestConfiguration { + TestConfiguration(const std::string& name_in, const Vector3& half_size_in, + S radius, const Vector3& p_BSo_in, bool colliding) + : name(name_in), half_size(half_size_in), r(radius), p_BSo(p_BSo_in), + expected_colliding(colliding) {} + + // Descriptive name of the test configuration. + std::string name; + // The half size of the axis-aligned, origin-centered box. + Vector3 half_size; + // Radius of the sphere. + S r; + // Position of the sphere's center in the box frame. + Vector3 p_BSo; + + // Indicates if this test configuration is expected to be in collision. + bool expected_colliding{false}; + + // Collision values; only valid if expected_colliding is true. + S expected_depth{-1}; + Vector3 expected_normal; + Vector3 expected_pos; + + // Distance values; only valid if expected_colliding is false. + S expected_distance{-1}; + // The points on sphere and box, respectively, closest to the others measured + // and expressed in the box frame B. Only defined if separated. + Vector3 expected_p_BSb; + Vector3 expected_p_BBs; +}; + +// Utility for creating a copy of the input configurations and appending more +// labels to the configuration name -- aids in debugging. +template +std::vector> AppendLabel( + const std::vector>& configurations, + const std::string& label) { + std::vector> configs; + for (const auto& config : configurations) { + configs.push_back(config); + configs.back().name += " - " + label; + } + return configs; +} + +// Returns a collection of configurations where sphere and box are uniformly +// scaled. +template +std::vector> GetUniformConfigurations() { + // Common configuration values + // Box and sphere dimensions. + const S w = 0.6; + const S d = 1.2; + const S h = 3.6; + const S r = 0.7; + const Vector3 half_size{w / 2, d / 2, h / 2}; + const bool collides = true; + + std::vector> configurations; + + { + // Case: Completely separated. Nearest point on the +z face. + const Vector3 p_BS{half_size(0) * S(0.5), half_size(1) * S(0.5), + half_size(2) + r * S(1.1)}; + configurations.emplace_back( + "Separated; nearest face +z", half_size, r, p_BS, !collides); + // Not colliding --> no collision values. + TestConfiguration& config = configurations.back(); + config.expected_distance = p_BS(2) - half_size(2) - r; + config.expected_p_BBs = Vector3{p_BS(0), p_BS(1), half_size(2)}; + config.expected_p_BSb = Vector3{p_BS(0), p_BS(1), p_BS(2) - r}; + } + + { + // Case: Sphere completely separated with center in vertex Voronoi region. + const Vector3 p_BS = half_size + Vector3{r, r, r} * S(1.25); + configurations.emplace_back( + "Separated; nearest +x, +y, +z corner", half_size, r, p_BS, !collides); + // Not colliding --> no collision values. + TestConfiguration& config = configurations.back(); + // position vector from sphere center (S) to nearest point on box (N). + const Vector3 r_SN = half_size - p_BS; + const S len_r_SN = r_SN.norm(); + config.expected_distance = len_r_SN - r; + config.expected_p_BBs = half_size; + config.expected_p_BSb = p_BS + r_SN * (r / len_r_SN); + } + + // Case: Intersection with the sphere center *outside* the box. + // Subcase: sphere in face voronoi region -- normal should be in face + // direction. + // Intersects the z+ face with a depth of half the radius and a normal in the + // -z direction. + { + const S target_depth = r * 0.5; + const Vector3 p_BS{half_size + Vector3{0, 0, r - target_depth}}; + configurations.emplace_back( + "Colliding: center outside, center projects onto +z face", half_size, r, + p_BS, collides); + TestConfiguration& config = configurations.back(); + config.expected_depth = target_depth; + config.expected_normal = -Vector3::UnitZ(); + config.expected_pos = Vector3{p_BS(0), p_BS(1), (h - target_depth) / 2}; + + // Colliding; no distance values required. + } + + // Subcase: sphere in vertex Voronoi region -- normal should be in the + // direction from sphere center to box corner. + { + const S target_depth = r * 0.5; + const Vector3 n_SB_B = Vector3(-1, -2, -3).normalized(); + const Vector3 p_BS = half_size - n_SB_B * (r - target_depth); + configurations.emplace_back( + "Colliding: center outside, center nearest +x, +y, +z vertex", + half_size, r, p_BS, collides); + TestConfiguration& config = configurations.back(); + config.expected_depth = target_depth; + config.expected_normal = n_SB_B; + config.expected_pos = half_size + n_SB_B * (target_depth * 0.5); + + // Colliding; no distance values required. + } + + // Case: Intersection with the sphere center *inside* the box. We create six + // tests; one for each face of the box. The center will be closest to the + // target face. For the target face f, the normal should be in the -fₙ + // direction (fₙ = normal of face f), the penetration depth is + // radius plus distance to face, and the position is half the penetration + // depth from the face in the -fₙ direction. + // The distance to the face f will be some value less than the smallest half + // size to guarantee no confusion regarding different dimensions. + const std::string axis_name[] = {"x", "y", "z"}; + const S center_inset = half_size.minCoeff() * 0.5; + for (int axis = 0; axis < 3; ++axis) { + for (int sign : {-1, 1}) { + const Vector3 dir = sign * Vector3::Unit(axis); + const Vector3 p_BS = dir * (center_inset - half_size(axis)); + configurations.emplace_back( + "Colliding: center inside, center nearest " + + std::string(sign > 0 ? "+" : "-") + axis_name[axis] + " face", + half_size, r, p_BS, collides); + TestConfiguration& config = configurations.back(); + config.expected_depth = center_inset + r; + config.expected_normal = dir; + config.expected_pos = dir * ((r + center_inset) / 2 - half_size(axis)); + + // Colliding; no distance values required. + } + } + + // TODO(SeanCurtis-TRI): Consider pushing the point off the face by less than + // epsilon. + + // Sub-case: Sphere center lies on face. + { + const Vector3 p_BS{S(0.), S(0.), half_size(2)}; + configurations.emplace_back("Sphere center lies on +z face", half_size, r, + p_BS, collides); + TestConfiguration& config = configurations.back(); + config.expected_depth = r; + config.expected_normal = -Vector3::UnitZ(); + config.expected_pos << p_BS(0), p_BS(1), p_BS(2) - r / 2; + } + + // Sub-case: Sphere center lies on corner. + { + // Alias the half_size as the coordinate of the +x, +y, +z corner. + const Vector3& p_BS = half_size; + configurations.emplace_back("Sphere center lies on +x, +y, +z corner", + half_size, r, p_BS, collides); + TestConfiguration& config = configurations.back(); + config.expected_depth = r; + // Sphere center is equidistant to three faces (+x, +y, +z). As documented, + // in this case, the +x face is selected (by aribtrary priority) and the + // normal points *into* that face. + config.expected_normal = -Vector3::UnitX(); + config.expected_pos << p_BS(0) - r / 2, p_BS(1), p_BS(2); + } + + // Case: Sphere and box origins are coincident. + + // Coincident centers subcase: The box is a cube, so the all directions + // produce the same minimum dimension; normal should point in the -x + // direction. + { + configurations.emplace_back( + "Sphere and cube origins coincident", Vector3{10, 10, 10}, 5, + Vector3::Zero(), collides); + TestConfiguration& config = configurations.back(); + config.expected_depth = 15; + config.expected_normal = -Vector3::UnitX(); + config.expected_pos << 2.5, 0, 0; + } + + // Coincident centers subcase: Box height and depth are equal and smaller than + // width; the normal should point in the negative x-direction. + { + configurations.emplace_back( + "Sphere and box coincident - x & z are minimum dimension", + Vector3{10, 15, 10}, 5, Vector3::Zero(), collides); + TestConfiguration& config = configurations.back(); + config.expected_depth = 15; + config.expected_normal = -Vector3::UnitX(); + config.expected_pos << 2.5, 0, 0; + } + + // Coincident centers subcase: Box width is the smallest dimension; the normal + // should point in the negative x-direction. + { + configurations.emplace_back( + "Sphere and box coincident - x is minimum dimension", + Vector3{10, 12, 14}, 5, Vector3::Zero(), collides); + TestConfiguration& config = configurations.back(); + config.expected_depth = 15; + config.expected_normal = -Vector3::UnitX(); + config.expected_pos << 2.5, 0, 0; + } + + // Coincident centers subcase: Box height and depth are equal and smaller than + // width; the normal should point in the negative y-direction. + { + configurations.emplace_back( + "Sphere and box coincident - y & z are minimum dimension", + Vector3{15, 10, 10}, 5, Vector3::Zero(), collides); + TestConfiguration& config = configurations.back(); + config.expected_depth = 15; + config.expected_normal = -Vector3::UnitY(); + config.expected_pos << 0, 2.5, 0; + } + + // Coincident centers subcase: Box depth is the smallest dimension; the normal + // should point in the negative y-direction. + { + configurations.emplace_back( + "Sphere and box coincident - y is minimum dimension", + Vector3{15, 10, 14}, 5, Vector3::Zero(), collides); + TestConfiguration& config = configurations.back(); + config.expected_depth = 15; + config.expected_normal = -Vector3::UnitY(); + config.expected_pos << 0, 2.5, 0; + } + + // Coincident centers subcase: Box height is the smallest dimension; the + // normal should point in the negative z-direction. + { + configurations.emplace_back( + "Sphere and box coincident - z is minimum dimension", + Vector3{15, 12, 10}, 5, Vector3::Zero(), collides); + TestConfiguration& config = configurations.back(); + config.expected_depth = 15; + config.expected_normal = -Vector3::UnitZ(); + config.expected_pos << 0, 0, 2.5; + } + + return configurations; +} + +// Returns a collection of configurations where sphere and box are scaled +// very differently. +template +std::vector> GetNonUniformConfigurations() { + std::vector> configurations; + + { + // Case: long "skinny" box and tiny sphere. Nearest feature is the +z face. + const Vector3 half_size(15, 1, 1); + const S r = 0.01; + { + // Subcase: colliding. + const Vector3 p_BS{half_size(0) * S(0.95), S(0.), + half_size(2) + r * S(0.5)}; + configurations.emplace_back("Long, skinny box collides with small sphere", + half_size, r, p_BS, true /* colliding */); + TestConfiguration& config = configurations.back(); + config.expected_normal = -Vector3::UnitZ(); + config.expected_depth = r - (p_BS(2) - half_size(2)); + config.expected_pos = + Vector3{p_BS(0), p_BS(1), + half_size(2) - config.expected_depth / 2}; + } + { + // Subcase: not-colliding. + const S distance = r * 0.1; + const Vector3 p_BS{half_size(0) * S(0.95), S(0.), + half_size(2) + r + distance}; + configurations.emplace_back( + "Long, skinny box *not* colliding with small sphere", half_size, + r, p_BS, false /* not colliding */); + TestConfiguration& config = configurations.back(); + config.expected_distance = distance; + config.expected_p_BSb = p_BS - Vector3{0, 0, r}; + config.expected_p_BBs << p_BS(0), p_BS(1), half_size(2); + } + } + + { + // Case: Large sphere collides with small box. Nearest feature is the +x, + // +y, +z corner. + const Vector3 half_size(0.1, 0.15, 0.2); + const S r = 10; + const Vector3 n_SB = Vector3{-1, -2, -3}.normalized(); + { + // Subcase: colliding. + S target_depth = half_size.minCoeff() * 0.5; + const Vector3 p_BS = half_size - n_SB * (r - target_depth); + configurations.emplace_back("Large sphere colliding with tiny box", + half_size, r, p_BS, true /* colliding */); + TestConfiguration& config = configurations.back(); + config.expected_normal = n_SB; + config.expected_depth = target_depth; + config.expected_pos = half_size + n_SB * (target_depth * 0.5); + } + { + // Subcase: not colliding. + S distance = half_size.minCoeff() * 0.1; + const Vector3 p_BS = half_size - n_SB * (r + distance); + configurations.emplace_back( + "Large sphere *not* colliding with tiny box", half_size, + r, p_BS, false /* not colliding */); + TestConfiguration& config = configurations.back(); + config.expected_distance = distance; + config.expected_p_BSb = p_BS + n_SB * r; + config.expected_p_BBs = half_size; + } + } + + return configurations; +} + +template +using EvalFunc = + std::function &, const Transform3 &, + const Matrix3 &, S)>; + +// This evaluates an instance of a test configuration and confirms the results +// match the expected data. The test configuration is defined in the box's +// frame with an unrotated sphere. The parameters provide the test +// configuration, an pose of the box's frame in the world frame. +// +// Evaluates the collision query twice. Once as the boolean "is colliding" test +// and once with the collision characterized with depth, normal, and position. +template +void EvalCollisionForTestConfiguration(const TestConfiguration& config, + const Transform3& X_WB, + const Matrix3& R_SB, + S eps) { + // Set up the experiment from input parameters and test configuration. + Box box{config.half_size * 2}; + Sphere sphere{config.r}; + Transform3 X_BS = Transform3::Identity(); + X_BS.translation() = config.p_BSo; + X_BS.linear() = R_SB; + Transform3 X_WS = X_WB * X_BS; + + bool colliding = sphereBoxIntersect(sphere, X_WS, box, X_WB, nullptr); + EXPECT_EQ(colliding, config.expected_colliding) << config.name; + + std::vector> contacts; + colliding = sphereBoxIntersect(sphere, X_WS, box, X_WB, &contacts); + EXPECT_EQ(colliding, config.expected_colliding) << config.name; + if (config.expected_colliding) { + EXPECT_EQ(contacts.size(), 1u) << config.name; + const ContactPoint& contact = contacts[0]; + EXPECT_NEAR(contact.penetration_depth, config.expected_depth, eps) + << config.name; + EXPECT_TRUE(CompareMatrices(contact.normal, + X_WB.linear() * config.expected_normal, eps, + MatrixCompareType::absolute)) + << config.name; + EXPECT_TRUE(CompareMatrices(contact.pos, X_WB * config.expected_pos, eps, + MatrixCompareType::absolute)) + << config.name; + } else { + EXPECT_EQ(contacts.size(), 0u) << config.name; + } +} + +// This evaluates an instance of a test configuration and confirms the results +// match the expected data. The test configuration is defined in the box's +// frame with an unrotated sphere. The parameters provide the test +// configuration. +// +// Evaluates the distance query twice. Once as the boolean "is separated" test +// and once with the separation characterized with distance and surface points. +template +void EvalDistanceForTestConfiguration(const TestConfiguration& config, + const Transform3& X_WB, + const Matrix3& R_SB, + S eps) { + // Set up the experiment from input parameters and test configuration. + Box box{config.half_size * 2}; + Sphere sphere{config.r}; + Transform3 X_BS = Transform3::Identity(); + X_BS.translation() = config.p_BSo; + X_BS.linear() = R_SB; + Transform3 X_WS = X_WB * X_BS; + + bool separated = sphereBoxDistance(sphere, X_WS, box, X_WB, nullptr, + nullptr, nullptr); + EXPECT_NE(separated, config.expected_colliding) << config.name; + + // Initializing this to -2, to confirm that a non-colliding scenario sets + // distance to -1. + S distance{-2}; + Vector3 p_WSb{0, 0, 0}; + Vector3 p_WBs{0, 0, 0}; + + separated = + sphereBoxDistance(sphere, X_WS, box, X_WB, &distance, &p_WSb, &p_WBs); + EXPECT_NE(separated, config.expected_colliding) << config.name; + if (!config.expected_colliding) { + EXPECT_NEAR(distance, config.expected_distance, eps) + << config.name; + EXPECT_TRUE(CompareMatrices(p_WSb, + X_WB * config.expected_p_BSb, eps, + MatrixCompareType::absolute)) + << config.name; + EXPECT_TRUE(CompareMatrices(p_WBs, + X_WB * config.expected_p_BBs, eps, + MatrixCompareType::absolute)) + << config.name; + } else { + EXPECT_EQ(distance, S(-1)) << config.name; + EXPECT_TRUE(CompareMatrices(p_WSb, Vector3::Zero(), 0, + MatrixCompareType::absolute)); + EXPECT_TRUE(CompareMatrices(p_WBs, Vector3::Zero(), 0, + MatrixCompareType::absolute)); + } +} + +// This test defines the transforms for performing the single collision test. +template +void QueryWithVaryingWorldFrames( + const std::vector>& configurations, + EvalFunc query_eval, const Matrix3& R_BS = Matrix3::Identity()) { + // Evaluate all the configurations with the given box pose in frame F. + auto evaluate_all = [&R_BS, query_eval]( + const std::vector>& configs, + const Transform3& X_FB) { + for (const auto config : configs) { + query_eval(config, X_FB, R_BS, Eps::value()); + } + }; + + // Frame F is the box frame. + Transform3 X_FB = Transform3::Identity(); + evaluate_all(AppendLabel(configurations, "X_FB = I"), X_FB); + + // Simple arbitrary translation away from the origin. + X_FB.translation() << 1.3, 2.7, 6.5; + evaluate_all(AppendLabel(configurations, "X_FB is translation"), X_FB); + + std::string axis_name[] = {"x", "y", "z"}; + // 90 degree rotation around each axis. + for (int axis = 0; axis < 3; ++axis) { + std::string label = "X_FB is 90-degree rotation around " + axis_name[axis]; + AngleAxis angle_axis{constants::pi() / 2, Vector3::Unit(axis)}; + X_FB.linear() << angle_axis.matrix(); + evaluate_all(AppendLabel(configurations, label), X_FB); + } + + // Arbitrary orientation. + { + AngleAxis angle_axis{constants::pi() / 3, + Vector3{1, 2, 3}.normalized()}; + X_FB.linear() << angle_axis.matrix(); + evaluate_all(AppendLabel(configurations, "X_FB is arbitrary rotation"), + X_FB); + } + + // Near axis aligned. + { + AngleAxis angle_axis{constants::eps_12(), Vector3::UnitX()}; + X_FB.linear() << angle_axis.matrix(); + evaluate_all(AppendLabel(configurations, "X_FB is near identity"), + X_FB); + } +} + +// Runs all test configurations across multiple poses in the world frame -- +// changing the orientation of the sphere -- should have no affect on the +// results. +template +void QueryWithOrientedSphere( + const std::vector>& configurations, + EvalFunc query_eval) { + + std::string axis_name[] = {"x", "y", "z"}; + + // 90 degree rotation around each axis. + for (int axis = 0; axis < 3; ++axis) { + AngleAxis angle_axis{constants::pi() / 2, Vector3::Unit(axis)}; + std::string label = "sphere rotate 90-degrees around " + axis_name[axis]; + QueryWithVaryingWorldFrames(AppendLabel(configurations, label), + query_eval, angle_axis.matrix()); + } + + // Arbitrary orientation. + { + AngleAxis angle_axis{constants::pi() / 3, + Vector3{1, 2, 3}.normalized()}; + std::string label = "sphere rotated arbitrarily"; + QueryWithVaryingWorldFrames(AppendLabel(configurations, label), + query_eval, angle_axis.matrix()); + } + + // Near axis aligned. + { + AngleAxis angle_axis{constants::eps_12(), Vector3::UnitX()}; + std::string label = "sphere rotated near axes"; + QueryWithVaryingWorldFrames(AppendLabel(configurations, label), + query_eval, angle_axis.matrix()); + } +} + +//====================================================================== + +// Tests the helper function that finds the closest point in the box. +GTEST_TEST(SphereBoxPrimitiveTest, NearestPointInBox) { + NearestPointInBox(); + NearestPointInBox(); +} + +// Evaluates collision on all test configurations across multiple poses in the +// world frame - but the sphere rotation is always the identity. +GTEST_TEST(SphereBoxPrimitiveTest, CollisionAcrossVaryingWorldFrames) { + QueryWithVaryingWorldFrames(GetUniformConfigurations(), + EvalCollisionForTestConfiguration); + QueryWithVaryingWorldFrames(GetUniformConfigurations(), + EvalCollisionForTestConfiguration); +} + +// Evaluates collision on all test configurations across multiple poses in the +// world frame - the sphere is rotated arbitrarily. +GTEST_TEST(SphereBoxPrimitiveTest, CollisionWithSphereRotations) { + QueryWithOrientedSphere(GetUniformConfigurations(), + EvalCollisionForTestConfiguration); + QueryWithOrientedSphere(GetUniformConfigurations(), + EvalCollisionForTestConfiguration); +} + +// Evaluates collision on a small set of configurations where the box and scale +// are of radically different scales - evaluation across multiple poses in the +// world frame. +GTEST_TEST(SphereBoxPrimitiveTest, CollisionIncompatibleScales) { + QueryWithVaryingWorldFrames(GetNonUniformConfigurations(), + EvalCollisionForTestConfiguration); + QueryWithVaryingWorldFrames( + GetNonUniformConfigurations(), + EvalCollisionForTestConfiguration); +} + +// Evaluates distance on all test configurations across multiple poses in the +// world frame - but the sphere rotation is always the identity. +GTEST_TEST(SphereBoxPrimitiveTest, DistanceAcrossVaryingWorldFrames) { + QueryWithVaryingWorldFrames(GetUniformConfigurations(), + EvalDistanceForTestConfiguration); + QueryWithVaryingWorldFrames(GetUniformConfigurations(), + EvalDistanceForTestConfiguration); +} + +// Evaluates distance on all test configurations across multiple poses in the +// world frame - the sphere is rotated arbitrarily. +GTEST_TEST(SphereBoxPrimitiveTest, DistanceWithSphereRotations) { + QueryWithOrientedSphere(GetUniformConfigurations(), + EvalDistanceForTestConfiguration); + QueryWithOrientedSphere(GetUniformConfigurations(), + EvalDistanceForTestConfiguration); +} + +// Evaluates distance on a small set of configurations where the box and scale +// are of radically different scales - evaluation across multiple poses in the +// world frame. +GTEST_TEST(SphereBoxPrimitiveTest, DistanceIncompatibleScales) { + QueryWithVaryingWorldFrames(GetNonUniformConfigurations(), + EvalDistanceForTestConfiguration); + QueryWithVaryingWorldFrames(GetNonUniformConfigurations(), + EvalDistanceForTestConfiguration); +} + +} // namespace +} // namespace detail +} // namespace fcl + +//============================================================================== +int main(int argc, char *argv[]) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/test_fcl_collision.cpp b/test/test_fcl_collision.cpp index ad68e62ff..86b373f1d 100644 --- a/test/test_fcl_collision.cpp +++ b/test/test_fcl_collision.cpp @@ -827,108 +827,6 @@ void test_mesh_mesh() } } -// Tests the tolerance value in the CollisionRequest, confirming that it -// propagates down to the solution. It creates a box and sphere and positions -// them so that they *barely* intersect and show that we get different answers -// when changing the collision request's gjk_tolerance value. -// -// The unit sphere is located at the origin. The box is rotated and positioned -// so that a single corner most deeply penetrates into the sphere from the +z -// direction. -// -// The point of this test is *not* to prove that the collision algorithm is -// correct. But merely to show that different tolerances produce different -// answers (i.e., the tolerances are getting through). The answer based on a -// smaller tolerance may or may not be a better answer; that is the subject of -// other unit tests. -// -// The test is formulated this way because in the disparity in behavior between -// the ccd and indep GJK implementations. They differ enough in response to -// tolerance, that a test that actually tested for convergence to "truth" -// became unwieldy. (Specifically, it seems the ccd implementation's answers -// would converge to an answer that differed from the *real* answer by 1e-8 or -// half the precision that I would've hoped for. -// TODO(SeanCurtis-TRI): Create test that confirms that smaller tolerances lead -// to better answers. -template -void CollisionRequestGjkToleranceTest() { - typedef typename Eigen::NumTraits::Real Real; - - using GeometryPtr_t = std::shared_ptr>; - - const S pi = fcl::constants::pi(); - const Real radius = Real(1.0); - const Real size = Real(1.0); - const Real depth = Real(1e-7); - const Vector3 contact_point{0, 0, radius - Real(0.5) * depth}; - - GeometryPtr_t box_geometry(new fcl::Box(size, size, size)); - // Rotate the box twice. The first time brings the edge into contact with the - // sphere. The second brings the corner into contact. Then position it so - // that the corner lies at the position (0, 0, Z), where Z = radius - depth. - Transform3 box_pose{AngleAxis(-pi / 4, Vector3::UnitY()) * - AngleAxis(pi / 4, Vector3::UnitX())}; - Vector3 corner{-size / 2, -size / 2, -size / 2}; - Vector3 rotated_corner = box_pose.linear() * corner; - box_pose.translation() << -rotated_corner(0), - -rotated_corner(1), -rotated_corner(2) + radius - depth; - rotated_corner = box_pose * corner; - // Confirm corner is where it is expected. - EXPECT_NEAR(rotated_corner(0), 0, Eigen::NumTraits::dummy_precision()); - EXPECT_NEAR(rotated_corner(1), 0, Eigen::NumTraits::dummy_precision()); - EXPECT_NEAR(rotated_corner(2), radius - depth, - Eigen::NumTraits::dummy_precision()); - fcl::CollisionObject box(box_geometry, box_pose); - - GeometryPtr_t sphere_geometry(new fcl::Sphere(1)); - fcl::CollisionObject sphere(sphere_geometry, Transform3::Identity()); - - auto test_tolerance = [&box, &sphere] (fcl::GJKSolverType solver_type) { - fcl::CollisionRequest request; - request.num_max_contacts = 1; - request.enable_contact = true; - request.gjk_solver_type = solver_type; - - // 1/4 of the Real's bits in precision. - const Real loose_tolerance = std::pow(Eigen::NumTraits::epsilon(), 0.25); - // 7/8 of the Real's bits in precision (7/8 = 0.875). - const Real tight_tolerance = - std::pow(Eigen::NumTraits::epsilon(), 0.875); - - request.gjk_tolerance = loose_tolerance; - fcl::CollisionResult result_loose; - fcl::collide(&box, &sphere, request, result_loose); - - request.gjk_tolerance = tight_tolerance; - fcl::CollisionResult result_tight; - fcl::collide(&box, &sphere, request, result_tight); - - if (result_loose.numContacts() == result_tight.numContacts()) { - // If there are *no* contacts reported, differences cannot be detected. - GTEST_ASSERT_EQ(1u, result_loose.numContacts()); - const Vector3& pos_loose = result_loose.getContact(0).pos; - const Vector3& pos_tight = result_tight.getContact(0).pos; - // Doing literal bit-wise comparisons. Different bits is sufficient - // evidence to prove difference. - bool is_same = pos_loose(0) == pos_tight(0) && - pos_loose(1) == pos_tight(1) && - pos_loose(2) == pos_tight(2); - EXPECT_FALSE(is_same); - } - // If the number of contacts don't match, then *clearly* the tests have - // produced different results and the tolerance made a difference. - }; - - test_tolerance(fcl::GJKSolverType::GST_INDEP); - test_tolerance(fcl::GJKSolverType::GST_LIBCCD); -} - -GTEST_TEST(FCL_COLLISION, CollisionRequestGjkTolerance) { - CollisionRequestGjkToleranceTest(); - // NOTE: FCL doesn't build for float. -// CollisionRequestGjkToleranceTest(); -} - GTEST_TEST(FCL_COLLISION, OBB_Box_test) { // test_OBB_Box_test(); diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp index 0ad349b1f..52e92115a 100755 --- a/test/test_fcl_geometric_shapes.cpp +++ b/test/test_fcl_geometric_shapes.cpp @@ -834,6 +834,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_boxbox) template void test_shapeIntersection_spherebox() { + // A collection of bool constants to make reading lists of bool easier. + const bool collides = true; + const bool check_position = true; + const bool check_depth = true; + const bool check_normal = true; + const bool check_opposite_normal = false; + Sphere s1(20); Box s2(5, 5, 5); @@ -847,36 +854,46 @@ void test_shapeIntersection_spherebox() tf1 = Transform3::Identity(); tf2 = Transform3::Identity(); - // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (-1, 0, 0). + // NOTE: The centers of the box and sphere are coincident. The documented + // convention is that the normal is aligned with the smallest dimension of + // the box, pointing in the negative direction of that axis. *This* test is + // the driving basis for that definition. contacts.resize(1); contacts[0].normal << -1, 0, 0; - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, collides, contacts, + !check_position, !check_depth, check_normal); tf1 = transform; tf2 = transform; - // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, collides, contacts, + !check_position, !check_depth, !check_normal); tf1 = Transform3::Identity(); tf2 = Transform3(Translation3(Vector3(22.5, 0, 0))); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + // As documented in sphere_box.h, touching is considered collision, so this + // should produce a collision. + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, collides, contacts, + !check_position, !check_depth, !check_normal); tf1 = transform; tf2 = transform * Transform3(Translation3(Vector3(22.501, 0, 0))); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, !collides); tf1 = Transform3::Identity(); tf2 = Transform3(Translation3(Vector3(22.4, 0, 0))); contacts.resize(1); contacts[0].normal << 1, 0, 0; - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, collides, contacts, + !check_position, !check_depth, check_normal); tf1 = transform; tf2 = transform * Transform3(Translation3(Vector3(22.4, 0, 0))); contacts.resize(1); contacts[0].normal = transform.linear() * Vector3(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-4); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, collides, contacts, + !check_position, !check_depth, check_normal, + !check_opposite_normal, 1e-4); } GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spherebox) diff --git a/test/test_fcl_sphere_box.cpp b/test/test_fcl_sphere_box.cpp new file mode 100644 index 000000000..5b4d6c53d --- /dev/null +++ b/test/test_fcl_sphere_box.cpp @@ -0,0 +1,216 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. 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 (2018) */ + +#include + +#include +#include + +#include "eigen_matrix_compare.h" +#include "fcl/narrowphase/collision_object.h" +#include "fcl/narrowphase/distance.h" + +// TODO(SeanCurtis-TRI): Modify this test so it can be re-used for the distance +// query -- such that the sphere is slightly separated instead of slightly +// penetrating. See test_sphere_box.cpp for an example. + +// This collides a box with a sphere. The box is long and skinny with size +// (w, d, h) and its geometric frame is aligned with the world frame. +// The sphere has radius r and is positioned at (sx, sy, sz) with an identity +// orientation. In this configuration, the sphere penetrates the box slightly +// on its face that faces in the +z direction. The contact is *fully* contained +// in that face. (As illustrated below.) +// +// Side view +// z small sphere +// ┆ ↓ +// ┏━━━━━━━━━━━━━━━━━━━┿━━━━━━━━━━━━◯━━━━━━┓ ┬ +// ╂┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┼┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄╂ x h +// ┗━━━━━━━━━━━━━━━━━━━┿━━━━━━━━━━━━━━━━━━━┛ ┴ +// ┆ +// +// ├────────────────── w ──────────────────┤ +// +// Top view +// y small sphere +// ┆ ↓ +// ┏━━━━━━━━━━━━━━━━━━━┿━━━━━━━━━━━━━━━━━━━┓ ┬ +// ╂┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┼┄┄┄┄┄┄┄┄┄┄┄┄◯┄┄┄┄┄┄╂ x d +// ┗━━━━━━━━━━━━━━━━━━━┿━━━━━━━━━━━━━━━━━━━┛ ┴ +// ┆ +// +// Properties of expected outcome: +// - One contact *should* be reported, +// - Penetration depth δ should be: radius - (sz - h / 2), +// - Contact point should be at: [sx, sy, h / 2 - δ / 2], and +// - Contact normal should be [0, 0, -1] (pointing from sphere into box). +// +// NOTE: Orientation of the sphere should *not* make a difference and is not +// tested here; it relies on the sphere-box primitive algorithm unit tests to +// have robustly tested that. +// +// This test *fails* if GJK is used to evaluate the collision. It passes if the +// custom sphere-box algorithm is used, and, therefore, its purpose is to +// confirm that the custom algorithm is being applied. It doesn't exhaustively +// test sphere-box collision. It merely confirms the tested primitive algorithm +// has been wired up correctly. +template +void LargeBoxSmallSphereTest(fcl::GJKSolverType solver_type) { + using fcl::Vector3; + using Real = typename fcl::constants::Real; + const Real eps = fcl::constants::eps(); + + // Configure geometry. + + // Box and sphere dimensions. + const Real w = 0.115; + const Real h = 0.0025; + const Real d = 0.01; + // The magnitude of the box's extents along each axis. + const Vector3 half_size{w / 2, d / 2, h / 2}; + const Real r = 0.0015; + auto sphere_geometry = std::make_shared>(r); + auto box_geometry = std::make_shared>(w, d, h); + + // Poses of the geometry. + fcl::Transform3 X_WB = fcl::Transform3::Identity(); + + // Compute multiple sphere locations. All at the same height to produce a + // fixed, expected penetration depth of half of its radius. The reported + // position of the contact will have the x- and y- values of the sphere + // center, but be half the target_depth below the +z face, i.e., + // pz = (h / 2) - (target_depth / 2) + const Real target_depth = r * 0.5; + // Sphere center's height (position in z). + const Real sz = h / 2 + r - target_depth; + const Real pz = h / 2 - target_depth / 2; + // This transform will get repeated modified in the nested for loop below. + fcl::Transform3 X_WS = fcl::Transform3::Identity(); + + fcl::CollisionObject sphere(sphere_geometry, X_WS); + fcl::CollisionObject box(box_geometry, X_WB); + + // Expected results. (Expected position is defined inside the for loop as it + // changes with each new position of the sphere.) + const S expected_depth = target_depth; + // This normal direction assumes the *sphere* is the first collision object. + // If the order is reversed, the normal must be likewise reversed. + const Vector3 expected_normal = -Vector3::UnitZ(); + + // Set up the collision request. + fcl::CollisionRequest collision_request(1 /* num contacts */, + true /* enable_contact */); + collision_request.gjk_solver_type = solver_type; + + // Sample around the surface of the +z face on the box for a fixed penetration + // depth. Note: the +z face extends in the +/- x and y directions up to the + // distance half_size. Notes on the selected samples: + // - We've picked positions such that the *whole* sphere projects onto the + // +z face (e.g., half_size(i) - r). This *guarantees* that the contact is + // completely contained in the +z face so there is no possible ambiguity + // in the results. + // - We've picked points out near the boundaries, in the middle, and *near* + // zero without being zero. The GJK algorithm can actually provide the + // correct result at the (eps, eps) sample points. We leave those sample + // points in to confirm no degradation. + const std::vector x_values{ + -half_size(0) + r, -half_size(0) * S(0.5), -eps, 0, eps, + half_size(0) * S(0.5), half_size(0) - r}; + const std::vector y_values{ + -half_size(1) + r, -half_size(1) * S(0.5), -eps, 0, eps, + half_size(1) * S(0.5), half_size(1) - r}; + for (Real sx : x_values) { + for (Real sy : y_values ) { + // Repose the sphere. + X_WS.translation() << sx, sy, sz; + sphere.setTransform(X_WS); + + auto evaluate_collision = [&collision_request, &X_WS]( + const fcl::CollisionObject* s1, const fcl::CollisionObject* s2, + S expected_depth, const Vector3& expected_normal, + const Vector3& expected_pos, Real eps) { + // Compute collision. + fcl::CollisionResult collision_result; + std::size_t contact_count = + fcl::collide(s1, s2, collision_request, collision_result); + + // Test answers + if (contact_count == collision_request.num_max_contacts) { + std::vector> contacts; + collision_result.getContacts(contacts); + GTEST_ASSERT_EQ(contacts.size(), collision_request.num_max_contacts); + + const fcl::Contact& contact = contacts[0]; + EXPECT_NEAR(contact.penetration_depth, expected_depth, eps) + << "Sphere at: " << X_WS.translation().transpose(); + EXPECT_TRUE(fcl::CompareMatrices(contact.normal, + expected_normal, + eps, + fcl::MatrixCompareType::absolute)) + << "Sphere at: " << X_WS.translation().transpose(); + EXPECT_TRUE(fcl::CompareMatrices( + contact.pos, expected_pos, eps, fcl::MatrixCompareType::absolute)) + << "Sphere at: " << X_WS.translation().transpose(); + } else { + EXPECT_TRUE(false) << "No contacts reported for sphere at: " + << X_WS.translation().transpose(); + } + }; + + Vector3 expected_pos{sx, sy, pz}; + evaluate_collision(&sphere, &box, expected_depth, expected_normal, + expected_pos, eps); + evaluate_collision(&box, &sphere, expected_depth, -expected_normal, + expected_pos, eps); + } + } +} + +GTEST_TEST(FCL_SPHERE_BOX, LargBoxSmallSphere_ccd) { + LargeBoxSmallSphereTest(fcl::GJKSolverType::GST_LIBCCD); + LargeBoxSmallSphereTest(fcl::GJKSolverType::GST_LIBCCD); +} + +GTEST_TEST(FCL_SPHERE_BOX, LargBoxSmallSphere_indep) { + LargeBoxSmallSphereTest(fcl::GJKSolverType::GST_INDEP); + LargeBoxSmallSphereTest(fcl::GJKSolverType::GST_INDEP); +} + +//============================================================================== +int main(int argc, char* argv[]) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 8d7dc8f7d202aefdca21dcad8c4ef104d5aa8f95 Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Fri, 3 Aug 2018 07:33:50 -0700 Subject: [PATCH 07/34] Move travis CI to use xcode 9 instead of 7.3 (#266) * Move travis CI to use xcode 9 instead of 7.3 --- .travis.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index dd2697c75..7a8eeca7c 100644 --- a/.travis.yml +++ b/.travis.yml @@ -20,11 +20,11 @@ matrix: compiler: clang env: BUILD_TYPE=Release COVERALLS=OFF - os: osx - osx_image: xcode7.3 + osx_image: xcode9 compiler: clang env: BUILD_TYPE=Debug COVERALLS=OFF - os: osx - osx_image: xcode7.3 + osx_image: xcode9 compiler: clang env: BUILD_TYPE=Release COVERALLS=OFF From a278363d0e46fa03af25c1fe88843cd36a81a6a4 Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Fri, 3 Aug 2018 20:11:46 -0700 Subject: [PATCH 08/34] Change transform mode from AffineCompact to Isometry (#318) This increases the memory footprint of the `Transform` class by 33% (ie., 3x4 to 4x4). However, the X_AB.inverse() method will now implicitly use the transpose of the linear() component rather than computing an inverse. --- CHANGELOG.md | 1 + include/fcl/common/types.h | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 710400e79..f0788d60a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,6 +11,7 @@ * Math * Switched to Eigen for math operations: [#96](https://github.com/flexible-collision-library/fcl/issues/96), [#150](https://github.com/flexible-collision-library/fcl/pull/150) + * fcl::Transform defined to be an Isometry to facilitate inverses [#318](https://github.com/flexible-collision-library/fcl/pull/318) * Broadphase diff --git a/include/fcl/common/types.h b/include/fcl/common/types.h index b5187fa62..53f49d711 100644 --- a/include/fcl/common/types.h +++ b/include/fcl/common/types.h @@ -86,7 +86,7 @@ template using Quaternion = Eigen::Quaternion; template -using Transform3 = Eigen::Transform; +using Transform3 = Eigen::Transform; template using Translation3 = Eigen::Translation; From cca120856d80223d1655c427b7da153f4375134c Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Wed, 8 Aug 2018 10:12:07 -0700 Subject: [PATCH 09/34] Add primitive cylinder-sphere collision and distance query (#321) * Add custom sphere-cylinder collision and distance tests By default, the GJK solver was being used for performing distance queries between cylinders and spheres. For small features, the answer was being dominated by the iterative tolerance and producing wildly problematic values. The logical thing to do is to perform sphere-cylinder collisions using knowledge of the primitives. This commit adds the following: - A new test illustrating the error of GJK is used (see test_fcl_sphere_cylinder.cpp) - Adds the custom sphere-cylinder collision/distance (sphere_cylinder.h and sphere_cylinder-inl.h) - Adds *extensive* unit tests on the custom algorithm. - Ties the custom algorithm into the libccd and indep GJK solvers. --- CHANGELOG.md | 1 + .../narrowphase/detail/gjk_solver_indep-inl.h | 43 +- .../detail/gjk_solver_libccd-inl.h | 43 +- .../sphere_cylinder-inl.h | 276 ++++++ .../sphere_cylinder.h | 139 +++ .../sphere_cylinder.cpp | 63 ++ test/CMakeLists.txt | 1 + .../primitive_shape_algorithm/CMakeLists.txt | 1 + .../test_sphere_cylinder.cpp | 937 ++++++++++++++++++ test/test_fcl_sphere_cylinder.cpp | 229 +++++ 10 files changed, 1729 insertions(+), 4 deletions(-) create mode 100644 include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h create mode 100644 include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h create mode 100644 src/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.cpp create mode 100644 test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp create mode 100644 test/test_fcl_sphere_cylinder.cpp diff --git a/CHANGELOG.md b/CHANGELOG.md index f0788d60a..1e4074f97 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,7 @@ * Narrowphase * Add custom sphere-box collision and distance algorithms for both solvers: [#316](https://github.com/flexible-collision-library/fcl/pull/316) + * Add custom sphere-cylinder collision and distance algorithms for both solvers: [#321](https://github.com/flexible-collision-library/fcl/pull/321) * Distance diff --git a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h b/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h index 4cd8482c5..3e386737a 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h +++ b/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h @@ -51,6 +51,7 @@ #include "fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.h" +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/box_box.h" @@ -184,7 +185,7 @@ bool GJKSolver_indep::shapeIntersect( // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | box | O | O | | | | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | sphere |/////| O | | O | | | O | O | O | +// | sphere |/////| O | | O | | O | O | O | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | ellipsoid |/////|////////| | | | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ @@ -249,6 +250,8 @@ FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Capsule, detail::sphereCapsuleInters FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Box, detail::sphereBoxIntersect) +FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Cylinder, detail::sphereCylinderIntersect) + FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Halfspace, detail::sphereHalfspaceIntersect) FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Ellipsoid, Halfspace, detail::ellipsoidHalfspaceIntersect) FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Box, Halfspace, detail::boxHalfspaceIntersect) @@ -675,7 +678,7 @@ bool GJKSolver_indep::shapeSignedDistance( // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | box | | O | | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | sphere |/////| O | | O | | | | | O | +// | sphere |/////| O | | O | | O | | | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | ellipsoid |/////|////////| | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ @@ -764,6 +767,42 @@ struct ShapeDistanceIndepImpl, Sphere> } }; +//============================================================================== +template +struct ShapeDistanceIndepImpl, Cylinder> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Sphere& s1, + const Transform3& tf1, + const Cylinder& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereCylinderDistance(s1, tf1, s2, tf2, dist, p1, p2); + } +}; + +//============================================================================== +template +struct ShapeDistanceIndepImpl, Sphere> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Cylinder& s1, + const Transform3& tf1, + const Sphere& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereCylinderDistance(s2, tf2, s1, tf1, dist, p2, p1); + } +}; + //============================================================================== template struct ShapeDistanceIndepImpl, Sphere> diff --git a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h b/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h index 745583ef7..4e6903215 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h +++ b/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h @@ -48,6 +48,7 @@ #include "fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.h" +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/box_box.h" @@ -180,7 +181,7 @@ bool GJKSolver_libccd::shapeIntersect( // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | box | O | O | | | | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | sphere |/////| O | | O | | | O | O | O | +// | sphere |/////| O | | O | | O | O | O | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | ellipsoid |/////|////////| | | | | O | O | TODO | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ @@ -245,6 +246,8 @@ FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Capsule, detail::sphereCapsuleInter FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Box, detail::sphereBoxIntersect) +FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Cylinder, detail::sphereCylinderIntersect) + FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Halfspace, detail::sphereHalfspaceIntersect) FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Ellipsoid, Halfspace, detail::ellipsoidHalfspaceIntersect) FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Box, Halfspace, detail::boxHalfspaceIntersect) @@ -656,7 +659,7 @@ bool GJKSolver_libccd::shapeDistance( // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | box | | O | | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | sphere |/////| O | | O | | | | | O | +// | sphere |/////| O | | O | | O | | | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | ellipsoid |/////|////////| | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ @@ -745,6 +748,42 @@ struct ShapeDistanceLibccdImpl, Sphere> } }; +//============================================================================== +template +struct ShapeDistanceLibccdImpl, Cylinder> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Sphere& s1, + const Transform3& tf1, + const Cylinder& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereCylinderDistance(s1, tf1, s2, tf2, dist, p1, p2); + } +}; + +//============================================================================== +template +struct ShapeDistanceLibccdImpl, Sphere> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Cylinder& s1, + const Transform3& tf1, + const Sphere& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return detail::sphereCylinderDistance(s2, tf2, s1, tf1, dist, p2, p1); + } +}; + //============================================================================== template struct ShapeDistanceLibccdImpl, Sphere> diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h new file mode 100644 index 000000000..44070c04e --- /dev/null +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h @@ -0,0 +1,276 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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 Open Source Robotics Foundation 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 (2018) */ + +#ifndef FCL_NARROWPHASE_DETAIL_SPHERECYLINDER_INL_H +#define FCL_NARROWPHASE_DETAIL_SPHERECYLINDER_INL_H + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h" + +namespace fcl { +namespace detail { + +extern template FCL_EXPORT bool +sphereCylinderIntersect(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, + std::vector>* contacts); + +//============================================================================== + +extern template FCL_EXPORT bool +sphereCylinderDistance(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, + double* distance, Vector3* p_FSc, + Vector3* p_FCs); + +//============================================================================== + +// Helper function for cylinder-sphere queries. Given a cylinder defined in its +// canonical frame C (i.e., centered on the origin with cylinder's height axis +// aligned to the Cz axis) and a query point Q, determines point N, the point +// *inside* the cylinder nearest to Q. Note: this is *not* necessarily the +// nearest point on the surface of the cylinder; if Q is inside the cylinder, +// then the nearest point is Q itself. +// @param height The height of the cylinder. +// @param radius The radius of the cylinder. +// @param p_CQ The position vector from frame C's origin to the query +// point Q measured and expressed in frame C. +// @param[out] p_CN_ptr A position vector from frame C's origin to the point N +// measured and expressed in frame C. +// @returns true if the nearest point is a different point than the query point. +// @pre p_CN_ptr must point to a valid Vector3 instance. +template +bool nearestPointInCylinder(const S& height, const S& radius, + const Vector3& p_CQ, Vector3* p_CN_ptr) { + assert(p_CN_ptr != nullptr); + Vector3& p_CN = *p_CN_ptr; + p_CN = p_CQ; + + bool clamped = false; + + // Linearly clamp along the cylinders height axis. + const S half_height = height / 2; + if (p_CQ(2) > half_height) { + clamped = true; + p_CN(2) = half_height; + } else if (p_CQ(2) < -half_height) { + clamped = true; + p_CN(2) = -half_height; + } + + // Clamp according to the circular cross section. + Vector2 r_CQ{p_CQ(0), p_CQ(1)}; + S squared_distance = r_CQ.dot(r_CQ); + if (squared_distance > radius * radius) { + // The query point lies outside the *circular* extent of the cylinder. + clamped = true; + r_CQ *= radius / sqrt(squared_distance); + p_CN(0) = r_CQ(0); + p_CN(1) = r_CQ(1); + } + + return clamped; +} + +//============================================================================== + +template +FCL_EXPORT bool sphereCylinderIntersect( + const Sphere& sphere, const Transform3& X_FS, + const Cylinder& cylinder, const Transform3& X_FC, + std::vector>* contacts) { + const S& r_s = sphere.radius; + // Find the sphere center So (abbreviated as S) in the cylinder's frame. + const Transform3 X_CS = X_FC.inverse() * X_FS; + const Vector3 p_CS = X_CS.translation(); + + // Find N, the nearest point *inside* the cylinder to the sphere center S + // (measure and expressed in frame C). + Vector3 p_CN; + bool S_is_outside = nearestPointInCylinder(cylinder.lz, cylinder.radius, p_CS, + &p_CN); + + // Compute the position vector from the sphere center S to the nearest point N + // in the cylinder frame C. If the center is inside the cylinder, this will + // be the zero vector. + const Vector3 p_SN_C = p_CN - p_CS; + const S p_SN_squared_dist = p_SN_C.squaredNorm(); + // The nearest point to the sphere is *farther* than radius; they are *not* + // penetrating. + if (p_SN_squared_dist > r_s * r_s) + return false; + + // Now we know they are colliding. + + if (contacts != nullptr) { + // Return values have been requested. + S depth{0}; + // Normal pointing from sphere into cylinder (in cylinder's frame) + Vector3 n_SC_C; + // Contact position (P) in the cylinder frame. + Vector3 p_CP; + + // We want to make sure that differences exceed machine precision -- we + // don't want normal and contact position dominated by noise. However, + // because we apply an arbitrary rigid transform to the sphere's center, we + // lose bits of precision. For an arbitrary non-identity transform, 4 bits + // is the maximum possible precision loss. So, we only consider a point to + // be outside the cylinder if it's distance is at least that epsilon. + // Furthermore, in finding the *near* face, a better candidate must be more + // than this epsilon closer to the sphere center (see the test in the + // else branch). + const auto eps = 16 * constants::eps(); + if (S_is_outside && p_SN_squared_dist > eps * eps) { + // The sphere center is *measurably outside* the cylinder. There are three + // possibilities: nearest point lies on the cap face, cap edge, or barrel. + // In all three cases, the normal points from the nearest point to the + // sphere center. Penetration depth is the radius minus the distance + // between the pair of points. And the contact point is simply half the + // depth from the nearest point in the normal direction. + + // Distance from closest point (N) to sphere center (S). + const S d_NS = sqrt(p_SN_squared_dist); + n_SC_C = p_SN_C / d_NS; + depth = r_s - d_NS; + p_CP = p_CN + n_SC_C * (depth * 0.5); + } else { + // The center is inside. It's either nearer the barrel or the end face + // (with preference for the end face). + const S& h = cylinder.lz; + const S face_distance = p_CS(2) >= 0 ? h / 2 - p_CS(2) : p_CS(2) + h / 2; + // For the barrel to be picked over the face, it must be more than + // epsilon closer to the sphere center. + + // The direction from the sphere to the nearest point on the barrel on + // the z = 0 plane. + const Vector2 n_SB_xy = Vector2(p_CS(0), p_CS(1)); + const S d_CS_xy = n_SB_xy.norm(); + const S barrel_distance = cylinder.radius - d_CS_xy; + // If the center is near the Voronoi boundary between the near face and + // the barrel, then this test would be affected by the precision loss + // inherent in computing p_CS. If we did a *strict* comparison, then + // we would get a different answer just by changing X_FC. This requires + // the barrel to be closer by an amount that subsumes the potential + // precision loss. + if (barrel_distance < face_distance - eps) { + // Closest to the barrel. + if (d_CS_xy > eps) { + // Normal towards barrel + n_SC_C << -n_SB_xy(0) / d_CS_xy, -n_SB_xy(1) / d_CS_xy, 0; + depth = r_s + barrel_distance; + p_CP = p_CS + n_SC_C * ((r_s - barrel_distance) / 2); + } else { + // Sphere center is on the central spine of the cylinder, as per + // documentation, assume we have penetration coming in from the +x + // direction. + n_SC_C = -Vector3::UnitX(); + depth = r_s + cylinder.radius; + p_CP = p_CS + n_SC_C * ((r_s - barrel_distance) / 2); + } + } else { + // Closest to the face. + n_SC_C << 0, 0, 0; + // NOTE: This sign *may* seem counter-intuitive. A center nearest the +z + // face produces a normal in the -z direction; this is because the + // normal points from the sphere and into the cylinder; and the + // penetration is *into* the +z face (so points in the -z direction). + n_SC_C(2) = p_CS(2) >= 0 ? -1 : 1; + depth = face_distance + r_s; + p_CP = p_CS + n_SC_C * ((r_s - face_distance) / 2); + } + } + contacts->emplace_back(X_FC.linear() * n_SC_C, X_FC * p_CP, depth); + } + return true; +} + +//============================================================================== + +template +FCL_EXPORT bool sphereCylinderDistance(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, S* distance, + Vector3* p_FSc, Vector3* p_FCs) { + // Find the sphere center S in the cylinder's frame. + const Transform3 X_CS = X_FC.inverse() * X_FS; + const Vector3 p_CS = X_CS.translation(); + const S r_s = sphere.radius; + + // Find N, the nearest point *inside* the cylinder to the sphere center S + // (measured and expressed in frame C). + Vector3 p_CN; + bool S_is_outside = nearestPointInCylinder(cylinder.lz, cylinder.radius, p_CS, + &p_CN); + + if (S_is_outside) { + // If N is not S, we know the sphere center is *outside* the cylinder (but + // we don't know yet if the they are completely separated). + + // Compute the position vector from the nearest point N to the sphere center + // S in the frame C. + const Vector3 p_NS_C = p_CS - p_CN; + const S p_NS_squared_dist = p_NS_C.squaredNorm(); + if (p_NS_squared_dist > r_s * r_s) { + // The distance to the nearest point is greater than the sphere radius; + // we have proven separation. + S d{-1}; + if (distance || p_FCs || p_FSc) + d = sqrt(p_NS_squared_dist); + if (distance != nullptr) + *distance = d - r_s; + if (p_FCs != nullptr) + *p_FCs = X_FC * p_CN; + if (p_FSc != nullptr) { + const Vector3 p_CSc = p_CS - (p_NS_C * r_s / d); + *p_FSc = X_FC * p_CSc; + } + return true; + } + } + + // We didn't *prove* separation, so we must be in penetration. + if (distance != nullptr) *distance = -1; + return false; +} + +} // namespace detail +} // namespace fcl + +#endif // FCL_NARROWPHASE_DETAIL_SPHERECYLINDER_INL_H diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h new file mode 100644 index 000000000..4209b03a0 --- /dev/null +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h @@ -0,0 +1,139 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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 Open Source Robotics Foundation 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 (2018) */ + +#ifndef FCL_NARROWPHASE_DETAIL_SPHERECYLINDER_H +#define FCL_NARROWPHASE_DETAIL_SPHERECYLINDER_H + +#include "fcl/geometry/shape/cylinder.h" +#include "fcl/geometry/shape/sphere.h" +#include "fcl/narrowphase/contact_point.h" + +namespace fcl { + +namespace detail { + +/** @name Custom cylinder-sphere proximity algorithms + + These functions provide custom algorithms for analyzing the relationship + between a sphere and a cylinder. + + These functions make use of the + [Drake monogram + notation](http://drake.mit.edu/doxygen_cxx/group__multibody__notation__basics.html) + to describe quantities (particularly the poses of shapes). + + Both shapes must be posed in a common frame (notated as F). This common frame + is typically the world frame W. Regardless, if the optional output data is + returned, it will be reported in this common frame F. + + The functions can be executed in one of two ways: to perform a strict boolean + query (is colliding/is separated) or to get data which characterizes the + colliding or separating state (e.g., penetration depth vs separating distance). + The difference in usage is defined by whether the optional out parameters + are null or non-null. In the documentation, these are labeled "(optional)". + + For these functions, if the sphere and cylinder are detected to be *touching* + this is considered a collision. As such, a collision query would report true + and a separating distance query would report false. + */ + +// NOTE: the choice to consider touching contact as collision is predicated on +// the implementation in sphere-sphere contact. + +//@{ + +/** Detect collision between the sphere and cylinder. If colliding, return + characterization of collision in the provided vector. + + The reported depth is guaranteed to be continuous with respect to the relative + pose. In contrast, the normal and contact position are only guaranteed to be + similarly continuous while the sphere center lies *outside* the cylinder. + However, if the sphere center lies *inside* the cylinder, there are regions of + discontinuity in both normal and contact position. This is due to the fact that + there is not necessarily a *unique* characterization of penetration depth + (e.g., the sphere center may be equidistant to both a cap face as well as the + barrel). This ambiguity is resolved through an arbitrary prioritization scheme. + If the sphere center is equidistant to both an end face and the barrel, the end + face is used for normal and contact position computation. If the sphere center + is closer to the barrel, but there is no unique solution (i.e., the sphere + center lies on the center axis), the sphere is arbitrarily considered to be + penetrating from the direction of the cylinder's +x direction (yielding a + contact normal in the cylinder's -x direction.) + + @param sphere The sphere geometry. + @param X_FS The pose of the sphere S in the common frame F. + @param cylinder The cylinder geometry. + @param X_FC The pose of the cylinder C in the common frame F. + @param contacts[out] (optional) If the shapes collide, the contact point data + will be appended to the end of this vector. + @return True if the objects are colliding (including touching). + @tparam S The scalar parameter (must be a valid Eigen scalar). */ +template +FCL_EXPORT bool sphereCylinderIntersect(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, + std::vector>* contacts); + +/** Evaluate the minimum separating distance between a sphere and cylinder. If + separated, the nearest points on each shape will be returned in frame F. + @param sphere The sphere geometry. + @param X_FS The pose of the sphere S in the common frame F. + @param cylinder The cylinder geometry. + @param X_FC The pose of the cylinder C in the common frame F. + @param distance[out] (optional) The separating distance between the cylinder + and sphere. Set to -1 if the shapes are penetrating. + @param p_FSc[out] (optional) The closest point on the *sphere* to the + cylinder measured and expressed in frame F. + @param p_FCs[out] (optional) The closest point on the *cylinder* to the + sphere measured and expressed in frame F. + @return True if the objects are separated. + @tparam S The scalar parameter (must be a valid Eigen scalar). */ +template +FCL_EXPORT bool sphereCylinderDistance(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, S* distance, + Vector3* p_FSc, Vector3* p_FCs); + +//@} + +} // namespace detail +} // namespace fcl + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h" + +#endif // FCL_NARROWPHASE_DETAIL_SPHERECYLINDER_H diff --git a/src/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.cpp b/src/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.cpp new file mode 100644 index 000000000..d9e87d599 --- /dev/null +++ b/src/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.cpp @@ -0,0 +1,63 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. 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) (2018) */ + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h" + +namespace fcl +{ + +namespace detail +{ + +template bool +sphereCylinderIntersect(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, + std::vector>* contacts); + +//============================================================================== + +template bool +sphereCylinderDistance(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, + double* distance, Vector3* p_FSc, + Vector3* p_FCs); + +} // namespace detail +} // namespace fcl diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 36d435629..1ae1ea5ce 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -61,6 +61,7 @@ set(tests test_fcl_simple.cpp test_fcl_sphere_box.cpp test_fcl_sphere_capsule.cpp + test_fcl_sphere_cylinder.cpp test_fcl_sphere_sphere.cpp ) diff --git a/test/narrowphase/detail/primitive_shape_algorithm/CMakeLists.txt b/test/narrowphase/detail/primitive_shape_algorithm/CMakeLists.txt index 8e908cf78..af5fb5455 100644 --- a/test/narrowphase/detail/primitive_shape_algorithm/CMakeLists.txt +++ b/test/narrowphase/detail/primitive_shape_algorithm/CMakeLists.txt @@ -1,5 +1,6 @@ set(tests test_sphere_box.cpp + test_sphere_cylinder.cpp ) # Build all the tests diff --git a/test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp b/test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp new file mode 100644 index 000000000..afc813024 --- /dev/null +++ b/test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp @@ -0,0 +1,937 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. 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) (2018) */ + +// Tests the custom sphere-cylinder tests: distance and collision. + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h" + +#include +#include + +#include + +#include "eigen_matrix_compare.h" +#include "fcl/geometry/shape/cylinder.h" +#include "fcl/geometry/shape/sphere.h" + +namespace fcl { +namespace detail { +namespace { + +// In the worst case (with arbitrary frame orientations) it seems like I'm +// losing about 4 bits of precision in the solution (compared to performing +// the equivalent query without any rotations). This encodes that bit loss to +// an epsilon value appropriate to the scalar type. +// +// TODO(SeanCurtis-TRI): These eps values are *not* optimal. They are the result +// of a *number* of issues. +// 1. Generally, for float the scalar must be at least 20 * ε. The arbitrary +// rotation *really* beats up on the precision. +// 2. CI uses Eigen 3.2.0. The threshold must be 22 * ε for the tests to pass. +// This is true, even for doubles. Later versions (e.g., 3.2.92, aka +// 3.3-beta1) can pass with a tolerance of 16 * ε. +// 3. Mac CI requires another bump in the multiplier for floats. So, floats here +// are 24. +// Upgrade the Eigen version so that this tolerance can be reduced. +template +struct Eps { + using Real = typename constants::Real; + static Real value() { return 22 * constants::eps(); } +}; + +template <> +struct Eps { + using Real = typename constants::Real; + static Real value() { return 24 * constants::eps(); } +}; + +// Utility function for evaluating points inside cylinders. Tests various +// configurations of points and cylinders. +template void NearestPointInCylinder() { + // Picking sizes that are *not* powers of two and *not* uniform in size. + const S r = 0.6; + const S h = 1.8; + Vector3 p_CN; + Vector3 p_CQ; + + // Case: query point at origin. + p_CQ << 0, 0, 0; + bool N_is_not_S = nearestPointInCylinder(h, r, p_CQ, &p_CN); + EXPECT_FALSE(N_is_not_S) << "point at origin"; + EXPECT_TRUE(CompareMatrices(p_CN, p_CQ, 0, MatrixCompareType::absolute)) + << "point at origin"; + + // Per cylinder-half tests (i.e., above and below the z = 0 plane). + for (S z_sign : {-1, 1}) { + for (const auto& dir : {Vector3(1, 0, 0), + Vector3(0, 1, 0), + Vector3(1, 1, 0).normalized(), + Vector3(-1, 2, 0).normalized(), + Vector3(1, -2, 0).normalized(), + Vector3(-2, -3, 0).normalized()}) { + const Vector3 z_offset_internal{0, 0, h * S(0.5) * z_sign}; + const Vector3 z_offset_external{0, 0, h * S(1.5) * z_sign}; + const Vector3 radial_offset_internal = dir * (r * S(0.5)); + const Vector3 radial_offset_external = dir * (r * S(1.5)); + + using std::to_string; + + std::stringstream ss; + ss << "dir: " << dir.transpose() << ", z: " << z_sign; + const std::string parameters = ss.str(); + // Case: point inside (no clamped values). + p_CQ = radial_offset_internal + z_offset_internal; + N_is_not_S = nearestPointInCylinder(h, r, p_CQ, &p_CN); + EXPECT_FALSE(N_is_not_S) << "Sphere at origin - " << parameters; + EXPECT_TRUE( + CompareMatrices(p_CN, p_CQ, 0, MatrixCompareType::absolute)) + << "Sphere at origin - " << parameters; + + // Case: clamped only by the barrel. + p_CQ = radial_offset_external + z_offset_internal; + N_is_not_S = nearestPointInCylinder(h, r, p_CQ, &p_CN); + EXPECT_TRUE(N_is_not_S) + << "Clamped by barrel - " << parameters; + const Vector3 point_on_barrel = z_offset_internal + dir * r; + EXPECT_NEAR(point_on_barrel(0), p_CN(0), Eps::value()) + << "Clamped by barrel - " << parameters; + EXPECT_NEAR(point_on_barrel(1), p_CN(1), Eps::value()) + << "Clamped by barrel - " << parameters; + EXPECT_EQ(p_CQ(2), p_CN(2)) + << "Clamped by barrel - " << parameters; + + // Case: clamped only by the end face. + p_CQ = radial_offset_internal + z_offset_external; + N_is_not_S = nearestPointInCylinder(h, r, p_CQ, &p_CN); + EXPECT_TRUE(N_is_not_S) << "Clamped by end face - " << parameters; + EXPECT_EQ(p_CQ(0), p_CN(0)) << "Clamped by end face - " << parameters; + EXPECT_EQ(p_CQ(1), p_CN(1)) << "Clamped by end face - " << parameters; + EXPECT_EQ(0.5 * h * z_sign, p_CN(2)) << "Clamped by end face - " + << parameters; + + // Case: clamped by both end face and barrel. + p_CQ = radial_offset_external + z_offset_external; + N_is_not_S = nearestPointInCylinder(h, r, p_CQ, &p_CN); + EXPECT_TRUE(N_is_not_S) << "Fully clamped - " << parameters; + EXPECT_NEAR(point_on_barrel(0), p_CN(0), Eps::value()) + << "Fully clamped - " << parameters; + EXPECT_NEAR(point_on_barrel(1), p_CN(1), Eps::value()) + << "Fully clamped - " << parameters; + EXPECT_EQ(0.5 * h * z_sign, p_CN(2)) << "Fully clamped - " + << parameters; + } + } +} + +// Defines the test configuration for a single test. It includes the geometry +// and the pose of the sphere in the cylinder's frame C. It also includes the +// expected answers in that same frame. It does not include those quantities +// that vary from test invocation to invocation (e.g., the pose of the cylinder +// in the world frame or the *orientation* of the sphere). +// +// Collision and distance are complementary queries -- two objects in collision +// have no defined distance because they are *not* separated and vice versa. +// These configurations allow for the test of the complementarity property. +template +struct TestConfiguration { + TestConfiguration(const std::string& name_in, const S& r_c_in, + const S& h_c_in, const S& r_s_in, + const Vector3 &p_CSo_in, bool colliding) + : name(name_in), + cylinder_half_len(h_c_in / 2), + r_c(r_c_in), + r_s(r_s_in), + p_CSo(p_CSo_in), + expected_colliding(colliding) {} + + // Descriptive name of the test configuration. + std::string name; + // Half the length of the cylinder along the z-axis. + S cylinder_half_len; + // Radius of the cylinder. + S r_c; + // Radius of the sphere. + S r_s; + // Position of the sphere's center in the cylinder frame. + Vector3 p_CSo; + + // Indicates if this test configuration is expected to be in collision. + bool expected_colliding{false}; + + // Collision values; only valid if expected_colliding is true. + S expected_depth{-1}; + Vector3 expected_normal; + Vector3 expected_pos; + + // Distance values; only valid if expected_colliding is false. + S expected_distance{-1}; + // The points on sphere and cylinder, respectively, closest to the other shape + // measured and expressed in the cylinder frame C. Only defined if separated. + Vector3 expected_p_CSc; + Vector3 expected_p_CCs; +}; + +// Utility for creating a copy of the input configurations and appending more +// labels to the configuration name -- aids in debugging. +template +std::vector> AppendLabel( + const std::vector>& configurations, + const std::string& label) { + std::vector> configs; + for (const auto& config : configurations) { + configs.push_back(config); + configs.back().name += " - " + label; + } + return configs; +} + +// Returns a collection of configurations where sphere and cylinder are similar +// in size. +template +std::vector> GetUniformConfigurations() { + std::vector> configurations; + + // Common configuration values + // Cylinder and sphere dimensions. + const S h_c = 0.6; + const S half_h_c = h_c / 2; + const S r_c = 1.2; + const S r_s = 0.7; + const bool collides = true; + + // Collection of directions parallel to the z = 0 plane. Used for sampling + // queries in various directions around the barrel. Note: for the tests to be + // correct, these normals must all have unit length and a zero z-component. + std::vector> barrel_directions{ + Vector3{1, 0, 0}, + Vector3{0, 1, 0}, + Vector3(1, S(0.5), 0).normalized(), + Vector3(-1, S(0.5), 0).normalized(), + Vector3(-1, -S(0.5), 0).normalized(), + Vector3(1, -S(0.5), 0).normalized()}; + + { + // Case: Completely separated. Nearest point on the +z face. + const Vector3 p_CS{r_c * S(0.25), r_c * S(0.25), + half_h_c + r_s * S(1.1)}; + configurations.emplace_back( + "Separated; nearest face +z", r_c, h_c, r_s, p_CS, !collides); + + TestConfiguration& config = configurations.back(); + // Not colliding --> no collision values. + config.expected_distance = p_CS(2) - half_h_c - r_s; + config.expected_p_CCs = Vector3{p_CS(0), p_CS(1), half_h_c}; + config.expected_p_CSc = Vector3{p_CS(0), p_CS(1), p_CS(2) - r_s}; + } + + { + // Case: Sphere completely separated with center in barrel Voronoi region. + const S target_distance = 0.1; + const Vector3 n_SC = Vector3{-1, -1, 0}.normalized(); + const Vector3 p_CS = Vector3{0, 0, half_h_c * S(0.5)} - + n_SC * (r_s + r_c + target_distance); + configurations.emplace_back( + "Separated; near barrel", r_c, h_c, r_s, p_CS, !collides); + + TestConfiguration& config = configurations.back(); + // Not colliding --> no collision values. + config.expected_distance = target_distance; + config.expected_p_CCs = -n_SC * r_c + Vector3{0, 0, p_CS(2)}; + config.expected_p_CSc = p_CS + n_SC * r_s; + } + + { + // Case: Sphere completely separated with center in *edge* Voronoi region. + const S target_distance = .1; + const Vector3 n_SC = Vector3{-1, -1, -1}.normalized(); + const Vector3 p_CCs = Vector3{0, 0, half_h_c} + + Vector3{-n_SC(0), -n_SC(1), 0}.normalized() * r_c; + const Vector3 p_CS = p_CCs - n_SC * (r_s + target_distance); + configurations.emplace_back( + "Separated; near barrel edge", r_c, h_c, r_s, p_CS, !collides); + + TestConfiguration& config = configurations.back(); + // Not colliding --> no collision values. + config.expected_distance = target_distance; + config.expected_p_CCs = p_CCs; + config.expected_p_CSc = p_CS + n_SC * r_s; + } + + using std::min; + const S target_depth = min(r_c, r_s) * S(0.25); + + // Case contact - sphere center outside cylinder. + // Sub-cases intersection *only* through cap face; one test for each face. + for (S sign : {S(-1), S(1)}) { + const Vector3 n_SC = Vector3::UnitZ() * -sign; + const Vector3 p_CCs = Vector3{r_c * S(0.25), r_c * S(0.25), + half_h_c * sign}; + const Vector3 p_CS = p_CCs - n_SC * (r_s - target_depth); + configurations.emplace_back( + "Colliding external sphere center; cap face in " + + (sign < 0 ? std::string("-z") : std::string("+z")), + r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = target_depth; + config.expected_normal = n_SC; + config.expected_pos = p_CCs + n_SC * (target_depth / 2); + // Colliding; no distance values required. + } + + // Sub-cases intersection *only* through barrel. Sampled in multiple + // directions. + for (const Vector3& n_CS : barrel_directions) { + const Vector3 p_CCs = Vector3{0, 0, half_h_c * S(.1)} + + n_CS * r_c; + const Vector3 p_CS = p_CCs + n_CS * (r_s - target_depth); + std::stringstream ss; + ss << "Colliding external sphere center; barrel from sphere center in" + << n_CS.transpose() << " direction"; + configurations.emplace_back(ss.str(), r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = target_depth; + config.expected_normal = -n_CS; + config.expected_pos = p_CCs - n_CS * (target_depth / 2); + // Colliding; no distance values required. + } + + // Sub-cases intersection through edge. + for (S sign : {S(-1), S(1)}) { + // Projection of vector from cylinder center to sphere center on the z=0 + // plane (and then normalized). + for (const Vector3& n_CS_xy : barrel_directions) { + const Vector3 p_CCs = Vector3{0, 0, sign * half_h_c} + + n_CS_xy * r_c; + const Vector3 n_CS = p_CCs.normalized(); + const Vector3 p_CS = p_CCs + n_CS * (r_s - target_depth); + std::stringstream ss; + ss << "Colliding external sphere center; edge from sphere center in" + << n_CS.transpose() << " direction"; + configurations.emplace_back(ss.str(), r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = target_depth; + config.expected_normal = -n_CS; + config.expected_pos = p_CCs - n_CS * (target_depth / 2); + // Colliding; no distance values required. + } + } + + // Case contact - sphere center *inside* cylinder. + + // Sub-cases: sphere is unambiguously closest to end face. One test for each + // end face. + for (S sign : {S(-1), S(1)}) { + // Distance from sphere center S to face F. + const S d_SF = 0.1; + const Vector3 n_SC = Vector3::UnitZ() * -sign; + const Vector3 p_CCs = Vector3{r_c * S(0.25), r_c * S(0.25), + half_h_c * sign}; + const Vector3 p_CS = p_CCs + n_SC * d_SF; + configurations.emplace_back( + "Colliding internal sphere center; cap face in " + + (sign < 0 ? std::string("-z") : std::string("+z")), + r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = d_SF + r_s; + config.expected_normal = n_SC; + config.expected_pos = p_CCs + n_SC * (config.expected_depth / 2); + // Colliding; no distance values required. + } + + // Sub-cases: sphere is unambiguously closest to barrel; sampling multiple + // directions. + for (const Vector3& n_CS : barrel_directions) { + // Distance from sphere center S to point B on barrel. + const S d_SB = 0.1; + const Vector3 p_CCs = Vector3{0, 0, half_h_c * S(.1)} + + n_CS * r_c; + const Vector3 p_CS = p_CCs - n_CS * d_SB; + std::stringstream ss; + ss << "Colliding internal sphere center; barrel from sphere located in " + << n_CS.transpose() << " direction"; + configurations.emplace_back(ss.str(), r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = r_s + d_SB; + config.expected_normal = -n_CS; + config.expected_pos = p_CCs - n_CS * (config.expected_depth / 2); + // Colliding; no distance values required. + } + + // Case contact - sphere center is *near* error-dominated regions + + // Sub-case: Sphere center is within epsilon *outside* of end face. + // Numerically, this is processed as if the center were inside the cylinder. + // For face contact, there's no difference. This test subsumes the test where + // the center lies *on* the surface of the cylinder. + for (S sign : {S(-1), S(1)}) { + // Distance from sphere center S to face F. + const S d_SF = Eps::value() / 2; + const Vector3 n_SC = Vector3::UnitZ() * -sign; + const Vector3 p_CCs = Vector3{r_c * S(0.25), r_c * S(0.25), + half_h_c * sign}; + const Vector3 p_CS = p_CCs - n_SC * d_SF; + configurations.emplace_back( + "Colliding sphere center outside by ε; cap face in " + + (sign < 0 ? std::string("-z") : std::string("+z")), + r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = r_s; + config.expected_normal = n_SC; + config.expected_pos = p_CCs + n_SC * (config.expected_depth / 2); + // Colliding; no distance values required. + } + + // Sub-case: Sphere center is within epsilon *outside* of barrel. + // Numerically, this is processed as if the center were inside the cylinder. + // For barrel contact, there's no difference. This test subsumes the test + // where the center lies *on* the surface of the cylinder. + for (const Vector3& n_CS : barrel_directions) { + // Distance from sphere center S to point B on barrel. + const S d_SB = Eps::value() / 2; + const Vector3 p_CCs = Vector3{0, 0, half_h_c * S(.1)} + + n_CS * r_c; + const Vector3 p_CS = p_CCs - n_CS * d_SB; + std::stringstream ss; + ss << "Colliding sphere center outside by ε; barrel from sphere located in " + << n_CS.transpose() << " direction"; + configurations.emplace_back(ss.str(), r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = r_s; + config.expected_normal = -n_CS; + config.expected_pos = p_CCs - n_CS * (config.expected_depth / 2); + // Colliding; no distance values required. + } + + // Sub-case: Sphere center is within epsilon *outside* of edge. + // Numerically, this is processed as if the center were inside the cylinder. + // If the center is in the Voronoi region of the edge, the reported normal + // will be either the face or the barrel -- whichever is closer. In this + // configuration, it is the face normal. This test subsumes the test where + // the center lies *on* the surface of the cylinder. + for (S sign : {S(-1), S(1)}) { + // Projection of vector from cylinder center to sphere center on the z=0 + // plane (and then normalized). + const S d_SC = Eps::value() / 2; + for (const Vector3& n_CS_xy : barrel_directions) { + const Vector3 p_CCs = Vector3{0, 0, sign * half_h_c} + + n_CS_xy * r_c; + const Vector3 n_CS = p_CCs.normalized(); + const Vector3 p_CS = p_CCs + n_CS * d_SC; + std::stringstream ss; + ss << "Colliding sphere center outside by ε; edge from sphere center in" + << n_CS.transpose() << " direction"; + configurations.emplace_back(ss.str(), r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = r_s; + // NOTE: Epsilon *outside* is considered inside so the normal direction + // will be either face or barrel -- and, in this case, it's face. + config.expected_normal = -sign * Vector3::UnitZ(); + config.expected_pos = p_CCs + config.expected_normal * (r_s / 2); + // Colliding; no distance values required. + } + } + + { + // Sub-case: Sphere center is on origin - face is closer. It should prefer + // the +z face. + const Vector3 p_CS = Vector3::Zero(); + // Guarantee that the barrel is farther than the face. + const S big_radius = h_c * 2; + configurations.emplace_back( + "Collision with sphere at origin; face nearest", big_radius, h_c, r_s, + p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = r_s + h_c / 2; + config.expected_normal = -Vector3::UnitZ(); + config.expected_pos = Vector3{0, 0, h_c / 2 - config.expected_depth / 2}; + // Colliding; no distance values required. + } + + { + // Sub-case: Sphere center is on origin - barrel is closer. + const Vector3 p_CS = Vector3::Zero(); + // Guarantee that the barrel is closer than the face. + const S big_height = r_c * 4; + configurations.emplace_back( + "Collision with sphere at origin; barrel nearest", r_c, big_height, r_s, + p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = r_s + r_c; + config.expected_normal = -Vector3::UnitX(); + config.expected_pos = Vector3{r_c - config.expected_depth / 2, 0, 0}; + // Colliding; no distance values required. + } + + return configurations; +} + +// Returns a collection of configurations where sphere and cylinder are scaled +// very differently. +template +std::vector> GetNonUniformConfigurations() { + std::vector> configurations; + + // Case: Large, flat cylinder and tiny sphere. + { + const S r_c = 9; + const S h_c = 0.1; + const S r_s = 0.025; + const bool collides = true; + const S target_depth = r_s / 2; + + // Sub-case: Colliding -- contact with +z face. + { + // Colliding sub-case. + const Vector3 p_CCs = Vector3(1, 2, 0).normalized() * (r_c - r_s) + + Vector3::UnitZ() * (h_c / 2); + const Vector3 p_CS{p_CCs + Vector3::UnitZ() * (r_s - target_depth)}; + configurations.emplace_back( + "Collision large disk, small sphere - contact at face", + r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = target_depth; + config.expected_normal = -Vector3::UnitZ(); + config.expected_pos = p_CCs - Vector3::UnitZ() * (target_depth / 2); + // Colliding; no distance values required. + } + + // Sub-case: Separated -- nearest feature +z face. + { + // Separated sub-case. + const Vector3 p_CCs = Vector3(1, 2, 0).normalized() * (r_c - r_s) + + Vector3::UnitZ() * (h_c / 2); + const Vector3 p_CS{p_CCs + + Vector3::UnitZ() * (r_s + target_depth)}; + configurations.emplace_back( + "Separation large disk, small sphere - nearest +z face", + r_c, h_c, r_s, p_CS, !collides); + + TestConfiguration& config = configurations.back(); + // Not colliding --> no collision values. + config.expected_distance = target_depth; + config.expected_p_CCs = p_CCs; + config.expected_p_CSc = p_CS - Vector3::UnitZ() * r_s; + } + + // Sub-case: Colliding -- contact with barrel. + const Vector3 n_CS = Vector3(1, 2, 0).normalized(); + const Vector3 p_CCs = n_CS * r_c + Vector3::UnitZ() * (r_s * 0.1); + { + // Colliding sub-case. + const Vector3 p_CS{p_CCs + n_CS * (r_s - target_depth)}; + configurations.emplace_back( + "Collision large disk, small sphere - contact at barrel", + r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = target_depth; + config.expected_normal = -n_CS; + config.expected_pos = p_CCs - n_CS * (target_depth / 2); + // Colliding; no distance values required. + } + + // Sub-case: Separated -- nearest feature is barrel. + { + // Separated sub-case. + const Vector3 p_CS{p_CCs + n_CS * (r_s + target_depth)}; + configurations.emplace_back( + "Separation large disk, small sphere - nearest barrel", + r_c, h_c, r_s, p_CS, !collides); + + TestConfiguration& config = configurations.back(); + // Not colliding --> no collision values. + config.expected_distance = target_depth; + config.expected_p_CCs = p_CCs; + config.expected_p_CSc = p_CS - n_CS * r_s; + } + } + + // Case: Large sphere and *tiny* cylinder. + { + const S r_c = 0.025; + const S h_c = 0.1; + const S r_s = 9; + const bool collides = true; + const S target_depth = r_c / 2; + + // Sub-case -- nearest feature is +z face. + { + const Vector3 p_CCs = + Vector3(1, 2, 0).normalized() * (r_c * S(0.5)) + + Vector3::UnitZ() * (h_c / 2); + + // Sub-case: Colliding. + { + const Vector3 + p_CS{p_CCs + Vector3::UnitZ() * (r_s - target_depth)}; + configurations.emplace_back( + "Collision large sphere, small disk - contact at face", + r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = target_depth; + config.expected_normal = -Vector3::UnitZ(); + config.expected_pos = p_CCs - Vector3::UnitZ() * (target_depth / 2); + // Colliding; no distance values required. + } + + // Subsub-case: Separated + { + const Vector3 p_CS{p_CCs + + Vector3::UnitZ() * (r_s + target_depth)}; + configurations.emplace_back( + "Separation large sphere, small disk - nearest +z face", + r_c, h_c, r_s, p_CS, !collides); + + TestConfiguration& config = configurations.back(); + // Not colliding --> no collision values. + config.expected_distance = target_depth; + config.expected_p_CCs = p_CCs; + config.expected_p_CSc = p_CS - Vector3::UnitZ() * r_s; + } + } + + // Sub-case: Nearest feature is barrel + { + const Vector3 n_CS = Vector3(1, 2, 0).normalized(); + const Vector3 p_CCs = n_CS * r_c + Vector3::UnitZ() * (h_c * 0.1); + + // Subsub-case: Colliding. + { + const Vector3 p_CS{p_CCs + n_CS * (r_s - target_depth)}; + configurations.emplace_back( + "Collision large sphere, small disk - contact at barrel", + r_c, h_c, r_s, p_CS, collides); + + TestConfiguration& config = configurations.back(); + config.expected_depth = target_depth; + config.expected_normal = -n_CS; + config.expected_pos = p_CCs - n_CS * (target_depth / 2); + // Colliding; no distance values required. + } + + // Subsub-case: Separated . + { + const Vector3 p_CS{p_CCs + n_CS * (r_s + target_depth)}; + configurations.emplace_back( + "Separation large sphere, small disk - nearest barrel", + r_c, h_c, r_s, p_CS, !collides); + + TestConfiguration& config = configurations.back(); + // Not colliding --> no collision values. + config.expected_distance = target_depth; + config.expected_p_CCs = p_CCs; + config.expected_p_CSc = p_CS - n_CS * r_s; + } + } + } + + return configurations; +} + +template +using EvalFunc = +std::function&, const Transform3&, + const Matrix3&, S)>; + +// This evaluates an instance of a test configuration and confirms the results +// match the expected data. The test configuration is defined in the cylinder's +// frame with an unrotated sphere. The parameters provide the test parameters: +// the sphere orientation and the cylinder's pose in the world frame. +// +// Evaluates the collision query twice. Once as the boolean "is colliding" test +// and once with the collision characterized with depth, normal, and position. +template +void EvalCollisionForTestConfiguration(const TestConfiguration& config, + const Transform3& X_WC, + const Matrix3& R_CS, + S eps) { + // Set up the experiment from input parameters and test configuration. + Cylinder cylinder(config.r_c, config.cylinder_half_len * S(2)); + Sphere sphere{config.r_s}; + Transform3 X_CS = Transform3::Identity(); + X_CS.translation() = config.p_CSo; + X_CS.linear() = R_CS; + Transform3 X_WS = X_WC * X_CS; + + bool colliding = sphereCylinderIntersect(sphere, X_WS, cylinder, X_WC, + nullptr); + EXPECT_EQ(config.expected_colliding, colliding) << config.name; + + std::vector> contacts; + colliding = sphereCylinderIntersect(sphere, X_WS, cylinder, X_WC, &contacts); + EXPECT_EQ(colliding, config.expected_colliding) << config.name; + if (config.expected_colliding) { + EXPECT_EQ(1u, contacts.size()) << config.name; + const ContactPoint &contact = contacts[0]; + EXPECT_NEAR(config.expected_depth, contact.penetration_depth, eps) + << config.name; + EXPECT_TRUE(CompareMatrices(contact.normal, + X_WC.linear() * config.expected_normal, eps, + MatrixCompareType::absolute)) + << config.name; + EXPECT_TRUE(CompareMatrices(contact.pos, X_WC * config.expected_pos, eps, + MatrixCompareType::absolute)) + << config.name; + } else { + EXPECT_EQ(contacts.size(), 0u) << config.name; + } +} + +// This evaluates an instance of a test configuration and confirms the results +// match the expected data. The test configuration is defined in the cylinder's +// frame with an unrotated sphere. The parameters provide the test +// configuration. +// +// Evaluates the distance query twice. Once as the boolean "is separated" test +// and once with the separation characterized with distance and surface points. +template +void EvalDistanceForTestConfiguration(const TestConfiguration& config, + const Transform3& X_WC, + const Matrix3& R_CS, + S eps) { + // Set up the experiment from input parameters and test configuration. + Cylinder cylinder(config.r_c, config.cylinder_half_len * S(2)); + Sphere sphere{config.r_s}; + Transform3 X_CS = Transform3::Identity(); + X_CS.translation() = config.p_CSo; + X_CS.linear() = R_CS; + Transform3 X_WS = X_WC * X_CS; + + bool separated = sphereCylinderDistance(sphere, X_WS, cylinder, X_WC, + nullptr, nullptr, nullptr); + EXPECT_NE(separated, config.expected_colliding) << config.name; + + // Initializing this to -2, to confirm that a non-colliding scenario sets + // distance to -1. + S distance{-2}; + Vector3 p_WSc{0, 0, 0}; + Vector3 p_WCs{0, 0, 0}; + + separated = sphereCylinderDistance(sphere, X_WS, cylinder, X_WC, &distance, + &p_WSc, &p_WCs); + EXPECT_NE(separated, config.expected_colliding) << config.name; + if (!config.expected_colliding) { + EXPECT_NEAR(distance, config.expected_distance, eps) + << config.name; + EXPECT_TRUE(CompareMatrices(p_WSc, + X_WC * config.expected_p_CSc, eps, + MatrixCompareType::absolute)) + << config.name; + EXPECT_TRUE(CompareMatrices(p_WCs, + X_WC * config.expected_p_CCs, eps, + MatrixCompareType::absolute)) + << config.name; + } else { + EXPECT_EQ(distance, S(-1)) << config.name; + EXPECT_TRUE(CompareMatrices(p_WSc, Vector3::Zero(), 0, + MatrixCompareType::absolute)); + EXPECT_TRUE(CompareMatrices(p_WCs, Vector3::Zero(), 0, + MatrixCompareType::absolute)); + } +} + +// This test defines the transforms for performing the single collision test. +template +void QueryWithVaryingWorldFrames( + const std::vector>& configurations, + EvalFunc query_eval, const Matrix3& R_CS = Matrix3::Identity()) { + // Evaluate all the configurations with the given cylinder pose in frame F. + auto evaluate_all = [&R_CS, query_eval]( + const std::vector>& configs, + const Transform3& X_FC) { + for (const auto config : configs) { + query_eval(config, X_FC, R_CS, Eps::value()); + } + }; + + // Frame F is coincident and aligned with the cylinder frame. + Transform3 X_FC = Transform3::Identity(); + evaluate_all(AppendLabel(configurations, "X_FC = I"), X_FC); + + // Simple arbitrary translation away from the origin. + X_FC.translation() << 1.3, 2.7, 6.5; + evaluate_all(AppendLabel(configurations, "X_FC is translation"), X_FC); + + std::string axis_name[] = {"x", "y", "z"}; + // 90 degree rotation around each axis. + for (int axis = 0; axis < 3; ++axis) { + std::string label = "X_FC is 90-degree rotation around " + axis_name[axis]; + AngleAxis angle_axis{constants::pi() / 2, Vector3::Unit(axis)}; + X_FC.linear() << angle_axis.matrix(); + evaluate_all(AppendLabel(configurations, label), X_FC); + } + + // Arbitrary orientation. + { + AngleAxis angle_axis{constants::pi() / 3, + Vector3{1, 2, 3}.normalized()}; + X_FC.linear() << angle_axis.matrix(); + evaluate_all(AppendLabel(configurations, "X_FC is arbitrary rotation"), + X_FC); + } + + // Near axis aligned. + { + AngleAxis angle_axis{constants::eps_12(), Vector3::UnitX()}; + X_FC.linear() << angle_axis.matrix(); + evaluate_all(AppendLabel(configurations, "X_FC is near identity"), + X_FC); + } +} + +// Runs all test configurations across multiple poses in the world frame -- +// changing the orientation of the sphere -- should have no affect on the +// results. +template +void QueryWithOrientedSphere( + const std::vector>& configurations, + EvalFunc query_eval) { + + std::string axis_name[] = {"x", "y", "z"}; + + // 90 degree rotation around each axis. + for (int axis = 0; axis < 3; ++axis) { + AngleAxis angle_axis{constants::pi() / 2, Vector3::Unit(axis)}; + std::string label = "sphere rotate 90-degrees around " + axis_name[axis]; + QueryWithVaryingWorldFrames(AppendLabel(configurations, label), + query_eval, angle_axis.matrix()); + } + + // Arbitrary orientation. + { + AngleAxis angle_axis{constants::pi() / 3, + Vector3{1, 2, 3}.normalized()}; + std::string label = "sphere rotated arbitrarily"; + QueryWithVaryingWorldFrames(AppendLabel(configurations, label), + query_eval, angle_axis.matrix()); + } + + // Near axis aligned. + { + AngleAxis angle_axis{constants::eps_12(), Vector3::UnitX()}; + std::string label = "sphere rotated near axes"; + QueryWithVaryingWorldFrames(AppendLabel(configurations, label), + query_eval, angle_axis.matrix()); + } +} + +//====================================================================== + +// Tests the helper function that finds the closest point in the cylinder. +GTEST_TEST(SphereCylinderPrimitiveTest, NearestPointInCylinder) { + NearestPointInCylinder(); + NearestPointInCylinder(); +} + +// Evaluates collision on all test configurations across multiple poses in the +// world frame - but the sphere rotation is always the identity. +GTEST_TEST(SphereCylinderPrimitiveTest, CollisionAcrossVaryingWorldFrames) { + QueryWithVaryingWorldFrames(GetUniformConfigurations(), + EvalCollisionForTestConfiguration); + QueryWithVaryingWorldFrames( + GetUniformConfigurations(), + EvalCollisionForTestConfiguration); +} + +// Evaluates collision on all test configurations across multiple poses in the +// world frame - the sphere is rotated arbitrarily. +GTEST_TEST(SphereCylinderPrimitiveTest, CollisionWithSphereRotations) { + QueryWithOrientedSphere(GetUniformConfigurations(), + EvalCollisionForTestConfiguration); + QueryWithOrientedSphere(GetUniformConfigurations(), + EvalCollisionForTestConfiguration); +} + +// Evaluates collision on a small set of configurations where the cylinder and +// sphere are of radically different scales - evaluation across multiple poses +// in the world frame. +GTEST_TEST(SphereCylinderPrimitiveTest, CollisionIncompatibleScales) { + QueryWithVaryingWorldFrames(GetNonUniformConfigurations(), + EvalCollisionForTestConfiguration); + QueryWithVaryingWorldFrames( + GetNonUniformConfigurations(), + EvalCollisionForTestConfiguration); +} + +// Evaluates distance on all test configurations across multiple poses in the +// world frame - but the sphere rotation is always the identity. +GTEST_TEST(SphereCylinderPrimitiveTest, DistanceAcrossVaryingWorldFrames) { + QueryWithVaryingWorldFrames(GetUniformConfigurations(), + EvalDistanceForTestConfiguration); + QueryWithVaryingWorldFrames(GetUniformConfigurations(), + EvalDistanceForTestConfiguration); +} + +// Evaluates distance on all test configurations across multiple poses in the +// world frame - the sphere is rotated arbitrarily. +GTEST_TEST(SphereCylinderPrimitiveTest, DistanceWithSphereRotations) { + QueryWithOrientedSphere(GetUniformConfigurations(), + EvalDistanceForTestConfiguration); + QueryWithOrientedSphere(GetUniformConfigurations(), + EvalDistanceForTestConfiguration); +} + +// Evaluates distance on a small set of configurations where the cylinder and +// sphere are of radically different scales - evaluation across multiple poses +// in the world frame. +GTEST_TEST(SphereCylinderPrimitiveTest, DistanceIncompatibleScales) { + QueryWithVaryingWorldFrames(GetNonUniformConfigurations(), + EvalDistanceForTestConfiguration); + QueryWithVaryingWorldFrames(GetNonUniformConfigurations(), + EvalDistanceForTestConfiguration); +} + +} // namespace +} // namespace detail +} // namespace fcl + +//============================================================================== +int main(int argc, char *argv[]) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/test_fcl_sphere_cylinder.cpp b/test/test_fcl_sphere_cylinder.cpp new file mode 100644 index 000000000..105d49e3b --- /dev/null +++ b/test/test_fcl_sphere_cylinder.cpp @@ -0,0 +1,229 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. 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 (2018) */ + +#include + +#include +#include + +#include "eigen_matrix_compare.h" +#include "fcl/narrowphase/collision_object.h" +#include "fcl/narrowphase/distance.h" + +// TODO(SeanCurtis-TRI): Modify this test so it can be re-used for the distance +// query -- such that the sphere is slightly separated instead of slightly +// penetrating. See test_sphere_cylinder.cpp for an example. + +// This collides a cylinder with a sphere. The cylinder is disk-like with a +// large radius (r_c) and small height (h_c) and its geometric frame is aligned +// with the world frame. The sphere has radius r_s and is positioned at +// (sx, sy, sz) with an identity orientation. In this configuration, the sphere +// penetrates the cylinder slightly on the top face near the edge. The contact +// is *fully* contained in that face. (As illustrated below.) +// +// Side view +// z small sphere +// ┆ ↓ +// ┏━━━━━━━━━━━━┿━━━━◯━━━━━━┓ ┬ +// ┄┄┄┄┄┄╂┄┄┄┄┄┄┄┄┄┄┄┄┼┄┄┄┄┄┄┄┄┄┄┄╂┄ x h_c +// ┗━━━━━━━━━━━━┿━━━━━━━━━━━┛ ┴ +// ┆ +// +// ├──── r_c───┤ +// +// Top view +// y +// ┆ +// ******* small sphere ┬ +// ** ┆ **╱ │ +// * ┆ ◯ * │ +// * ┆ * │ +// * ┆ * r_c +// * ┆ * │ +// * ┆ * │ +// * ┆ * │ +// ┄┄┄┄┄┄┄*┄┄┄┄┄┄┄┄┄┄┼┄┄┄┄┄┄┄┄┄┄*┄┄┄┄ x ┴ +// * ┆ * +// * ┆ * +// * ┆ * +// * ┆ * +// * ┆ * +// * ┆ * +// ** ┆ ** +// ******* +// ┆ +// Properties of expected outcome: +// - One contact *should* be reported, +// - Penetration depth δ should be: r_s - (sz - h_c / 2), +// - Contact point should be at: [sx, sy, h_c / 2 - δ / 2], and +// - Contact normal should be [0, 0, -1] (pointing from sphere into cylinder). +// +// NOTE: Orientation of the sphere should *not* make a difference and is not +// tested here; it relies on the sphere-cylinder primitive algorithm unit tests +// to have robustly tested that. +// +// This test *fails* if GJK is used to evaluate the collision. It passes if the +// custom sphere-cylinder algorithm is used, and, therefore, its purpose is to +// confirm that the custom algorithm is being applied. It doesn't exhaustively +// test sphere-cylinder collision. It merely confirms the tested primitive +// algorithm has been wired up correctly. +template +void LargeCylinderSmallSphereTest(fcl::GJKSolverType solver_type) { + using fcl::Vector3; + using Real = typename fcl::constants::Real; + const Real eps = fcl::constants::eps(); + + // Configure geometry. + + // Cylinder and sphere dimensions. + const Real r_c = 9; + const Real h_c = 0.0025; + const Real r_s = 0.0015; + + auto sphere_geometry = std::make_shared>(r_s); + auto cylinder_geometry = std::make_shared>(r_c, h_c); + + // Pose of the cylinder in the world frame. + const fcl::Transform3 X_WC = fcl::Transform3::Identity(); + + // Compute multiple sphere locations. All at the same height to produce a + // fixed, expected penetration depth of half of its radius. The reported + // position of the contact will have the x- and y- values of the sphere + // center, but be half the target_depth below the +z face, i.e., + // pz = (h_c / 2) - (target_depth / 2) + const Real target_depth = r_s * 0.5; + // Sphere center's height (position in z). + const Real sz = h_c / 2 + r_s - target_depth; + const Real pz = h_c / 2 - target_depth / 2; + // This transform will get repeatedly modified in the nested for loop below. + fcl::Transform3 X_WS = fcl::Transform3::Identity(); + + fcl::CollisionObject sphere(sphere_geometry, X_WS); + fcl::CollisionObject cylinder(cylinder_geometry, X_WC); + + // Expected results. (Expected position is defined inside the for loop as it + // changes with each new position of the sphere.) + const S expected_depth = target_depth; + // This normal direction assumes the *sphere* is the first collision object. + // If the order is reversed, the normal must be likewise reversed. + const Vector3 expected_normal = -Vector3::UnitZ(); + + // Set up the collision request. + fcl::CollisionRequest collision_request(1 /* num contacts */, + true /* enable_contact */); + collision_request.gjk_solver_type = solver_type; + + // Sample around the surface of the +z face on the disk for a fixed + // penetration depth. Note: the +z face is a disk with radius r_c. Notes on + // the selected samples: + // - We've picked positions such that the *whole* sphere projects onto the + // +z face. This *guarantees* that the contact is completely contained in + // the +z face so there is no possible ambiguity in the results. + // - We've picked points out near the boundaries, in the middle, and *near* + // zero without being zero. The GJK algorithm can actually provide the + // correct result at the (eps, eps) sample points. We leave those sample + // points in to confirm no degradation. + const std::vector r_values{0, eps, r_c / 2, r_c - r_s}; + const S pi = fcl::constants::pi(); + const std::vector theta_values{0, pi/2, pi, 3 * pi / 4}; + + for (const Real r : r_values) { + for (const Real theta : theta_values ) { + // Don't just evaluate at nice, axis-aligned directions. Pick some number + // that can't be perfectly represented. + const Real angle = theta + pi / 7; + const Real sx = cos(angle) * r; + const Real sy = sin(angle) * r; + // Repose the sphere. + X_WS.translation() << sx, sy, sz; + sphere.setTransform(X_WS); + + auto evaluate_collision = [&collision_request, &X_WS]( + const fcl::CollisionObject* s1, const fcl::CollisionObject* s2, + S expected_depth, const Vector3& expected_normal, + const Vector3& expected_pos, Real eps) { + // Compute collision. + fcl::CollisionResult collision_result; + std::size_t contact_count = + fcl::collide(s1, s2, collision_request, collision_result); + + // Test answers + if (contact_count == collision_request.num_max_contacts) { + std::vector> contacts; + collision_result.getContacts(contacts); + GTEST_ASSERT_EQ(contacts.size(), collision_request.num_max_contacts); + + const fcl::Contact& contact = contacts[0]; + EXPECT_NEAR(contact.penetration_depth, expected_depth, eps) + << "Sphere at: " << X_WS.translation().transpose(); + EXPECT_TRUE(fcl::CompareMatrices(contact.normal, + expected_normal, + eps, + fcl::MatrixCompareType::absolute)) + << "Sphere at: " << X_WS.translation().transpose(); + EXPECT_TRUE(fcl::CompareMatrices( + contact.pos, expected_pos, eps, fcl::MatrixCompareType::absolute)) + << "Sphere at: " << X_WS.translation().transpose(); + } else { + EXPECT_TRUE(false) << "No contacts reported for sphere at: " + << X_WS.translation().transpose(); + } + }; + + Vector3 expected_pos{sx, sy, pz}; + evaluate_collision(&sphere, &cylinder, expected_depth, expected_normal, + expected_pos, eps); + evaluate_collision(&cylinder, &sphere, expected_depth, -expected_normal, + expected_pos, eps); + } + } +} + +GTEST_TEST(FCL_SPHERE_CYLINDER, LargCylinderSmallSphere_ccd) { + LargeCylinderSmallSphereTest(fcl::GJKSolverType::GST_LIBCCD); + LargeCylinderSmallSphereTest(fcl::GJKSolverType::GST_LIBCCD); +} + +GTEST_TEST(FCL_SPHERE_CYLINDER, LargBoxSmallSphere_indep) { + LargeCylinderSmallSphereTest(fcl::GJKSolverType::GST_INDEP); + LargeCylinderSmallSphereTest(fcl::GJKSolverType::GST_INDEP); +} + +//============================================================================== +int main(int argc, char* argv[]) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 9d25b53bbdee67adb66474b956265fbfd6d13f18 Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Wed, 8 Aug 2018 13:43:01 -0700 Subject: [PATCH 10/34] Add a guard to `faceNormalPointingOutward` to detect degeneracy (#324) This adds the `triangle_area_is_zero` test to `faceNormalPointingOutward`. This is a prerequisite to the `isOutsidePolytopeFace` test. The test is rendered meaningless if the reported normal is defined by numerical noise. If a scenario is failing due to the `isOutsidePolytopFace` assertion failing, this will detect if its due to face degeneracies. --- .../gjk_libccd-inl.h | 128 ++++++++-------- .../test_gjk_libccd-inl_epa.cpp | 138 ++++++++++++++++++ 2 files changed, 207 insertions(+), 59 deletions(-) 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 47f7aab4d..c41f12d32 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 @@ -765,6 +765,64 @@ static int simplexToPolytope4(const void *obj1, const void *obj2, return 0; } +/** Reports true if p and q are coincident. */ +static bool are_coincident(const ccd_vec3_t& p, const ccd_vec3_t& q) { + // This uses a scale-dependent basis for determining coincidence. It examines + // each axis independently, and only, if all three axes are sufficiently + // close (relative to its own scale), are the two points considered + // coincident. + // + // For dimension i, two values are considered the same if: + // |pᵢ - qᵢ| <= ε·max(1, |pᵢ|, |qᵢ|) + // And the points are coincident if the previous condition holds for all + // `i ∈ {0, 1, 2}` (i.e. the x-, y-, *and* z-dimensions). + using std::abs; + using std::max; + + const ccd_real_t eps = constants::eps(); + // NOTE: Wrapping "1.0" with ccd_real_t accounts for mac problems where ccd + // is actually float based. + for (int i = 0; i < 3; ++i) { + const ccd_real_t tolerance = + max({ccd_real_t{1}, abs(p.v[i]), abs(q.v[i])}) * eps; + const ccd_real_t delta = abs(p.v[i] - q.v[i]); + if (delta > tolerance) return false; + } + return true; +} + +/** Determines if the the triangle defined by the three vertices has zero area. + Area can be zero for one of two reasons: + - the triangle is so small that the vertices are functionally coincident, or + - the vertices are co-linear. + Both conditions are computed with respect to machine precision. + @returns true if the area is zero. */ +static bool triangle_area_is_zero(const ccd_vec3_t& a, const ccd_vec3_t& b, + const ccd_vec3_t& c) { + // First coincidence condition. This doesn't *explicitly* test for b and c + // being coincident. That will be captured in the subsequent co-linearity + // test. If b and c *were* coincident, it would be cheaper to perform the + // coincidence test than the co-linearity test. + // However, the expectation is that typically the triangle will not have zero + // area. In that case, we want to minimize the number of tests performed on + // the average, so we prefer to eliminate one coincidence test. + if (are_coincident(a, b) || are_coincident(a, c)) return true; + + // We're going to compute the *sine* of the angle θ between edges (given that + // the vertices are *not* coincident). If the sin(θ) < ε, the edges are + // co-linear. + ccd_vec3_t AB, AC, n; + ccdVec3Sub2(&AB, &b, &a); + ccdVec3Sub2(&AC, &c, &a); + ccdVec3Normalize(&AB); + ccdVec3Normalize(&AC); + ccdVec3Cross(&n, &AB, &AC); + const ccd_real_t eps = constants::eps(); + // Second co-linearity condition. + if (ccdVec3Len2(&n) < eps * eps) return true; + return false; +} + /** * Computes the normal vector of a triangular face on a polytope, and the normal * vector points outward from the polytope. Notice we assume that the origin @@ -778,6 +836,17 @@ static int simplexToPolytope4(const void *obj1, const void *obj2, */ static ccd_vec3_t faceNormalPointingOutward(const ccd_pt_t* polytope, const ccd_pt_face_t* face) { + // This doesn't necessarily define a triangle; I don't know that the third + // vertex added here is unique from the other two. +#ifndef NDEBUG + // quick test for degeneracy + const ccd_vec3_t& a = face->edge[0]->vertex[1]->v.v; + const ccd_vec3_t& b = face->edge[0]->vertex[0]->v.v; + const ccd_vec3_t& test_v = face->edge[1]->vertex[0]->v.v; + const ccd_vec3_t& c = are_coincident(test_v, a) || are_coincident(test_v, b) ? + face->edge[1]->vertex[1]->v.v : test_v; + assert(!triangle_area_is_zero(a, b, c)); +#endif // We find two edges of the triangle as e1 and e2, and the normal vector // of the face is e1.cross(e2). ccd_vec3_t e1, e2; @@ -1363,65 +1432,6 @@ static int __ccdEPA(const void *obj1, const void *obj2, return 0; } - -/** Reports true if p and q are coincident. */ -static bool are_coincident(const ccd_vec3_t& p, const ccd_vec3_t& q) { - // This uses a scale-dependent basis for determining coincidence. It examines - // each axis independently, and only, if all three axes are sufficiently - // close (relative to its own scale), are the two points considered - // coincident. - // - // For dimension i, two values are considered the same if: - // |pᵢ - qᵢ| <= ε·max(1, |pᵢ|, |pᵢ|) - // And the points are coincident if the previous condition for all - // `i ∈ {0, 1, 2}` (i.e. the x-, y-, *and* z-dimensions). - using std::abs; - using std::max; - - const ccd_real_t eps = constants::eps(); - // NOTE: Wrapping "1.0" with ccd_real_t accounts for mac problems where ccd - // is actually float based. - for (int i = 0; i < 3; ++i) { - const ccd_real_t scale = - max({ccd_real_t{1}, abs(p.v[i]), abs(q.v[i])}) * eps; - const ccd_real_t delta = abs(p.v[i] - q.v[i]); - if (delta > scale) return false; - } - return true; -} - -/** Determines if the the triangle defined by the three vertices has zero area. - Area can be zero for one of two reasons: - - the triangle is so small that the vertices are functionally coincident, or - - the vertices are co-linear. - Both conditions are computed with respect to machine precision. - @returns true if the area is zero. */ -static bool triangle_area_is_zero(const ccd_vec3_t& a, const ccd_vec3_t& b, - const ccd_vec3_t& c) { - // First coincidence condition. This doesn't *explicitly* test for b and c - // being coincident. That will be captured in the subsequent co-linearity - // test. If b and c *were* coincident, it would be cheaper to perform the - // coincidence test than the co-linearity test. - // However, the expectation is that typically the triangle will not have zero - // area. In that case, we want to minimize the number of tests performed on - // the average, so we prefer to eliminate one coincidence test. - if (are_coincident(a, b) || are_coincident(a, c)) return true; - - // We're going to compute the *sine* of the angle θ between edges (given that - // the vertices are *not* coincident). If the sin(θ) < ε, the edges are - // co-linear. - ccd_vec3_t AB, AC, n; - ccdVec3Sub2(&AB, &b, &a); - ccdVec3Sub2(&AC, &c, &a); - ccdVec3Normalize(&AB); - ccdVec3Normalize(&AC); - ccdVec3Cross(&n, &AB, &AC); - const ccd_real_t eps = constants::eps(); - // Second co-linearity condition. - if (ccdVec3Len2(&n) < eps * eps) return true; - return false; -} - /** Given a single support point, `q`, extract the point `p1` and `p2`, the points on object 1 and 2, respectively, in the support data of `q`. */ static void extractObjectPointsFromPoint(ccd_support_t *q, ccd_vec3_t *p1, diff --git a/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp index 6a9afcba0..ec23ed77c 100644 --- a/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp +++ b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp @@ -293,6 +293,144 @@ GTEST_TEST(FCL_GJK_EPA, isOutsidePolytopeFace) { CheckPointOutsidePolytopeFace(0, 0, 0, 3, expect_inside); } +#ifndef NDEBUG + +/** A degenerate tetrahedron due to vertices considered to be coincident. + It is, strictly speaking a valid tetrahedron, but the points are so close that + the calculations on edge lengths cannot be trusted. + + More particularly, one face is *very* small but the other three faces are quite + long with horrible aspect ratio. + + Vertices v0, v1, and v2 are all close to each other, v3 is distant. + Edges e0, e1, and e2 connect vertices (v0, v1, and v2) and, as such, have very + short length. Edges e3, e4, and e5 connect to the distance vertex and have + long length. + + Face 0 is the small face. Faces 1-3 all include one edge of the small face. + + All faces should be considered degenerate due to coincident points. */ +class CoincidentTetrahedron : public Polytope { + public: + CoincidentTetrahedron() : Polytope() { + const ccd_real_t delta = constants::eps() / 4; + v().resize(4); + e().resize(6); + f().resize(4); + auto AddTetrahedronVertex = [this](ccd_real_t x, ccd_real_t y, + ccd_real_t z) { + return ccdPtAddVertexCoords(&this->polytope(), x, y, z); + }; + v()[0] = AddTetrahedronVertex(0.5, delta, delta); + v()[1] = AddTetrahedronVertex(0.5, -delta, delta); + v()[2] = AddTetrahedronVertex(0.5, -delta, -delta); + v()[3] = AddTetrahedronVertex(-0.5, 0, 0); + e()[0] = ccdPtAddEdge(&polytope(), &v(0), &v(1)); + e()[1] = ccdPtAddEdge(&polytope(), &v(1), &v(2)); + e()[2] = ccdPtAddEdge(&polytope(), &v(2), &v(0)); + e()[3] = ccdPtAddEdge(&polytope(), &v(0), &v(3)); + e()[4] = ccdPtAddEdge(&polytope(), &v(1), &v(3)); + e()[5] = ccdPtAddEdge(&polytope(), &v(2), &v(3)); + f()[0] = ccdPtAddFace(&polytope(), &e(0), &e(1), &e(2)); + f()[1] = ccdPtAddFace(&polytope(), &e(0), &e(3), &e(4)); + f()[2] = ccdPtAddFace(&polytope(), &e(1), &e(4), &e(5)); + f()[3] = ccdPtAddFace(&polytope(), &e(3), &e(5), &e(2)); + } +}; + +// Tests against a polytope with a face where all the points are too close to +// distinguish. +GTEST_TEST(FCL_GJK_EPA, isOutsidePolytopeFace_DegenerateFace_Coincident0) { + ::testing::FLAGS_gtest_death_test_style = "threadsafe"; + CoincidentTetrahedron p; + + // The test point doesn't matter; it'll never get that far. + // NOTE: For platform compatibility, the assertion message is pared down to + // the simplest component: the actual function call in the assertion. + ccd_vec3_t pt{{10, 10, 10}}; + ASSERT_DEATH( + libccd_extension::isOutsidePolytopeFace(&p.polytope(), &p.f(0), &pt), + ".*!triangle_area_is_zero.*"); +} + +// Tests against a polytope with a face where *two* points are too close to +// distinguish. +GTEST_TEST(FCL_GJK_EPA, isOutsidePolytopeFace_DegenerateFace_Coincident1) { + ::testing::FLAGS_gtest_death_test_style = "threadsafe"; + CoincidentTetrahedron p; + + // The test point doesn't matter; it'll never get that far. + // NOTE: For platform compatibility, the assertion message is pared down to + // the simplest component: the actual function call in the assertion. + ccd_vec3_t pt{{10, 10, 10}}; + ASSERT_DEATH( + libccd_extension::isOutsidePolytopeFace(&p.polytope(), &p.f(1), &pt), + ".*!triangle_area_is_zero.*"); +} + +/** A degenerate tetrahedron due to vertices considered to be colinear. + It is, strictly speaking a valid tetrahedron, but the vertices are so close to + being colinear, that the area can't meaningfully be computed. + + More particularly, one face is *very* large but the fourth vertex lies just + slightly off that plane *over* one of the edges. The face that is incident to + that edge and vertex will have colinear edges. + + Vertices v0, v1, and v2 are form the large triangle. v3 is the slightly + off-plane vertex. Edges e0, e1, and e2 connect vertices (v0, v1, and v2). v3 + projects onto edge e0. Edges e3 and e4 connect v0 and v1 to v3, respectively. + Edges e3 and e4 are colinear. Edge e5 is the remaining, uninteresting edge. + Face 0 is the large triangle. + Face 1 is the bad face. Faces 2 and 3 are irrelevant. */ +class ColinearTetrahedron : public Polytope { + public: + ColinearTetrahedron() : Polytope() { + const ccd_real_t delta = constants::eps() / 100; + v().resize(4); + e().resize(6); + f().resize(4); + auto AddTetrahedronVertex = [this](ccd_real_t x, ccd_real_t y, + ccd_real_t z) { + return ccdPtAddVertexCoords(&this->polytope(), x, y, z); + }; + v()[0] = AddTetrahedronVertex(0.5, -0.5 / std::sqrt(3), -1); + v()[1] = AddTetrahedronVertex(-0.5, -0.5 / std::sqrt(3), -1); + v()[2] = AddTetrahedronVertex(0, 1 / std::sqrt(3), -1); + // This point should lie *slightly* above the edge connecting v0 and v1. + v()[3] = AddTetrahedronVertex(0, -0.5 / std::sqrt(3), -1 + delta); + + e()[0] = ccdPtAddEdge(&polytope(), &v(0), &v(1)); + e()[1] = ccdPtAddEdge(&polytope(), &v(1), &v(2)); + e()[2] = ccdPtAddEdge(&polytope(), &v(2), &v(0)); + e()[3] = ccdPtAddEdge(&polytope(), &v(0), &v(3)); + e()[4] = ccdPtAddEdge(&polytope(), &v(1), &v(3)); + e()[5] = ccdPtAddEdge(&polytope(), &v(2), &v(3)); + f()[0] = ccdPtAddFace(&polytope(), &e(0), &e(1), &e(2)); + f()[1] = ccdPtAddFace(&polytope(), &e(0), &e(3), &e(4)); + f()[2] = ccdPtAddFace(&polytope(), &e(1), &e(4), &e(5)); + f()[3] = ccdPtAddFace(&polytope(), &e(3), &e(5), &e(2)); + } +}; + +// Tests against a polytope with a face where two edges are colinear. +GTEST_TEST(FCL_GJK_EPA, isOutsidePolytopeFace_DegenerateFace_Colinear) { + ::testing::FLAGS_gtest_death_test_style = "threadsafe"; + ColinearTetrahedron p; + + // This test point should pass w.r.t. the big face. + ccd_vec3_t pt{{0, 0, -10}}; + EXPECT_TRUE(libccd_extension::isOutsidePolytopeFace(&p.polytope(), &p.f(0), + &pt)); + // Face 1, however, is definitely colinear. + // NOTE: For platform compatibility, the assertion message is pared down to + // the simplest component: the actual function call in the assertion. + ASSERT_DEATH( + libccd_extension::isOutsidePolytopeFace(&p.polytope(), &p.f(1), &pt), + ".*!triangle_area_is_zero.*"); +} +#endif + + // Construct a polytope with the following shape, namely an equilateral triangle // on the top, and an equilateral triangle of the same size, but rotate by 60 // degrees on the bottom. We will then connect the vertices of the equilateral From 91d9d4d5735e44f91ba9df013c9fcd16a22938e4 Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Sun, 12 Aug 2018 14:42:50 -0700 Subject: [PATCH 11/34] Correct distance queries to report nearest point in world frame (#288) * Distance queries return nearest points in the world frame The DistanceResult struct documents the near points as being defined in the *world* frame. This wasn't true in the code. 1. Correct both gjk solver implementations to put the pose in *world* frame. 2. Modify sphere-sphere and sphere-capsule custom implementations to do the same. 3. Fix some related typos in documentation. 4. Update tests to expect points in world frame. --- .../narrowphase/detail/gjk_solver_indep-inl.h | 13 +- .../detail/gjk_solver_libccd-inl.h | 18 --- .../sphere_capsule-inl.h | 18 ++- .../sphere_sphere-inl.h | 4 +- include/fcl/narrowphase/distance_result.h | 4 +- test/test_fcl_capsule_box_1.cpp | 8 +- test/test_fcl_capsule_box_2.cpp | 6 +- test/test_fcl_distance.cpp | 31 +++-- test/test_fcl_signed_distance.cpp | 121 +++++++++++++++--- test/test_fcl_sphere_sphere.cpp | 36 +++--- 10 files changed, 177 insertions(+), 82 deletions(-) diff --git a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h b/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h index 3e386737a..cf93d4558 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h +++ b/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h @@ -625,8 +625,10 @@ struct ShapeDistanceIndepImpl if(distance) *distance = (w0 - w1).norm(); - if(p1) *p1 = w0; - if(p2) (*p2).noalias() = shape.toshape0.inverse() * w1; + // Answer is solved in Shape1's local frame; answers are given in the + // world frame. + if(p1) p1->noalias() = tf1 * w0; + if(p2) p2->noalias() = tf1 * w1; return true; } @@ -880,8 +882,9 @@ struct ShapeTriangleDistanceIndepImpl } if(distance) *distance = (w0 - w1).norm(); + // The answers are produced in world coordinates. Keep them there. if(p1) *p1 = w0; - if(p2) (*p2).noalias() = shape.toshape0 * w1; + if(p2) *p2 = w1; return true; } else @@ -970,8 +973,8 @@ struct ShapeTransformedTriangleDistanceIndepImpl } if(distance) *distance = (w0 - w1).norm(); - if(p1) *p1 = w0; - if(p2) (*p2).noalias() = shape.toshape0 * w1; + if(p1) p1->noalias() = tf1 * w0; + if(p2) p2->noalias() = tf1 * w1; return true; } else diff --git a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h b/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h index 4e6903215..d9ffffe80 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h +++ b/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h @@ -567,12 +567,6 @@ struct ShapeSignedDistanceLibccdImpl p1, p2); - if (p1) - (*p1).noalias() = tf1.inverse(Eigen::Isometry) * *p1; - - if (p2) - (*p2).noalias() = tf2.inverse(Eigen::Isometry) * *p2; - detail::GJKInitializer::deleteGJKObject(o1); detail::GJKInitializer::deleteGJKObject(o2); @@ -624,12 +618,6 @@ struct ShapeDistanceLibccdImpl p1, p2); - if (p1) - (*p1).noalias() = tf1.inverse(Eigen::Isometry) * *p1; - - if (p2) - (*p2).noalias() = tf2.inverse(Eigen::Isometry) * *p2; - detail::GJKInitializer::deleteGJKObject(o1); detail::GJKInitializer::deleteGJKObject(o2); @@ -848,8 +836,6 @@ struct ShapeTriangleDistanceLibccdImpl dist, p1, p2); - if(p1) - (*p1).noalias() = tf.inverse(Eigen::Isometry) * *p1; detail::GJKInitializer::deleteGJKObject(o1); detail::triDeleteGJKObject(o2); @@ -923,10 +909,6 @@ struct ShapeTransformedTriangleDistanceLibccdImpl dist, p1, p2); - if(p1) - (*p1).noalias() = tf1.inverse(Eigen::Isometry) * *p1; - if(p2) - (*p2).noalias() = tf2.inverse(Eigen::Isometry) * *p2; detail::GJKInitializer::deleteGJKObject(o1); detail::triDeleteGJKObject(o2); diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule-inl.h index dd524ad6a..c85d56991 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule-inl.h @@ -137,8 +137,16 @@ bool sphereCapsuleDistance(const Sphere& s1, const Transform3& tf1, S distance = diff.norm() - s1.radius - s2.radius; - if(distance <= 0) + if(distance <= 0) { + // NOTE: By assigning this a negative value, it allows the ultimately + // calling code in distance-inl.h (distance() method) to use collision to + // determine penetration depth and contact points. NOTE: This is a + // *horrible* thing. + // TODO(SeanCurtis-TRI): Create a *signed* distance variant of this and use + // it to determined signed distance, penetration, and distance. + if (dist) *dist = -1; return false; + } if(dist) *dist = distance; @@ -146,10 +154,14 @@ bool sphereCapsuleDistance(const Sphere& s1, const Transform3& tf1, if(p1) { *p1 = s_c - diff * s1.radius; - *p1 = tf1.inverse(Eigen::Isometry) * tf2 * (*p1); + *p1 = tf2 * (*p1); } - if(p2) *p2 = segment_point + diff * s1.radius; + if(p2) + { + *p2 = segment_point + diff * s2.radius; + *p2 = tf2 * (*p2); + } return true; } diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h index efeeca4eb..d12671cfb 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h @@ -99,8 +99,8 @@ bool sphereSphereDistance(const Sphere& s1, const Transform3& tf1, if(len > s1.radius + s2.radius) { if(dist) *dist = len - (s1.radius + s2.radius); - if(p1) *p1 = tf1.inverse(Eigen::Isometry) * (o1 - diff * (s1.radius / len)); - if(p2) *p2 = tf2.inverse(Eigen::Isometry) * (o2 + diff * (s2.radius / len)); + if(p1) *p1 = (o1 - diff * (s1.radius / len)); + if(p2) *p2 = (o2 + diff * (s2.radius / len)); return true; } diff --git a/include/fcl/narrowphase/distance_result.h b/include/fcl/narrowphase/distance_result.h index fb53b36d8..045d2c80d 100644 --- a/include/fcl/narrowphase/distance_result.h +++ b/include/fcl/narrowphase/distance_result.h @@ -63,9 +63,9 @@ struct FCL_EXPORT DistanceResult /// @sa DistanceRequest::enable_signed_distance S min_distance; - /// @brief Nearest points in the world coordinates + /// @brief Nearest points in the world coordinates. /// - /// @sa DeistanceRequest::enable_nearest_points + /// @sa DistanceRequest::enable_nearest_points Vector3 nearest_points[2]; /// @brief collision object 1 diff --git a/test/test_fcl_capsule_box_1.cpp b/test/test_fcl_capsule_box_1.cpp index e37b0762a..0be5633cf 100644 --- a/test/test_fcl_capsule_box_1.cpp +++ b/test/test_fcl_capsule_box_1.cpp @@ -71,7 +71,7 @@ void test_distance_capsule_box(fcl::GJKSolverType solver_type, S solver_toleranc // Nearest point on box fcl::Vector3 o2 (distanceResult.nearest_points [1]); EXPECT_NEAR (distanceResult.min_distance, 0.5, test_tolerance); - EXPECT_NEAR (o1 [0], -2.0, test_tolerance); + EXPECT_NEAR (o1 [0], 1.0, test_tolerance); EXPECT_NEAR (o1 [1], 0.0, test_tolerance); EXPECT_NEAR (o2 [0], 0.5, test_tolerance); EXPECT_NEAR (o2 [1], 0.0, test_tolerance); @@ -89,7 +89,7 @@ void test_distance_capsule_box(fcl::GJKSolverType solver_type, S solver_toleranc EXPECT_NEAR (distanceResult.min_distance, 2.0, test_tolerance); EXPECT_NEAR (o1 [0], 0.0, test_tolerance); EXPECT_NEAR (o1 [1], 0.0, test_tolerance); - EXPECT_NEAR (o1 [2], -4.0, test_tolerance); + EXPECT_NEAR (o1 [2], 4.0, test_tolerance); EXPECT_NEAR (o2 [0], 0.0, test_tolerance); EXPECT_NEAR (o2 [1], 0.0, test_tolerance); @@ -107,9 +107,9 @@ void test_distance_capsule_box(fcl::GJKSolverType solver_type, S solver_toleranc o2 = distanceResult.nearest_points [1]; EXPECT_NEAR (distanceResult.min_distance, 5.5, test_tolerance); - EXPECT_NEAR (o1 [0], 0.0, test_tolerance); + EXPECT_NEAR (o1 [0], -6.0, test_tolerance); EXPECT_NEAR (o1 [1], 0.0, test_tolerance); - EXPECT_NEAR (o1 [2], 4.0, test_tolerance); + EXPECT_NEAR (o1 [2], 0.0, test_tolerance); EXPECT_NEAR (o2 [0], -0.5, test_tolerance); EXPECT_NEAR (o2 [1], 0.0, test_tolerance); EXPECT_NEAR (o2 [2], 0.0, test_tolerance); diff --git a/test/test_fcl_capsule_box_2.cpp b/test/test_fcl_capsule_box_2.cpp index 41218cfa4..d3ee42521 100644 --- a/test/test_fcl_capsule_box_2.cpp +++ b/test/test_fcl_capsule_box_2.cpp @@ -74,9 +74,9 @@ void test_distance_capsule_box(fcl::GJKSolverType solver_type, S solver_toleranc fcl::Vector3 o2 = distanceResult.nearest_points [1]; EXPECT_NEAR (distanceResult.min_distance, 5.5, test_tolerance); - EXPECT_NEAR (o1 [0], 0.0, test_tolerance); - EXPECT_NEAR (o1 [1], 0.0, test_tolerance); - EXPECT_NEAR (o1 [2], 4.0, test_tolerance); + EXPECT_NEAR (o1 [0], -6.0, test_tolerance); + EXPECT_NEAR (o1 [1], 0.8, test_tolerance); + EXPECT_NEAR (o1 [2], 1.5, test_tolerance); EXPECT_NEAR (o2 [0], -0.5, test_tolerance); EXPECT_NEAR (o2 [1], 0.8, test_tolerance); EXPECT_NEAR (o2 [2], 1.5, test_tolerance); diff --git a/test/test_fcl_distance.cpp b/test/test_fcl_distance.cpp index d49dc7e1f..e703c12ff 100644 --- a/test/test_fcl_distance.cpp +++ b/test/test_fcl_distance.cpp @@ -39,6 +39,7 @@ #include "fcl/narrowphase/detail/traversal/collision_node.h" #include "test_fcl_utility.h" +#include "eigen_matrix_compare.h" #include "fcl_resources/config.h" // TODO(SeanCurtis-TRI): A file called `test_fcl_distance.cpp` should *not* have @@ -315,6 +316,10 @@ void NearestPointFromDegenerateSimplex() { // line segment. As a result, nearest points were populated with NaN values. // See https://github.com/flexible-collision-library/fcl/issues/293 for // more discussion. + // This test is only relevant if box-box distance is computed via GJK. If + // a custom test is created, this may no longer be relevant. + // TODO(SeanCurtis-TRI): Provide some mechanism where we can assert what the + // solving algorithm is (i.e., default convex vs. custom). DistanceResult result; DistanceRequest request; request.enable_nearest_points = true; @@ -335,15 +340,25 @@ void NearestPointFromDegenerateSimplex() { EXPECT_NO_THROW(fcl::distance(&box_object_1, &box_object_2, request, result)); - // The values here have been visually confirmed from the computation. + // These hard-coded values have been previously computed and visually + // inspected and considered to be the ground truth for this very specific + // test configuration. S expected_dist{0.053516322172152138}; - Vector3 expected_p0{-1.375, -0.098881502700918666, -0.025000000000000022}; - Vector3 expected_p1{0.21199965773384655, 0.074999692703297122, - 0.084299993303443954}; - EXPECT_TRUE(nearlyEqual(result.nearest_points[0], expected_p0)); - EXPECT_TRUE(nearlyEqual(result.nearest_points[1], expected_p1)); - // TODO(SeanCurtis-TRI): Change this tolerance to constants::eps_34() when - // the mac single/double libccd problem is resolved. + // The "nearest" points (N1 and N2) measured and expressed in box 1's and + // box 2's frames (B1 and B2, respectively). + const Vector3 expected_p_B1N1{-1.375, -0.098881502700918666, + -0.025000000000000022}; + const Vector3 expected_p_B2N2{0.21199965773384655, 0.074999692703297122, + 0.084299993303443954}; + // The nearest points in the world frame. + const Vector3 expected_p_WN1 = + box_object_1.getTransform() * expected_p_B1N1; + const Vector3 expected_p_WN2 = + box_object_2.getTransform() * expected_p_B2N2; + EXPECT_TRUE(CompareMatrices(result.nearest_points[0], expected_p_WN1, + DELTA(), MatrixCompareType::absolute)); + EXPECT_TRUE(CompareMatrices(result.nearest_points[1], expected_p_WN2, + DELTA(), MatrixCompareType::absolute)); EXPECT_NEAR(expected_dist, result.min_distance, constants::eps_34()); } diff --git a/test/test_fcl_signed_distance.cpp b/test/test_fcl_signed_distance.cpp index c67eb9f89..153581707 100644 --- a/test/test_fcl_signed_distance.cpp +++ b/test/test_fcl_signed_distance.cpp @@ -34,6 +34,7 @@ #include +#include "eigen_matrix_compare.h" #include "fcl/narrowphase/distance.h" #include "fcl/narrowphase/detail/traversal/collision_node.h" #include "fcl/narrowphase/detail/gjk_solver_libccd.h" @@ -48,8 +49,10 @@ bool verbose = false; template void test_distance_spheresphere(GJKSolverType solver_type) { - Sphere s1{20}; - Sphere s2{10}; + const S radius_1 = 20; + const S radius_2 = 10; + Sphere s1{radius_1}; + Sphere s2{radius_2}; Transform3 tf1{Transform3::Identity()}; Transform3 tf2{Transform3::Identity()}; @@ -61,48 +64,128 @@ void test_distance_spheresphere(GJKSolverType solver_type) DistanceResult result; - bool res{false}; + // Expecting distance to be 10 + result.clear(); + tf2.translation() = Vector3(40, 0, 0); + distance(&s1, tf1, &s2, tf2, request, result); + EXPECT_NEAR(result.min_distance, 10, 1e-6); + EXPECT_TRUE(CompareMatrices(result.nearest_points[0], Vector3(20, 0, 0), + request.distance_tolerance)); + EXPECT_TRUE(CompareMatrices(result.nearest_points[1], Vector3(30, 0, 0), + request.distance_tolerance)); + + // request.distance_tolerance is actually the square of the distance + // tolerance, namely the difference between distance returned from FCL's EPA + // implementation and the actual distance, is less than + // sqrt(request.distance_tolerance). + const S distance_tolerance = std::sqrt(request.distance_tolerance); + + // Expecting distance to be -5 + result.clear(); + tf2.translation() = Vector3(25, 0, 0); + distance(&s1, tf1, &s2, tf2, request, result); + EXPECT_NEAR(result.min_distance, -5, request.distance_tolerance); + + // TODO(JS): Only GST_LIBCCD can compute the pair of nearest points on the + // surface of the penetrating spheres. + if (solver_type == GST_LIBCCD) + { + EXPECT_TRUE(CompareMatrices(result.nearest_points[0], Vector3(20, 0, 0), + distance_tolerance)); + EXPECT_TRUE(CompareMatrices(result.nearest_points[1], Vector3(15, 0, 0), + distance_tolerance)); + } + + result.clear(); + tf2.translation() = Vector3(20, 0, 20); + distance(&s1, tf1, &s2, tf2, request, result); + + S expected_dist = + (tf1.translation() - tf2.translation()).norm() - radius_1 - radius_2; + EXPECT_NEAR(result.min_distance, expected_dist, distance_tolerance); + // TODO(JS): Only GST_LIBCCD can compute the pair of nearest points on the + // surface of the spheres. + if (solver_type == GST_LIBCCD) + { + Vector3 dir = (tf2.translation() - tf1.translation()).normalized(); + Vector3 p0_expected = dir * radius_1; + EXPECT_TRUE(CompareMatrices(result.nearest_points[0], p0_expected, + distance_tolerance)); + Vector3 p1_expected = tf2.translation() - dir * radius_2; + EXPECT_TRUE(CompareMatrices(result.nearest_points[1], p1_expected, + distance_tolerance)); + } +} + +template +void test_distance_spherecapsule(GJKSolverType solver_type) +{ + Sphere s1{20}; + Capsule s2{10, 20}; + + Transform3 tf1{Transform3::Identity()}; + Transform3 tf2{Transform3::Identity()}; + + DistanceRequest request; + request.enable_signed_distance = true; + request.enable_nearest_points = true; + request.gjk_solver_type = solver_type; + + DistanceResult result; // Expecting distance to be 10 result.clear(); tf2.translation() = Vector3(40, 0, 0); - res = distance(&s1, tf1, &s2, tf2, request, result); - EXPECT_TRUE(res); - EXPECT_TRUE(std::abs(result.min_distance - 10) < 1e-6); - EXPECT_TRUE(result.nearest_points[0].isApprox(Vector3(20, 0, 0))); - EXPECT_TRUE(result.nearest_points[1].isApprox(Vector3(-10, 0, 0))); + distance(&s1, tf1, &s2, tf2, request, result); + EXPECT_NEAR(result.min_distance, 10, request.distance_tolerance); + EXPECT_TRUE(CompareMatrices(result.nearest_points[0], Vector3(20, 0, 0), + request.distance_tolerance)); + EXPECT_TRUE(CompareMatrices(result.nearest_points[1], Vector3(30, 0, 0), + request.distance_tolerance)); // Expecting distance to be -5 result.clear(); tf2.translation() = Vector3(25, 0, 0); - res = distance(&s1, tf1, &s2, tf2, request, result); + distance(&s1, tf1, &s2, tf2, request, result); - EXPECT_TRUE(res); // request.distance_tolerance is actually the square of the distance // tolerance, namely the difference between distance returned from FCL's EPA // implementation and the actual distance, is less than // sqrt(request.distance_tolerance). const S distance_tolerance = std::sqrt(request.distance_tolerance); - EXPECT_NEAR(result.min_distance, -5, distance_tolerance); - - // TODO(JS): Only GST_LIBCCD can compute the pair of nearest points on the - // surface of the spheres. + ASSERT_NEAR(result.min_distance, -5, distance_tolerance); if (solver_type == GST_LIBCCD) { - EXPECT_TRUE(result.nearest_points[0].isApprox(Vector3(20, 0, 0), - distance_tolerance)); - EXPECT_TRUE(result.nearest_points[1].isApprox(Vector3(-10, 0, 0), - distance_tolerance)); + // NOTE: Currently, only GST_LIBCCD computes the pair of nearest points. + EXPECT_TRUE(CompareMatrices(result.nearest_points[0], Vector3(20, 0, 0), + distance_tolerance * 100)); + EXPECT_TRUE(CompareMatrices(result.nearest_points[1], Vector3(15, 0, 0), + distance_tolerance * 100)); } } //============================================================================== -GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere) + +GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_ccd) { test_distance_spheresphere(GST_LIBCCD); +} + +GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_indep) +{ test_distance_spheresphere(GST_INDEP); } +GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_capsule_ccd) +{ + test_distance_spherecapsule(GST_LIBCCD); +} + +GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_capsule_indep) +{ + test_distance_spherecapsule(GST_INDEP); +} + //============================================================================== int main(int argc, char* argv[]) { diff --git a/test/test_fcl_sphere_sphere.cpp b/test/test_fcl_sphere_sphere.cpp index 66db15cc5..78e9cc8ee 100644 --- a/test/test_fcl_sphere_sphere.cpp +++ b/test/test_fcl_sphere_sphere.cpp @@ -41,6 +41,7 @@ #include #include +#include "eigen_matrix_compare.h" #include "fcl/narrowphase/collision_object.h" #include "fcl/narrowphase/distance.h" #include "fcl/narrowphase/distance_request.h" @@ -106,8 +107,8 @@ void CheckSphereToSphereDistance(const SphereSpecification& sphere1, enable_nearest_points, enable_signed_distance, &result); EXPECT_NEAR(min_distance, min_distance_expected, tol); EXPECT_NEAR(result.min_distance, min_distance_expected, tol); - EXPECT_TRUE(result.nearest_points[0].isApprox(p1_expected, tol)); - EXPECT_TRUE(result.nearest_points[1].isApprox(p2_expected, tol)); + EXPECT_TRUE(fcl::CompareMatrices(result.nearest_points[0], p1_expected, tol)); + EXPECT_TRUE(fcl::CompareMatrices(result.nearest_points[1], p2_expected, tol)); } template @@ -118,18 +119,16 @@ struct SphereSphereDistance { min_distance = (sphere1.p_FC - sphere2.p_FC).norm() - sphere1.radius - sphere2.radius; const fcl::Vector3 AB = (sphere1.p_FC - sphere2.p_FC).normalized(); - p_S1P1 = AB * -sphere1.radius; - p_S2P2 = AB * sphere2.radius; + p_WP1 = sphere1.p_FC + AB * -sphere1.radius; + p_WP2 = sphere2.p_FC + AB * sphere2.radius; } SphereSpecification sphere1; SphereSpecification sphere2; S min_distance; - // The closest point P1 on sphere 1 is expressed in the sphere 1 body frame - // S1. - // The closest point P2 on sphere 2 is expressed in the sphere 2 body frame - // S2. - fcl::Vector3 p_S1P1; - fcl::Vector3 p_S2P2; + // The closest point P1 on sphere 1 is expressed in the world frame W. + fcl::Vector3 p_WP1; + // The closest point P2 on sphere 2 is expressed in the world frame W. + fcl::Vector3 p_WP2; }; template @@ -146,27 +145,28 @@ void TestSeparatingSpheres(S tol, fcl::GJKSolverType solver_type) { bool enable_signed_distance = true; CheckSphereToSphereDistance( spheres[i], spheres[j], solver_type, true, enable_signed_distance, - sphere_sphere_distance.min_distance, sphere_sphere_distance.p_S1P1, - sphere_sphere_distance.p_S2P2, tol); + sphere_sphere_distance.min_distance, sphere_sphere_distance.p_WP1, + sphere_sphere_distance.p_WP2, tol); // Now switch the order of sphere 1 with sphere 2 in calling // fcl::distance function, and test again. CheckSphereToSphereDistance( spheres[j], spheres[i], solver_type, true, enable_signed_distance, - sphere_sphere_distance.min_distance, sphere_sphere_distance.p_S2P2, - sphere_sphere_distance.p_S1P1, tol); + sphere_sphere_distance.min_distance, sphere_sphere_distance.p_WP2, + sphere_sphere_distance.p_WP1, tol); enable_signed_distance = false; CheckSphereToSphereDistance( spheres[i], spheres[j], solver_type, true, enable_signed_distance, - sphere_sphere_distance.min_distance, sphere_sphere_distance.p_S1P1, - sphere_sphere_distance.p_S2P2, tol); + sphere_sphere_distance.min_distance, sphere_sphere_distance.p_WP1, + sphere_sphere_distance.p_WP2, tol); + // Now switch the order of sphere 1 with sphere 2 in calling // fcl::distance function, and test again. CheckSphereToSphereDistance( spheres[j], spheres[i], solver_type, true, enable_signed_distance, - sphere_sphere_distance.min_distance, sphere_sphere_distance.p_S2P2, - sphere_sphere_distance.p_S1P1, tol); + sphere_sphere_distance.min_distance, sphere_sphere_distance.p_WP2, + sphere_sphere_distance.p_WP1, tol); // TODO (hongkai.dai@tri.global): Test enable_nearest_points=false. } From f15ffc21c9c0f6f6ab7d5369e8889ccbc59c2233 Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Tue, 14 Aug 2018 06:34:43 -0700 Subject: [PATCH 12/34] Clean up Convex geometry (including fixing AABB computation error) (#325) * Clean up Convex geometry (bug, documentation, API, and testing) The Convex class had a bug -- bad computation of local AABB. That was the springboard that led to numerous changes. They include: 1. Correction of AABB calculation (with supporting unit tests). 2. Overhaul of Convex API - removed unused members (plane_normals, plane_dis, and edges) - renamed remaining members to be more consistent with mesh description. - modification of references to those members. 3. Added extensive doxygen documentation consistent with the new API. 4. Initial unit tests for Convex geometric properties (volume, center-of-mass, and inertia tensor). 5. Note issues in the code with TODOs and link to github issues. 6. Update Changelog.md to reflect the change in this PR. --- CHANGELOG.md | 4 + include/fcl/geometry/collision_geometry-inl.h | 7 + include/fcl/geometry/shape/convex-inl.h | 293 ++++------ include/fcl/geometry/shape/convex.h | 137 +++-- include/fcl/geometry/shape/utility-inl.h | 6 +- .../gjk_libccd-inl.h | 9 +- .../minkowski_diff-inl.h | 4 +- .../primitive_shape_algorithm/halfspace-inl.h | 4 +- .../primitive_shape_algorithm/plane-inl.h | 4 +- test/CMakeLists.txt | 1 + test/geometry/CMakeLists.txt | 1 + test/geometry/shape/CMakeLists.txt | 8 + test/geometry/shape/test_convex.cpp | 522 ++++++++++++++++++ 13 files changed, 763 insertions(+), 237 deletions(-) create mode 100644 test/geometry/CMakeLists.txt create mode 100644 test/geometry/shape/CMakeLists.txt create mode 100644 test/geometry/shape/test_convex.cpp diff --git a/CHANGELOG.md b/CHANGELOG.md index 1e4074f97..2ba8ab1e7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,6 +13,10 @@ * Switched to Eigen for math operations: [#96](https://github.com/flexible-collision-library/fcl/issues/96), [#150](https://github.com/flexible-collision-library/fcl/pull/150) * fcl::Transform defined to be an Isometry to facilitate inverses [#318](https://github.com/flexible-collision-library/fcl/pull/318) +* Geometry + + * Simplified Convex class, deprecating old constructor in favor of simpler, documented constructor: [#325](https://github.com/flexible-collision-library/fcl/pull/325) + * Broadphase * Fixed redundant pair checking of SpatialHashingCollisionManager: [#156](https://github.com/flexible-collision-library/fcl/pull/156) diff --git a/include/fcl/geometry/collision_geometry-inl.h b/include/fcl/geometry/collision_geometry-inl.h index 0fbfc01f6..b5695325e 100644 --- a/include/fcl/geometry/collision_geometry-inl.h +++ b/include/fcl/geometry/collision_geometry-inl.h @@ -141,6 +141,13 @@ S CollisionGeometry::computeVolume() const template Matrix3 CollisionGeometry::computeMomentofInertiaRelatedToCOM() const { + // TODO(SeanCurtis-TRI): This is *horribly* inefficient. In complex cases, + // this will require computing volume integrals three times. The + // CollisionGeometry class should have a single virtual function that will + // return all three quantities in one call so that particular sub-classes can + // override this to process this answer more efficiently. The default + // implementation can be exactly these three calls. + // See: https://github.com/flexible-collision-library/fcl/issues/327. Matrix3 C = computeMomentofInertia(); Vector3 com = computeCOM(); S V = computeVolume(); diff --git a/include/fcl/geometry/shape/convex-inl.h b/include/fcl/geometry/shape/convex-inl.h index eefe94a69..8c671067c 100644 --- a/include/fcl/geometry/shape/convex-inl.h +++ b/include/fcl/geometry/shape/convex-inl.h @@ -34,6 +34,7 @@ */ /** @author Jia Pan */ +/** @author Sean Curtis (2018) Modify API and correct implementation bugs. */ #ifndef FCL_SHAPE_CONVEX_INL_H #define FCL_SHAPE_CONVEX_INL_H @@ -49,59 +50,31 @@ class FCL_EXPORT Convex; //============================================================================== template -Convex::Convex( - Vector3* plane_normals, S* plane_dis, int num_planes_, - Vector3* points, int num_points_, int* polygons_) - : ShapeBase() -{ - plane_normals = plane_normals; - plane_dis = plane_dis; - num_planes = num_planes_; - points = points; - num_points = num_points_; - polygons = polygons_; - edges = nullptr; - +Convex::Convex(int num_vertices, Vector3* vertices, + int num_faces, int* faces) + : ShapeBase(), + num_vertices(num_vertices), + vertices(vertices), + num_faces(num_faces), + faces(faces) { + assert(vertices != nullptr); + assert(faces != nullptr); + // Compute an interior point. We're computing the mean point and *not* some + // alternative such as the centroid or bounding box center. Vector3 sum = Vector3::Zero(); - for(int i = 0; i < num_points; ++i) - { - sum += points[i]; + for(int i = 0; i < num_vertices; ++i) { + sum += vertices[i]; } - - center = sum * (S)(1.0 / num_points); - - fillEdges(); -} - -//============================================================================== -template -Convex::Convex(const Convex& other) - : ShapeBase(other) -{ - plane_normals = other.plane_normals; - plane_dis = other.plane_dis; - num_planes = other.num_planes; - points = other.points; - polygons = other.polygons; - edges = new Edge[other.num_edges]; - memcpy(edges, other.edges, sizeof(Edge) * num_edges); -} - -//============================================================================== -template -Convex::~Convex() -{ - delete [] edges; + interior_point = sum * (S)(1.0 / num_vertices); } //============================================================================== template -void Convex::computeLocalAABB() -{ - this->aabb_local.min_.setConstant(-std::numeric_limits::max()); - this->aabb_local.max_.setConstant(std::numeric_limits::max()); - for(int i = 0; i < num_points; ++i) - this->aabb_local += points[i]; +void Convex::computeLocalAABB() { + this->aabb_local.min_.setConstant(std::numeric_limits::max()); + this->aabb_local.max_.setConstant(-std::numeric_limits::max()); + for(int i = 0; i < num_vertices; ++i) + this->aabb_local += vertices[i]; this->aabb_center = this->aabb_local.center(); this->aabb_radius = (this->aabb_local.min_ - this->aabb_center).norm(); @@ -109,15 +82,18 @@ void Convex::computeLocalAABB() //============================================================================== template -NODE_TYPE Convex::getNodeType() const -{ +NODE_TYPE Convex::getNodeType() const { return GEOM_CONVEX; } //============================================================================== +// TODO(SeanCurtis-TRI): When revisiting these, consider the following +// resources: +// https://www.geometrictools.com/Documentation/PolyhedralMassProperties.pdf +// http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.56.127&rep=rep1&type=pdf +// http://number-none.com/blow/inertia/bb_inertia.doc template -Matrix3 Convex::computeMomentofInertia() const -{ +Matrix3 Convex::computeMomentofInertia() const { Matrix3 C = Matrix3::Zero(); Matrix3 C_canonical; @@ -125,35 +101,37 @@ Matrix3 Convex::computeMomentofInertia() const 1/120.0, 1/ 60.0, 1/120.0, 1/120.0, 1/120.0, 1/ 60.0; - int* points_in_poly = polygons; - int* index = polygons + 1; - for(int i = 0; i < num_planes; ++i) - { - Vector3 plane_center = Vector3::Zero(); - - // compute the center of the polygon - for(int j = 0; j < *points_in_poly; ++j) - plane_center += points[index[j]]; - plane_center = plane_center * (1.0 / *points_in_poly); - - // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero) of the convex shape - const Vector3& v3 = plane_center; - for(int j = 0; j < *points_in_poly; ++j) - { + S vol_times_six = 0; + int* face_encoding = faces; + int* index = faces + 1; + for(int i = 0; i < num_faces; ++i) { + const int vertex_count = *face_encoding; + Vector3 face_center = Vector3::Zero(); + + // Compute the center of the face. + for(int j = 0; j < vertex_count; ++j) + face_center += vertices[index[j]]; + face_center = face_center * (1.0 / vertex_count); + + // Compute the volume of tetrahedron formed by the vertices on one of the + // polygon's edges, the center point, and the shape's frame's origin. + const Vector3& v3 = face_center; + for(int j = 0; j < vertex_count; ++j) { int e_first = index[j]; - int e_second = index[(j+1)%*points_in_poly]; - const Vector3& v1 = points[e_first]; - const Vector3& v2 = points[e_second]; + int e_second = index[(j + 1) % vertex_count]; + const Vector3& v1 = vertices[e_first]; + const Vector3& v2 = vertices[e_second]; S d_six_vol = (v1.cross(v2)).dot(v3); Matrix3 A; // this is A' in the original document A.row(0) = v1; A.row(1) = v2; A.row(2) = v3; C += A.transpose() * C_canonical * A * d_six_vol; // change accordingly + vol_times_six += d_six_vol; } - points_in_poly += (*points_in_poly + 1); - index = points_in_poly + 1; + face_encoding += vertex_count + 1; + index = face_encoding + 1; } S trace_C = C(0, 0) + C(1, 1) + C(2, 2); @@ -163,148 +141,101 @@ Matrix3 Convex::computeMomentofInertia() const -C(1, 0), trace_C - C(1, 1), -C(1, 2), -C(2, 0), -C(2, 1), trace_C - C(2, 2); - return m; + return m * (6 / vol_times_six); } //============================================================================== template -Vector3 Convex::computeCOM() const -{ +Vector3 Convex::computeCOM() const { Vector3 com = Vector3::Zero(); S vol = 0; - int* points_in_poly = polygons; - int* index = polygons + 1; - for(int i = 0; i < num_planes; ++i) - { - Vector3 plane_center = Vector3::Zero(); - - // compute the center of the polygon - for(int j = 0; j < *points_in_poly; ++j) - plane_center += points[index[j]]; - plane_center = plane_center * (1.0 / *points_in_poly); - - // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero) of the convex shape - const Vector3& v3 = plane_center; - for(int j = 0; j < *points_in_poly; ++j) - { + int* face_encoding = faces; + int* index = faces + 1; + for(int i = 0; i < num_faces; ++i) { + const int vertex_count = *face_encoding; + Vector3 face_center = Vector3::Zero(); + + // TODO(SeanCurtis-TRI): See note in computeVolume() on the efficiency of + // this approach. + + // Compute the center of the polygon. + for(int j = 0; j < vertex_count; ++j) + face_center += vertices[index[j]]; + face_center = face_center * (1.0 / vertex_count); + + // Compute the volume of tetrahedron formed by the vertices on one of the + // polygon's edges, the center point, and the shape's frame's origin. + const Vector3& v3 = face_center; + for(int j = 0; j < vertex_count; ++j) { int e_first = index[j]; - int e_second = index[(j+1)%*points_in_poly]; - const Vector3& v1 = points[e_first]; - const Vector3& v2 = points[e_second]; + int e_second = index[(j + 1) % vertex_count]; + const Vector3& v1 = vertices[e_first]; + const Vector3& v2 = vertices[e_second]; S d_six_vol = (v1.cross(v2)).dot(v3); vol += d_six_vol; - com += (points[e_first] + points[e_second] + plane_center) * d_six_vol; + com += (vertices[e_first] + vertices[e_second] + face_center) * d_six_vol; } - points_in_poly += (*points_in_poly + 1); - index = points_in_poly + 1; + face_encoding += vertex_count + 1; + index = face_encoding + 1; } return com / (vol * 4); // here we choose zero as the reference } //============================================================================== -template -S Convex::computeVolume() const -{ +template S Convex::computeVolume() const { S vol = 0; - int* points_in_poly = polygons; - int* index = polygons + 1; - for(int i = 0; i < num_planes; ++i) - { - Vector3 plane_center = Vector3::Zero(); - - // compute the center of the polygon - for(int j = 0; j < *points_in_poly; ++j) - plane_center += points[index[j]]; - plane_center = plane_center * (1.0 / *points_in_poly); - - // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero point) of the convex shape - const Vector3& v3 = plane_center; - for(int j = 0; j < *points_in_poly; ++j) - { + int *face_encoding = faces; + int *index = faces + 1; + for(int i = 0; i < num_faces; ++i) { + const int vertex_count = *face_encoding; + Vector3 face_center = Vector3::Zero(); + + // TODO(SeanCurtis-TRI): While this is general, this is inefficient. If the + // face happens to be a triangle, this does 3X the requisite work. + // If the face is a 4-gon, then this does 2X the requisite work. + // As N increases in the N-gon this approach's inherent relative penalty + // shrinks. Ideally, this should at least key on 3-gon and 4-gon before + // falling through to this. + + // Compute the center of the polygon. + for(int j = 0; j < vertex_count; ++j) + face_center += vertices[index[j]]; + face_center = face_center * (1.0 / vertex_count); + + // TODO(SeanCurtis-TRI): Because volume serves as the weights for + // center-of-mass an inertia computations, it should be refactored into its + // own function that can be invoked by providing three vertices (the fourth + // being the origin). + + // Compute the volume of tetrahedron formed by the vertices on one of the + // polygon's edges, the center point, and the shape's frame's origin. + const Vector3& v3 = face_center; + for(int j = 0; j < vertex_count; ++j) { int e_first = index[j]; - int e_second = index[(j+1)%*points_in_poly]; - const Vector3& v1 = points[e_first]; - const Vector3& v2 = points[e_second]; + int e_second = index[(j + 1) % vertex_count]; + const Vector3& v1 = vertices[e_first]; + const Vector3& v2 = vertices[e_second]; S d_six_vol = (v1.cross(v2)).dot(v3); vol += d_six_vol; } - points_in_poly += (*points_in_poly + 1); - index = points_in_poly + 1; + face_encoding += vertex_count + 1; + index = face_encoding + 1; } return vol / 6; } -//============================================================================== -template -void Convex::fillEdges() -{ - int* points_in_poly = polygons; - if(edges) delete [] edges; - - int num_edges_alloc = 0; - for(int i = 0; i < num_planes; ++i) - { - num_edges_alloc += *points_in_poly; - points_in_poly += (*points_in_poly + 1); - } - - edges = new Edge[num_edges_alloc]; - - points_in_poly = polygons; - int* index = polygons + 1; - num_edges = 0; - Edge e; - bool isinset; - for(int i = 0; i < num_planes; ++i) - { - for(int j = 0; j < *points_in_poly; ++j) - { - e.first = std::min(index[j], index[(j+1)%*points_in_poly]); - e.second = std::max(index[j], index[(j+1)%*points_in_poly]); - isinset = false; - for(int k = 0; k < num_edges; ++k) - { - if((edges[k].first == e.first) && (edges[k].second == e.second)) - { - isinset = true; - break; - } - } - - if(!isinset) - { - edges[num_edges].first = e.first; - edges[num_edges].second = e.second; - ++num_edges; - } - } - - points_in_poly += (*points_in_poly + 1); - index = points_in_poly + 1; - } - - if(num_edges < num_edges_alloc) - { - Edge* tmp = new Edge[num_edges]; - memcpy(tmp, edges, num_edges * sizeof(Edge)); - delete [] edges; - edges = tmp; - } -} - //============================================================================== template std::vector> Convex::getBoundVertices( - const Transform3& tf) const -{ - std::vector> result(num_points); - for(int i = 0; i < num_points; ++i) + const Transform3& tf) const { + std::vector> result(num_vertices); + for(int i = 0; i < num_vertices; ++i) { - result[i] = tf * points[i]; + result[i] = tf * vertices[i]; } return result; diff --git a/include/fcl/geometry/shape/convex.h b/include/fcl/geometry/shape/convex.h index 11d663cc5..6d5fed874 100644 --- a/include/fcl/geometry/shape/convex.h +++ b/include/fcl/geometry/shape/convex.h @@ -34,6 +34,7 @@ */ /** @author Jia Pan */ +/** @author Sean Curtis (2018) Streamline API and document. */ #ifndef FCL_SHAPE_CONVEX_H #define FCL_SHAPE_CONVEX_H @@ -43,7 +44,37 @@ namespace fcl { -/// @brief Convex polytope +/// @brief A convex polytope +/// +/// The %Convex class represents a subset of general meshes: convex meshes. +/// The class represents its underlying mesh quite simply: an ordered list of +/// vertices, `V = [v₀, v₁, ..., vₙ₋₁]`, and an ordered list of faces, +/// `F = [f₀, f₁, ..., fₘ₋₁]`, built on those vertices (via vertex _indices_). +/// +/// A mesh is only compatible with %Convex if it satisfies the following +/// properties: +/// - Each face, fᵢ, must be planar and completely lies on a supporting +/// plane πᵢ. The ordered sets `F = [f₀, f₁, ..., fₙ₋₁]` and +/// `Π = [π₀, π₁, ..., πₙ₋₁]` are defined such that face fᵢ has supporting +/// plane πᵢ. +/// - A face fᵢ is defined by an ordered list of vertex indices (e.g., +/// `fᵢ = [iᵢ₀, iᵢ₁, ..., iᵢₖ₋₁]` (for a face with k vertices and k edges). +/// The _ordering_ of the vertex indices must visit the face's vertices in a +/// _counter-clockwise_ order when viewed from outside the mesh and the +/// vertices must all lie on the face's supporting plane. +/// - Define functions `πᵢ(x)` which report the signed distance from point `x` +/// to plane `πᵢ`. To be convex, it must be true that +/// `π(v) ≤ 0, ∀ π ∈ Π ∧ v ∈ V`. +/// +/// If those requirements are satisfied, for a given convex region, the mesh +/// with the smallest number of faces *may* have non-triangular faces. In fact, +/// they could be polygons with an arbitrary number of edges/vertices. The +/// %Convex class supports compact representation. But it *also* supports +/// representations that decompose a large n-gon into a set of smaller polygons +/// or triangles. In this case, each of the triangles supporting planes would +/// be the same and the requirements listed above would still be satisfied. +/// +/// @tparam S_ The scalar type; must be a valid Eigen scalar. template class FCL_EXPORT Convex : public ShapeBase { @@ -51,65 +82,85 @@ class FCL_EXPORT Convex : public ShapeBase using S = S_; - /// @brief Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information - Convex(Vector3* plane_normals, - S* plane_dis, - int num_planes, - Vector3* points, - int num_points, - int* polygons); + // TODO(SeanCurtis-TRI): A huge shopping list of issues with this class are + // enumerated in https://github.com/flexible-collision-library/fcl/issues/326. + + /// @brief Constructor + /// + /// @note: The %Convex geometry does *not* take ownership of any of the data + /// provided. The data must remain valid for as long as the %Convex instance + /// and must be cleaned up explicitly. + /// + /// @warning: The %Convex class does *not* validate the input; it trusts that + /// the inputs truly represent a coherent convex polytope. + /// + /// @param num_vertices The number of vertices defined in `vertices`. + /// @param vertices The positions of the polytope vertices. + /// @param num_faces The number of faces defined in `faces`. + /// @param faces Encoding of the polytope faces. Must encode + /// `num_faces` number of faces. See member + /// documentation for details on encoding. + Convex(int num_vertices, Vector3* vertices, int num_faces, int* faces); /// @brief Copy constructor - Convex(const Convex& other); + Convex(const Convex& other) = default; - ~Convex(); + ~Convex() = default; - /// @brief Compute AABB + /// @brief Compute AABB in the geometry's canonical frame. void computeLocalAABB() override; - /// @brief Get node type: a conex polytope + /// @brief Get node type: a convex polytope. NODE_TYPE getNodeType() const override; - - Vector3* plane_normals; - S* plane_dis; - - /// @brief An array of indices to the points of each polygon, it should be the number of vertices - /// followed by that amount of indices to "points" in counter clockwise order - int* polygons; - - Vector3* points; - int num_points; - int num_edges; - int num_planes; - - struct Edge - { - int first, second; - }; - - Edge* edges; - - /// @brief center of the convex polytope, this is used for collision: center is guaranteed in the internal of the polytope (as it is convex) - Vector3 center; - - /// based on http://number-none.com/blow/inertia/bb_inertia.doc + /// @brief The total number of vertices in the convex mesh. + int num_vertices; + + /// @brief The vertex positions in the geometry's frame G. + Vector3* vertices; + + /// @brief The total number of faces in the convex mesh. + int num_faces; + + /// @brief The representation of the *faces* of the convex hull. + /// + /// The array is the concatenation of an integer-based representations of each + /// face. A single face is encoded as a sub-array of ints where the first int + /// is the *number* n of vertices in the face, and the following n values + /// are ordered indices into `vertices` which visit the vertex values in a + /// *counter-clockwise* order (viewed from the outside). + /// + /// For a well-formed face `f` consisting of indices [i₀, i₁, ..., iₘ₋₁], it + /// should be the case that: + /// + /// `eⱼ × eⱼ₊₁ · n̂ₚ = |eⱼ × eⱼ₊₁|, ∀ 0 ≤ j < m, j ∈ ℤ`, where + /// `n̂ₚ` is the normal for plane `π` on which face `f` lies. + /// `eⱼ = vertices[iⱼ] - vertices[iⱼ₋₁]` is the edge vector of face `f` + /// defined by adjacent vertex indices at jᵗʰ vertex (wrapping around such + /// that `j - 1 = m - 1` for `j = 0`). + /// + /// Satisfying this condition implies the following: + /// 1. vertices are not coincident and + /// 3. the indices of the face correspond to a proper counter-clockwise + /// ordering. + int* faces; + + /// @brief A point guaranteed to be on the interior of the convex polytope, + /// used for collision. + Vector3 interior_point; + + // Documentation inherited. Matrix3 computeMomentofInertia() const override; - // Documentation inherited + // Documentation inherited. Vector3 computeCOM() const override; - // Documentation inherited + // Documentation inherited. S computeVolume() const override; /// @brief get the vertices of some convex shape which can bound this shape in /// a specific configuration std::vector> getBoundVertices(const Transform3& tf) const; - -protected: - - /// @brief Get edge information - void fillEdges(); }; using Convexf = Convex; diff --git a/include/fcl/geometry/shape/utility-inl.h b/include/fcl/geometry/shape/utility-inl.h index 1fa77fc7a..518b64199 100644 --- a/include/fcl/geometry/shape/utility-inl.h +++ b/include/fcl/geometry/shape/utility-inl.h @@ -234,9 +234,9 @@ struct FCL_EXPORT ComputeBVImpl, Convex> const Vector3& T = tf.translation(); AABB bv_; - for(int i = 0; i < s.num_points; ++i) + for(int i = 0; i < s.num_vertices; ++i) { - Vector3 new_p = R * s.points[i] + T; + Vector3 new_p = R * s.vertices[i] + T; bv_ += new_p; } @@ -250,7 +250,7 @@ struct FCL_EXPORT ComputeBVImpl, Convex> { static void run(const Convex& s, const Transform3& tf, OBB& bv) { - fit(s.points, s.num_points, bv); + fit(s.vertices, s.num_vertices, bv); bv.axis = tf.linear(); bv.To = tf * bv.To; 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 c41f12d32..dbaa5495b 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 @@ -2105,15 +2105,15 @@ static void supportConvex(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v ccd_real_t maxdot, dot; int i; Vector3* curp; - const auto& center = c->convex->center; + const auto& center = c->convex->interior_point; ccdVec3Copy(&dir, dir_); ccdQuatRotVec(&dir, &c->rot_inv); maxdot = -CCD_REAL_MAX; - curp = c->convex->points; + curp = c->convex->vertices; - for(i = 0; i < c->convex->num_points; ++i, curp += 1) + for(i = 0; i < c->convex->num_vertices; ++i, curp += 1) { ccdVec3Set(&p, (*curp)[0] - center[0], (*curp)[1] - center[1], (*curp)[2] - center[2]); dot = ccdVec3Dot(&dir, &p); @@ -2167,7 +2167,8 @@ template static void centerConvex(const void* obj, ccd_vec3_t* c) { const auto *o = static_cast*>(obj); - ccdVec3Set(c, o->convex->center[0], o->convex->center[1], o->convex->center[2]); + const Vector3& p = o->convex->interior_point; + ccdVec3Set(c, p[0], p[1], p[2]); ccdQuatRotVec(c, &o->rot); ccdVec3Add(c, &o->pos); } diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff-inl.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff-inl.h index f639f4bc3..fdd9c6628 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff-inl.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff-inl.h @@ -183,9 +183,9 @@ Vector3 getSupport( { const Convex* convex = static_cast*>(shape); S maxdot = - std::numeric_limits::max(); - Vector3* curp = convex->points; + Vector3* curp = convex->vertices; Vector3 bestv = Vector3::Zero(); - for(int i = 0; i < convex->num_points; ++i, curp+=1) + for(int i = 0; i < convex->num_vertices; ++i, curp+=1) { S dot = dir.dot(*curp); if(dot > maxdot) diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h index e89330df4..c11279ef6 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h @@ -500,9 +500,9 @@ bool convexHalfspaceIntersect(const Convex& s1, const Transform3& tf1, Vector3 v; S depth = std::numeric_limits::max(); - for(int i = 0; i < s1.num_points; ++i) + for(int i = 0; i < s1.num_vertices; ++i) { - Vector3 p = tf1 * s1.points[i]; + Vector3 p = tf1 * s1.vertices[i]; S d = new_s2.signedDistance(p); if(d < depth) diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane-inl.h index 858a8608f..1ecf16f65 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane-inl.h @@ -648,9 +648,9 @@ bool convexPlaneIntersect(const Convex& s1, const Transform3& tf1, Vector3 v_min, v_max; S d_min = std::numeric_limits::max(), d_max = -std::numeric_limits::max(); - for(int i = 0; i < s1.num_points; ++i) + for(int i = 0; i < s1.num_vertices; ++i) { - Vector3 p = tf1 * s1.points[i]; + Vector3 p = tf1 * s1.vertices[i]; S d = new_s2.signedDistance(p); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 1ae1ea5ce..c2156d2f2 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -97,4 +97,5 @@ foreach(test ${tests}) add_fcl_test(${test}) endforeach(test) +add_subdirectory(geometry) add_subdirectory(narrowphase) diff --git a/test/geometry/CMakeLists.txt b/test/geometry/CMakeLists.txt new file mode 100644 index 000000000..a87b0abfb --- /dev/null +++ b/test/geometry/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(shape) diff --git a/test/geometry/shape/CMakeLists.txt b/test/geometry/shape/CMakeLists.txt new file mode 100644 index 000000000..b227271ab --- /dev/null +++ b/test/geometry/shape/CMakeLists.txt @@ -0,0 +1,8 @@ +set(tests + test_convex.cpp + ) + +# Build all the tests +foreach(test ${tests}) + add_fcl_test(${test}) +endforeach(test) diff --git a/test/geometry/shape/test_convex.cpp b/test/geometry/shape/test_convex.cpp new file mode 100644 index 000000000..07770966d --- /dev/null +++ b/test/geometry/shape/test_convex.cpp @@ -0,0 +1,522 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. 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) (2018) */ + +// Tests the implementation of a convex polytope geometry. + +#include "fcl/geometry/shape/convex.h" + +#include + +#include +#include + +#include "eigen_matrix_compare.h" +#include "fcl/common/types.h" + +namespace fcl { +namespace { + +using std::max; + +// Necessary to satisfy Eigen's alignment requirements. See +// http://eigen.tuxfamily.org/dox-devel/group__TopicStlContainers.html#StlContainers_vector +template +using PoseVector = std::vector, + Eigen::aligned_allocator>>; + +// Utilities to print scalar type in error messages. +template +struct ScalarString { + static std::string value() { return "unknown"; } +}; + +template <> +struct ScalarString { + static std::string value() { return "double"; } +}; + +template <> +struct ScalarString { + static std::string value() { return "float"; } +}; + +// Base definition of a "unit" convex polytope. Specific instances should define +// faces, vertices, and quantities such as volume, center of mass, and moment of +// inertia in terms of a scale factor. +template +class Polytope { + public: + explicit Polytope(S scale) : scale_(scale) {} + + virtual int face_count() const = 0; + virtual int vertex_count() const = 0; + virtual S volume() const = 0; + virtual Vector3 com() const = 0; + virtual Matrix3 principal_inertia_tensor() const = 0; + virtual std::string description() const = 0; + + // The scale of the polytope to use with test tolerances. + S scale() const { return scale_; } + const Vector3* points() const { return &vertices_[0]; } + const int* polygons() const { return &polygons_[0]; } + Convex MakeConvex() const { + // The Polytope class makes the pointers to vertices and faces const access. + // The Convex class calls for non-const pointers. Temporarily const-casting + // them to make it compatible. + return Convex(vertex_count(), const_cast*>(points()), + face_count(), const_cast(polygons())); + } + Vector3 min_point() const { + Vector3 m; + m.setConstant(std::numeric_limits::max()); + for (const Vector3& v : vertices_) { + for (int i = 0; i < 3; ++i) { + if (v(i) < m(i)) m(i) = v(i); + } + } + return m; + } + Vector3 max_point() const { + Vector3 m; + m.setConstant(-std::numeric_limits::max()); + for (const Vector3& v : vertices_) { + for (int i = 0; i < 3; ++i) { + if (v(i) > m(i)) m(i) = v(i); + } + } + return m; + } + Vector3 aabb_center() const { + return (max_point() + min_point()) / 2; + } + S aabb_radius() const { return (min_point() - aabb_center()).norm(); } + void SetPose(const Transform3& X_WP) { + for (auto& v : vertices_) { + v = X_WP * v; + } + } + + protected: + void add_vertex(const Vector3& vertex) { vertices_.push_back(vertex); } + void add_face(std::initializer_list indices) { + polygons_.push_back(static_cast(indices.size())); + polygons_.insert(polygons_.end(), indices); + } + // Confirms the number of vertices and number of polygons matches the counts + // implied by vertex_count() and face_count(), respectively. + void confirm_data() { + // Confirm point count. + GTEST_ASSERT_EQ(vertex_count(), static_cast(vertices_.size())); + + // Confirm face count. + // Count the number of faces encoded in polygons_; + int count = 0; + int i = 0; + while (i < static_cast(polygons_.size())) { + ++count; + i += polygons_[i] + 1; + } + GTEST_ASSERT_EQ(i, static_cast(polygons_.size())) + << "Badly defined polygons"; + GTEST_ASSERT_EQ(face_count(), count); + } + + private: + std::vector> vertices_; + std::vector polygons_; + S scale_{1}; +}; + +// A simple regular tetrahedron with edges of length `scale` centered on the +// origin. +template +class EquilateralTetrahedron : public Polytope { + public: + // Constructs the tetrahedron (of edge length `s`). + explicit EquilateralTetrahedron(S scale) : Polytope(scale), scale_(scale) { + // Tetrahedron vertices in the tet's canonical frame T. The tet is + // "centered" on the origin so that it's center of mass is simple [0, 0, 0]. + const S z_base = -1 / S(2 * sqrt(6.)); + Vector3 points_T[] = {{S(0.5), S(-0.5 / sqrt(3.)), z_base}, + {S(-0.5), S(-0.5 / sqrt(3.)), z_base}, + {S(0), S(1. / sqrt(3.)), z_base}, + {S(0), S(0), S(sqrt(3. / 8))}}; + for (const auto& v : points_T) { + this->add_vertex(scale * v); + }; + + // Now add the polygons + this->add_face({0, 1, 2}); + this->add_face({1, 0, 3}); + this->add_face({0, 2, 3}); + this->add_face({2, 1, 3}); + + this->confirm_data(); + } + // Properties of the polytope. + int face_count() const final { return 4; } + int vertex_count() const final { return 4; } + virtual S volume() const final { + // This assumes unit mass. + S s = this->scale(); + return S(sqrt(2) / 12) * s * s * s; + } + virtual Vector3 com() const final { return Vector3::Zero(); } + virtual Matrix3 principal_inertia_tensor() const { + // TODO(SeanCurtis-TRI): Replace this with a legitimate tensor. + throw std::logic_error("Not implemented yet"); + }; + std::string description() const final { + return "Tetrahedron with scale: " + std::to_string(this->scale()); + } + + private: + S scale_{0}; +}; + +// A simple cube with sides of length `scale`. +template +class Cube : public Polytope { + public: + Cube(S scale) : Polytope(scale) { + // Cube vertices in the cube's canonical frame C. + Vector3 points_C[] = {{S(-0.5), S(-0.5), S(-0.5)}, // v0 + {S(0.5), S(-0.5), S(-0.5)}, // v1 + {S(-0.5), S(0.5), S(-0.5)}, // v2 + {S(0.5), S(0.5), S(-0.5)}, // v3 + {S(-0.5), S(-0.5), S(0.5)}, // v4 + {S(0.5), S(-0.5), S(0.5)}, // v5 + {S(-0.5), S(0.5), S(0.5)}, // v6 + {S(0.5), S(0.5), S(0.5)}}; // v7 + for (const auto& v : points_C) { + this->add_vertex(scale * v); + } + + // Now add the polygons + this->add_face({1, 3, 7, 5}); // +x + this->add_face({0, 4, 6, 2}); // -x + this->add_face({4, 5, 7, 6}); // +y + this->add_face({0, 2, 3, 1}); // -y + this->add_face({6, 7, 3, 2}); // +z + this->add_face({0, 1, 5, 4}); // -z + + this->confirm_data(); + } + + // Polytope properties + int face_count() const final { return 6; } + int vertex_count() const final { return 8; } + virtual S volume() const final { + S s = this->scale(); + return s * s * s; + } + virtual Vector3 com() const final { return Vector3::Zero(); } + virtual Matrix3 principal_inertia_tensor() const { + S scale_sqd = this->scale() * this->scale(); + // This assumes unit mass. + return Eigen::DiagonalMatrix(1. / 6., 1. / 6., 1. / 6.) * scale_sqd; + }; + std::string description() const final { + return "Cube with scale: " + std::to_string(this->scale()); + } +}; + +void testConvexConstruction() { + Cube cube{1}; + // Set the cube at some other location to make sure that the interior point + // test/ doesn't pass just because it initialized to zero. + Vector3 p_WB(1, 2, 3); + cube.SetPose(Transform3(Eigen::Translation3d(p_WB))); + Convex convex = cube.MakeConvex(); + + // This doesn't depend on the correct logic in the constructor. But this is + // as convenient a time as any to test that it reports the right node type. + EXPECT_EQ(convex.getNodeType(), GEOM_CONVEX); + + // The constructor computes the interior point. + EXPECT_TRUE(CompareMatrices(convex.interior_point, p_WB)); +} + +template