From 235fc9555a4c4b39639144a237efc3f42722c690 Mon Sep 17 00:00:00 2001 From: "Jeongseok (JS) Lee" Date: Thu, 20 Nov 2025 14:26:48 -0800 Subject: [PATCH] Unify equality helpers and silence -Wfloat-equal - add valueEqual/isEqual/isApprox/isZero templates for scalars and Eigen types; handle +/-0 and NaNs - switch GenericJoint macro to valueEqual to avoid -Wfloat-equal in consumers - update GTest equals helper to share logic and added coverage tests Testing: pixi run lint; CI (Linux/macOS/Windows/gz-physics/Publish dartpy) --- dart/dynamics/detail/GenericJoint.hpp | 2 +- dart/math/Helpers.hpp | 208 ++++++++++++++++++++++++-- tests/helpers/GTestUtils.hpp | 109 ++++---------- tests/unit/CMakeLists.txt | 1 + tests/unit/math/test_ValueEqual.cpp | 144 ++++++++++++++++++ 5 files changed, 372 insertions(+), 92 deletions(-) create mode 100644 tests/unit/math/test_ValueEqual.cpp diff --git a/dart/dynamics/detail/GenericJoint.hpp b/dart/dynamics/detail/GenericJoint.hpp index 475a24a2c022b..2680cae285cda 100644 --- a/dart/dynamics/detail/GenericJoint.hpp +++ b/dart/dynamics/detail/GenericJoint.hpp @@ -82,7 +82,7 @@ } #define GenericJoint_SET_IF_DIFFERENT(mField, value) \ - if (value == Base::mAspectProperties.mField) \ + if (dart::math::valueEqual(value, Base::mAspectProperties.mField)) \ return; \ Base::mAspectProperties.mField = value; \ Joint::incrementVersion(); diff --git a/dart/math/Helpers.hpp b/dart/math/Helpers.hpp index b4f88cc6c1c8e..1a467f69df1de 100644 --- a/dart/math/Helpers.hpp +++ b/dart/math/Helpers.hpp @@ -34,13 +34,18 @@ #define DART_MATH_HELPERS_HPP_ // Standard Libraries +#include +#include #include #include +#include #include +#include #include #include #include +#include #include #include @@ -107,11 +112,6 @@ inline double Tsinc(double _theta) return 0.5 - sqrt(_theta) / 48; } -inline bool isZero(double _theta) -{ - return (std::abs(_theta) < 1e-6); -} - inline double asinh(double _X) { return log(_X + sqrt(_X * _X + 1)); @@ -166,17 +166,205 @@ inline typename DerivedA::PlainObject clip( return lower.cwiseMax(val.cwiseMin(upper)); } -inline bool isEqual(double _x, double _y) +template +constexpr T defaultAbsTolerance() +{ + return static_cast(1e-12); +} + +template +constexpr T defaultRelTolerance() +{ + return static_cast(1e-12); +} + +namespace detail { + +template +using FloatByteArray = std::array; + +template +inline bool valueEqualFloating(Float lhs, Float rhs) +{ + static_assert( + std::is_floating_point_v, + "valueEqualFloating expects floating point"); + + if (std::isnan(lhs) || std::isnan(rhs)) + return false; + + if (std::fpclassify(lhs) == FP_ZERO && std::fpclassify(rhs) == FP_ZERO) + return true; + + return std::bit_cast>(lhs) + == std::bit_cast>(rhs); +} + +} // namespace detail + +template +inline std::enable_if_t, bool> valueEqual( + const T& lhs, const T& rhs) +{ + return detail::valueEqualFloating(lhs, rhs); +} + +template +inline std:: + enable_if_t && !std::is_floating_point_v, bool> + valueEqual(const T& lhs, const T& rhs) +{ + return lhs == rhs; +} + +template +inline bool valueEqual( + const Eigen::MatrixBase& lhs, + const Eigen::MatrixBase& rhs) +{ + if (lhs.rows() != rhs.rows() || lhs.cols() != rhs.cols()) + return false; + + for (Eigen::Index r = 0; r < lhs.rows(); ++r) + for (Eigen::Index c = 0; c < lhs.cols(); ++c) + if (!valueEqual(lhs(r, c), rhs(r, c))) + return false; + + return true; +} + +template +inline bool isEqual(const T& lhs, const T& rhs) { - return (std::abs(_x - _y) < 1e-6); + return valueEqual(lhs, rhs); +} + +template +inline std::enable_if_t, bool> isApprox( + const T& lhs, + const T& rhs, + const T& abs_tol = defaultAbsTolerance(), + const T& rel_tol = defaultRelTolerance()) +{ + if (std::isnan(lhs) || std::isnan(rhs)) + return false; + + if (std::isinf(lhs) || std::isinf(rhs)) + return lhs == rhs; + + const T diff = std::abs(lhs - rhs); + const T scale = std::max(std::abs(lhs), std::abs(rhs)); + return diff <= std::max(abs_tol, rel_tol * scale); +} + +template +inline std::enable_if_t< + std::is_arithmetic_v && !std::is_floating_point_v, + bool> +isApprox(const T& lhs, const T& rhs, double abs_tol = 0.0, double rel_tol = 0.0) +{ + return isApprox( + static_cast(lhs), static_cast(rhs), abs_tol, rel_tol); +} + +template +inline std::enable_if_t< + std::is_floating_point_v< + typename DerivedA:: + Scalar> && std::is_floating_point_v, + bool> +isApprox( + const Eigen::MatrixBase& lhs, + const Eigen::MatrixBase& rhs, + const typename DerivedA::Scalar abs_tol + = defaultAbsTolerance(), + const typename DerivedA::Scalar rel_tol + = defaultRelTolerance()) +{ + if (lhs.rows() != rhs.rows() || lhs.cols() != rhs.cols()) + return false; + + for (Eigen::Index r = 0; r < lhs.rows(); ++r) + for (Eigen::Index c = 0; c < lhs.cols(); ++c) + if (!isApprox(lhs(r, c), rhs(r, c), abs_tol, rel_tol)) + return false; + + return true; +} + +template +inline std::enable_if_t< + std::is_arithmetic_v< + typename DerivedA:: + Scalar> && std::is_arithmetic_v && (!std::is_floating_point_v || !std::is_floating_point_v), + bool> +isApprox( + const Eigen::MatrixBase& lhs, + const Eigen::MatrixBase& rhs, + const std::common_type_t< + typename DerivedA::Scalar, + typename DerivedB::Scalar, + double> abs_tol + = std::common_type_t< + typename DerivedA::Scalar, + typename DerivedB::Scalar, + double>{0}, + const std::common_type_t< + typename DerivedA::Scalar, + typename DerivedB::Scalar, + double> rel_tol + = std::common_type_t< + typename DerivedA::Scalar, + typename DerivedB::Scalar, + double>{0}) +{ + using Common = std::common_type_t< + typename DerivedA::Scalar, + typename DerivedB::Scalar, + double>; + return isApprox( + lhs.template cast(), + rhs.template cast(), + static_cast(abs_tol), + static_cast(rel_tol)); +} + +template +inline std::enable_if_t, bool> isZero( + const T& value, const T& abs_tol = defaultAbsTolerance()) +{ + if (std::isnan(value)) + return false; + + return std::abs(value) <= abs_tol; +} + +template +inline std:: + enable_if_t && !std::is_floating_point_v, bool> + isZero(const T& value) +{ + return value == T{0}; +} + +template +inline std:: + enable_if_t, bool> + isZero( + const Eigen::MatrixBase& values, + const typename Derived::Scalar abs_tol + = defaultAbsTolerance()) +{ + if (values.size() == 0) + return true; + + return values.cwiseAbs().maxCoeff() <= abs_tol; } // check if it is an integer inline bool isInt(double _x) { - if (isEqual(round(_x), _x)) - return true; - return false; + return isApprox(round(_x), _x, static_cast(1e-6)); } /// \brief Returns whether _v is a NaN (Not-A-Number) value diff --git a/tests/helpers/GTestUtils.hpp b/tests/helpers/GTestUtils.hpp index 8125303480da9..a34ea571087b7 100644 --- a/tests/helpers/GTestUtils.hpp +++ b/tests/helpers/GTestUtils.hpp @@ -34,11 +34,16 @@ #define DART_UNITTESTS_GTESTUTILS_HPP_ #include "dart/math/Geometry.hpp" +#include "dart/math/Helpers.hpp" #include "dart/math/MathTypes.hpp" #include #include +#include + +#include + //============================================================================== #define EXPECT_VECTOR_DOUBLE_EQ(vec1, vec2) \ if (!::dart::test::equals(vec1, vec2)) { \ @@ -153,95 +158,37 @@ namespace dart { namespace test { -namespace detail { - -template -struct EqualsImpl +//============================================================================== +/// Returns true if the two matrices are equal within the given bound +template +bool equals( + const T1& expected, + const T2& actual, + std::common_type_t tol + = static_cast< + std::common_type_t>( + 1e-5)) { - static bool run( - const Eigen::DenseBase& expected, - const Eigen::DenseBase& actual, - typename DerivedA::Scalar tol) - { - // Get the matrix sizes and sanity check the call - const std::size_t n1 = expected.cols(), m1 = expected.rows(); - const std::size_t n2 = actual.cols(), m2 = actual.rows(); - if (m1 != m2 || n1 != n2) - return false; + if (expected.rows() != actual.rows() || expected.cols() != actual.cols()) + return false; - // Check each index - for (std::size_t i = 0; i < m1; i++) { - for (std::size_t j = 0; j < n1; j++) { - if (std::isnan(expected(i, j)) ^ std::isnan(actual(i, j))) { - return false; - } else if (std::abs(expected(i, j)) > 1) { - // Test relative error for values that are larger than 1 - if (std::abs((expected(i, j) - actual(i, j)) / expected(i, j)) > tol) - return false; - } else if (std::abs(expected(i, j) - actual(i, j)) > tol) { - return false; - } - } - } + using CommonScalar + = std::common_type_t; - // If no problems, the two matrices are equal - return true; - } -}; + for (Eigen::Index i = 0; i < expected.rows(); ++i) { + for (Eigen::Index j = 0; j < expected.cols(); ++j) { + const CommonScalar lhs = static_cast(expected(i, j)); + const CommonScalar rhs = static_cast(actual(i, j)); -// Workaround to resolve: "fpclassify': ambiguous call to overloaded function -// Reference: https://stackoverflow.com/a/61646279 -#ifdef _WIN32 -template -struct EqualsImpl< - DerivedA, - DerivedB, - std::enable_if_t::value>> -{ - static bool run( - const Eigen::DenseBase& expected, - const Eigen::DenseBase& actual, - typename DerivedA::Scalar tol) - { - // Get the matrix sizes and sanity check the call - const std::size_t n1 = expected.cols(), m1 = expected.rows(); - const std::size_t n2 = actual.cols(), m2 = actual.rows(); - if (m1 != m2 || n1 != n2) - return false; + if (std::isnan(lhs) && std::isnan(rhs)) + continue; - // Check each index - for (std::size_t i = 0; i < m1; i++) { - for (std::size_t j = 0; j < n1; j++) { - if (std::isnan(static_cast(expected(i, j))) - ^ std::isnan(static_cast(actual(i, j)))) { - return false; - } else if (std::abs(expected(i, j)) > 1) { - // Test relative error for values that are larger than 1 - if (std::abs((expected(i, j) - actual(i, j)) / expected(i, j)) > tol) - return false; - } else if (std::abs(expected(i, j) - actual(i, j)) > tol) { - return false; - } - } + if (!dart::math::isApprox(lhs, rhs, tol, tol)) + return false; } - - // If no problems, the two matrices are equal - return true; } -}; -#endif - -} // namespace detail -//============================================================================== -/// Returns true if the two matrices are equal within the given bound -template -bool equals( - const T1& expected, - const T2& actual, - typename T1::Scalar tol = static_cast(1e-5)) -{ - return detail::EqualsImpl::run(expected, actual, tol); + return true; } //============================================================================== diff --git a/tests/unit/CMakeLists.txt b/tests/unit/CMakeLists.txt index 3fa2ae4797763..459d824bd1129 100644 --- a/tests/unit/CMakeLists.txt +++ b/tests/unit/CMakeLists.txt @@ -80,6 +80,7 @@ dart_build_tests( math/test_Icosphere.cpp math/test_Math.cpp math/test_Random.cpp + math/test_ValueEqual.cpp math/test_TriMesh.cpp ) diff --git a/tests/unit/math/test_ValueEqual.cpp b/tests/unit/math/test_ValueEqual.cpp new file mode 100644 index 0000000000000..1402ccea087d5 --- /dev/null +++ b/tests/unit/math/test_ValueEqual.cpp @@ -0,0 +1,144 @@ +/* + * Copyright (c) 2011-2025, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/main/LICENSE + * + * This file is provided under the following "BSD-style" License: + * 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. + * 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 HOLDER 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. + */ + +#include "dart/math/Helpers.hpp" + +#include +#include + +#include + +#include + +using dart::math::valueEqual; + +TEST(ValueEqualTest, ScalarFloatingPointEquality) +{ + EXPECT_TRUE(valueEqual(1.0, 1.0)); + EXPECT_FALSE(valueEqual(1.0, 2.0)); + + const double base = 1.0; + const double next + = std::nextafter(base, std::numeric_limits::infinity()); + EXPECT_FALSE(valueEqual(base, next)); + + const double nanVal = std::numeric_limits::quiet_NaN(); + EXPECT_FALSE(valueEqual(nanVal, nanVal)); + EXPECT_FALSE(valueEqual(nanVal, 0.0)); + + EXPECT_TRUE(valueEqual( + std::numeric_limits::infinity(), + std::numeric_limits::infinity())); + EXPECT_FALSE(valueEqual( + std::numeric_limits::infinity(), + -std::numeric_limits::infinity())); +} + +TEST(ValueEqualTest, SignedZeroesAreEqual) +{ + EXPECT_TRUE(valueEqual(0.0, -0.0)); + EXPECT_TRUE(valueEqual(-0.0f, 0.0f)); +} + +TEST(ValueEqualTest, IntegralEquality) +{ + EXPECT_TRUE(valueEqual(7, 7)); + EXPECT_FALSE(valueEqual(7, 8)); + EXPECT_TRUE(valueEqual(static_cast(3), static_cast(3))); +} + +TEST(ValueEqualTest, EigenContainers) +{ + Eigen::Vector2d a; + a << 0.0, -0.0; + + Eigen::Vector2d b; + b << -0.0, 0.0; + + EXPECT_TRUE(valueEqual(a, b)); + + Eigen::Vector2d c = a; + c[1] = std::nextafter(1.0, 2.0); + EXPECT_FALSE(valueEqual(a, c)); +} + +TEST(ValueEqualTest, IsApproxScalar) +{ + using dart::math::isApprox; + + EXPECT_TRUE(isApprox(1.0, 1.0)); + EXPECT_TRUE(isApprox(1.0, 1.0 + 1e-7, 1e-6, 1e-6)); + EXPECT_FALSE(isApprox(1.0, 1.0 + 1e-3, 1e-6, 1e-6)); + + EXPECT_FALSE(isApprox( + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN())); +} + +TEST(ValueEqualTest, IsApproxEigen) +{ + using dart::math::isApprox; + + Eigen::Vector2d v1; + v1 << 1.0, 2.0; + Eigen::Vector2d v2 = v1; + v2[1] += 1e-7; + + EXPECT_TRUE(isApprox(v1, v2, 1e-6, 1e-6)); + EXPECT_FALSE(isApprox(v1, v2, 1e-8, 1e-8)); +} + +TEST(ValueEqualTest, IsZeroScalar) +{ + using dart::math::isZero; + + EXPECT_TRUE(isZero(0.0)); + EXPECT_TRUE(isZero(-0.0)); + EXPECT_TRUE(isZero(1e-7, 1e-6)); + EXPECT_FALSE(isZero(1e-3, 1e-6)); + EXPECT_TRUE(isZero(0)); + EXPECT_FALSE(isZero(5)); +} + +TEST(ValueEqualTest, IsZeroEigen) +{ + using dart::math::isZero; + + Eigen::Vector3d zeros = Eigen::Vector3d::Zero(); + EXPECT_TRUE(isZero(zeros)); + + Eigen::Vector3d nearZero = Eigen::Vector3d::Constant(5e-7); + EXPECT_TRUE(isZero(nearZero, 1e-6)); + + nearZero[1] = 1e-3; + EXPECT_FALSE(isZero(nearZero, 1e-6)); +}