diff --git a/include/fcl/math/constants.h b/include/fcl/math/constants.h index e3ecfdfa6..0ba4e91c2 100644 --- a/include/fcl/math/constants.h +++ b/include/fcl/math/constants.h @@ -39,17 +39,132 @@ #include "fcl/common/types.h" +#include +#include + namespace fcl { +namespace detail +{ + +// Helper struct for determining the underlying numerical type of scalars. +// Allows us to treat AutoDiffScalar and double as double type and +// AutoDIffScalar and float as float type. +template +struct ScalarTrait { + typedef typename S::Real type; +}; + +template <> +struct ScalarTrait { + typedef double type; +}; + +template <> +struct ScalarTrait { + typedef float type; +}; + +} // namespace detail + +/// A collection of scalar-dependent constants. This provides the ability to +/// get constant and tolerance values that are appropriately scaled and typed +/// to the scalar type `S`. +/// +/// Constants `pi()` and `phi()` are returned in the literal scalar type `S`. +/// In other words, if `S` is an `AutoDiffScalar<...>`, then the value of `pi` +/// and `phi` are likewise `AutoDiffScalar<...>` typed. +/// +/// Tolerances (e.g., `eps()` and its variants) are always provided in the +/// scalar's numerical representation. In other words, if `S` is a `double` or +/// `float`, the tolerances are given as `double` and `float` respectively. +/// For `AutoDiffScalar` it is more interesting. The `AutoDiffScalar` has an +/// underlying numerical representation (e.g., +/// `AutoDiffScalar>` has a double). It is the type of this +/// underlying numerical representation that is provided by the tolerance +/// functions. +/// +/// Generally, this is designed to work with `float`, `double`, `long double`, +/// and `AutoDiffScalar`. However, custom scalars will also work provided that +/// the scalar type provides a class member type `Real` which must be one of +/// `long double`, `double`, or `float`. E.g., +/// +/// ``` +/// struct MyScalar { +/// public: +/// typedef double Real; +/// ... +/// }; +/// ``` +/// +/// @note The tolerance values provided are defined so as to provide varying +/// precision that *scales* with the underlying numerical type. The +/// following contrast will make it clear. +/// ``` +/// S local_eps = 10 * std::numeric_limit::epsilon(); +/// ``` +/// The above example shows a common basis for defining a local epsilon. It +/// defines it as being an order of magnitude larger than the machine epsilon +/// for `S`. However, if `S` is a float, it's machine precision is essentially +/// 1e-7. A full decimal digit of precision is 1/7th of the available digits. +/// In contrast, double machine precision is approximately 2e-16. Throwing away +/// a digit there reduces the precision by only 1/16th. This technique +/// disproportionately punishes lower-precision numerical representations. +/// Instead, by taking the machine precision, epsilon, and raising it to +/// fractional values, we *scale* the precision. Roughly, `pow(epsilon, 1/2)` +/// gives us half the precision (3.5e-4 for floats and 1.5e-8 for doubles). +/// Similarly powers of 3/4 and 7/8 gives us three quarters and 7/8ths of +/// the bits of precision. +/// +/// \tparam S The scalar type for which constant values will be retrieved. template struct FCL_EXPORT constants { +typedef typename detail::ScalarTrait::type RealType; + /// The mathematical constant pi static constexpr S pi() { return S(3.141592653589793238462643383279502884197169399375105820974944592L); } /// The golden ratio static constexpr S phi() { return S(1.618033988749894848204586834365638117720309179805762862135448623L); } + +/// Defines the default accuracy for gjk and epa tolerance. It is defined as +/// pow(epsilon, 7/8) -- where epsilon is the machine precision epsilon. This +/// value reflects a *slightly* tighter bound than the historical value of 1e-6 +/// used for 32-bit floats. +static RealType gjk_default_tolerance() { + static const RealType value = eps_78(); + return value; +} + +/// Returns ε for the precision of the underlying scalar type. +static RealType eps() { + static_assert(std::is_floating_point::value, + "Constants can only be evaluated for scalars with floating " + "point implementations"); + static const RealType value = std::numeric_limits::epsilon(); + return value; +} + +/// Returns pow(ε, 7/8) for the precision of the underlying scalar type. +static RealType eps_78() { + static const RealType value = std::pow(eps(), 7./8.); + return value; +} + +/// Returns pow(ε, 3/4) for the precision of the underlying scalar type. +static RealType eps_34() { + static const RealType value = std::pow(eps(), 3./4.); + return value; +} + +/// Returns pow(ε, 1/2) for the precision of the underlying scalar type. +static RealType eps_12() { + static const RealType value = std::pow(eps(), 1./2.); + return value; +} + }; using constantsf = constants; diff --git a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h b/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h index a8dbf3de8..e75236202 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h +++ b/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h @@ -948,11 +948,11 @@ template GJKSolver_indep::GJKSolver_indep() { gjk_max_iterations = 128; - gjk_tolerance = 1e-6; + gjk_tolerance = constants::gjk_default_tolerance(); epa_max_face_num = 128; epa_max_vertex_num = 64; epa_max_iterations = 255; - epa_tolerance = 1e-6; + epa_tolerance = constants::gjk_default_tolerance(); enable_cached_guess = false; cached_guess = Vector3(1, 0, 0); } diff --git a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h b/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h index 3bf490eb3..7f0a60b82 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h +++ b/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h @@ -902,7 +902,7 @@ GJKSolver_libccd::GJKSolver_libccd() { max_collision_iterations = 500; max_distance_iterations = 1000; - collision_tolerance = 1e-6; + collision_tolerance = constants::gjk_default_tolerance(); distance_tolerance = 1e-6; } diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 321b69b97..825cfc753 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -48,6 +48,7 @@ set(tests test_fcl_capsule_capsule.cpp test_fcl_cylinder_half_space.cpp test_fcl_collision.cpp + test_fcl_constant_eps.cpp test_fcl_distance.cpp test_fcl_frontlist.cpp test_fcl_general.cpp diff --git a/test/test_fcl_constant_eps.cpp b/test/test_fcl_constant_eps.cpp new file mode 100644 index 000000000..ace382662 --- /dev/null +++ b/test/test_fcl_constant_eps.cpp @@ -0,0 +1,118 @@ +/* + * 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 */ + +#include "fcl/math/constants.h" + +#include +#include + +#include +#include + +namespace fcl { +namespace { + +// Some autodiff helpers +template +using Vector2 = Eigen::Matrix; +template +using AutoDiff2 = Eigen::AutoDiffScalar>; + +// Utility function for confirming that the value returned by `constants` is +// the expected value based on the scalar type. +template +void expect_eps_values(const char* type_name) { + static_assert(std::is_floating_point::value, + "Only use this helper for float and double types"); + S expected_eps = std::numeric_limits::epsilon(); + EXPECT_EQ(constants::eps(), expected_eps) << "Failed for " << type_name; + EXPECT_EQ(std::pow(expected_eps, S(0.5)), constants::eps_12()) + << "Failed for " << type_name; + EXPECT_EQ(std::pow(expected_eps, S(0.75)), constants::eps_34()) + << "Failed for " << type_name; + EXPECT_EQ(std::pow(expected_eps, S(0.875)), constants::eps_78()) + << "Failed for " << type_name; +} + +// Test that the values returned are truly a function of the precision of the +// underlying type. +GTEST_TEST(FCL_CONSTANTS_EPS, precision_dependent) { + expect_eps_values("double"); + expect_eps_values("float"); + // Double check that the float value and double values are *not* equal. + EXPECT_NE(constantsd::eps(), constantsf::eps()); + EXPECT_NE(constantsd::eps_12(), constantsf::eps_12()); + EXPECT_NE(constantsd::eps_34(), constantsf::eps_34()); + EXPECT_NE(constantsd::eps_78(), constantsf::eps_78()); +} + +template void expect_autodiff_constants(const char *type_name) { + EXPECT_TRUE((std::is_same>::pi()), + AutoDiff2>::value)) + << "Failed for " << type_name; + EXPECT_TRUE((std::is_same>::phi()), + AutoDiff2>::value)) + << "Failed for " << type_name; + EXPECT_TRUE( + (std::is_same>::eps()), S>::value)) + << "Failed for " << type_name; + EXPECT_TRUE( + (std::is_same>::eps_78()), S>::value)) + << "Failed for " << type_name; + EXPECT_TRUE( + (std::is_same>::eps_34()), S>::value)) + << "Failed for " << type_name; + EXPECT_TRUE( + (std::is_same>::eps_12()), S>::value)) + << "Failed for " << type_name; +} + +// Test the types returned by constants. pi and phi should return autodiff, but +// the tolerances should return real types. +GTEST_TEST(FCL_CONSTANTS_EPS, autodiff_compatibility) { + expect_autodiff_constants("double"); + expect_autodiff_constants("float"); +} + +} // namespace +} // namespace fcl + +//============================================================================== +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file