diff --git a/platforms/nuttx/NuttX/include/cxx/cmath b/platforms/nuttx/NuttX/include/cxx/cmath new file mode 100644 index 0000000000..3125ea7625 --- /dev/null +++ b/platforms/nuttx/NuttX/include/cxx/cmath @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (C) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +// minimal cmath + +#pragma once + +#include +#include + +#include + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +namespace std +{ + +#if !defined(FLT_EPSILON) +#define FLT_EPSILON __FLT_EPSILON__ +#endif + + +#ifdef isfinite +#undef isfinite +#endif // isfinite + +inline bool isfinite(float value) { return __builtin_isfinite(value); } +inline bool isfinite(double value) { return __builtin_isfinite(value); } +inline bool isfinite(long double value) { return __builtin_isfinite(value); } + + +#ifdef isnan +#undef isnan +#endif // isnan + +inline bool isnan(float value) { return __builtin_isnan(value); } +inline bool isnan(double value) { return __builtin_isnan(value); } +inline bool isnan(long double value) { return __builtin_isnan(value); } + + +#ifdef isinf +#undef isinf +#endif // isinf + +inline bool isinf(float value) { return __builtin_isinf_sign(value); } +inline bool isinf(double value) { return __builtin_isinf_sign(value); } +inline bool isinf(long double value) { return __builtin_isinf_sign(value); } + +/* + * NuttX has no usable C++ math library, so we need to provide the needed definitions here manually. + */ +#define NUTTX_WRAP_MATH_FUN_UNARY(name) \ + inline float name(float x) { return ::name##f(x); } \ + inline double name(double x) { return ::name(x); } \ + inline long double name(long double x) { return ::name##l(x); } + +#define NUTTX_WRAP_MATH_FUN_BINARY(name) \ + inline float name(float x, float y) { return ::name##f(x, y); } \ + inline double name(double x, double y) { return ::name(x, y); } \ + inline long double name(long double x, long double y) { return ::name##l(x, y); } + +NUTTX_WRAP_MATH_FUN_UNARY(fabs) +NUTTX_WRAP_MATH_FUN_UNARY(log) +NUTTX_WRAP_MATH_FUN_UNARY(log10) +NUTTX_WRAP_MATH_FUN_UNARY(exp) +NUTTX_WRAP_MATH_FUN_UNARY(sqrt) +NUTTX_WRAP_MATH_FUN_UNARY(sin) +NUTTX_WRAP_MATH_FUN_UNARY(cos) +NUTTX_WRAP_MATH_FUN_UNARY(tan) +NUTTX_WRAP_MATH_FUN_UNARY(asin) +NUTTX_WRAP_MATH_FUN_UNARY(acos) +NUTTX_WRAP_MATH_FUN_UNARY(atan) +NUTTX_WRAP_MATH_FUN_UNARY(sinh) +NUTTX_WRAP_MATH_FUN_UNARY(cosh) +NUTTX_WRAP_MATH_FUN_UNARY(tanh) +NUTTX_WRAP_MATH_FUN_UNARY(ceil) +NUTTX_WRAP_MATH_FUN_UNARY(floor) + +NUTTX_WRAP_MATH_FUN_BINARY(pow) +NUTTX_WRAP_MATH_FUN_BINARY(atan2) + +} diff --git a/src/drivers/cyphal/Actuators/EscClient.hpp b/src/drivers/cyphal/Actuators/EscClient.hpp index c0c7a32cc9..c2bf03e6f2 100644 --- a/src/drivers/cyphal/Actuators/EscClient.hpp +++ b/src/drivers/cyphal/Actuators/EscClient.hpp @@ -58,6 +58,7 @@ #include // UDRAL Specification Messages +using std::isfinite; #include #include diff --git a/src/drivers/cyphal/Publishers/Publisher.hpp b/src/drivers/cyphal/Publishers/Publisher.hpp index cb04ef5fdd..2661a053ef 100644 --- a/src/drivers/cyphal/Publishers/Publisher.hpp +++ b/src/drivers/cyphal/Publishers/Publisher.hpp @@ -46,6 +46,8 @@ #include #include +#include +using std::isfinite; #include #include "../CanardHandle.hpp" diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index de9c62005d..526c4573dc 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -282,11 +282,11 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure + #include "UavcanPublisherBase.hpp" #include @@ -126,14 +128,14 @@ public: ecefpositionvelocity.velocity_xyz[2] = NAN; // Use ecef_position_velocity for now... There is no heading field - if (!isnan(gps.heading)) { + if (!std::isnan(gps.heading)) { ecefpositionvelocity.velocity_xyz[0] = gps.heading; - if (!isnan(gps.heading_offset)) { + if (!std::isnan(gps.heading_offset)) { ecefpositionvelocity.velocity_xyz[1] = gps.heading_offset; } - if (!isnan(gps.heading_accuracy)) { + if (!std::isnan(gps.heading_accuracy)) { ecefpositionvelocity.velocity_xyz[2] = gps.heading_accuracy; } diff --git a/src/lib/matrix/matrix/AxisAngle.hpp b/src/lib/matrix/matrix/AxisAngle.hpp index bc935ec20f..049e54d277 100644 --- a/src/lib/matrix/matrix/AxisAngle.hpp +++ b/src/lib/matrix/matrix/AxisAngle.hpp @@ -71,8 +71,8 @@ public: AxisAngle &v = *this; Type mag = q.imag().norm(); - if (fabs(mag) >= Type(1e-10)) { - v = q.imag() * Type(Type(2) * atan2(mag, q(0)) / mag); + if (std::fabs(mag) >= Type(1e-10)) { + v = q.imag() * Type(Type(2) * std::atan2(mag, q(0)) / mag); } else { v = q.imag() * Type(Type(2) * Type(sign(q(0)))); diff --git a/src/lib/matrix/matrix/Dcm.hpp b/src/lib/matrix/matrix/Dcm.hpp index 87d5e2f354..274728ceeb 100644 --- a/src/lib/matrix/matrix/Dcm.hpp +++ b/src/lib/matrix/matrix/Dcm.hpp @@ -123,12 +123,12 @@ public: Dcm(const Euler &euler) { Dcm &dcm = *this; - Type cosPhi = Type(cos(euler.phi())); - Type sinPhi = Type(sin(euler.phi())); - Type cosThe = Type(cos(euler.theta())); - Type sinThe = Type(sin(euler.theta())); - Type cosPsi = Type(cos(euler.psi())); - Type sinPsi = Type(sin(euler.psi())); + Type cosPhi = Type(std::cos(euler.phi())); + Type sinPhi = Type(std::sin(euler.phi())); + Type cosThe = Type(std::cos(euler.theta())); + Type sinThe = Type(std::sin(euler.theta())); + Type cosPsi = Type(std::cos(euler.psi())); + Type sinPsi = Type(std::sin(euler.psi())); dcm(0, 0) = cosThe * cosPsi; dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; diff --git a/src/lib/matrix/matrix/Dual.hpp b/src/lib/matrix/matrix/Dual.hpp index 284b45d012..26641e0f51 100644 --- a/src/lib/matrix/matrix/Dual.hpp +++ b/src/lib/matrix/matrix/Dual.hpp @@ -13,6 +13,8 @@ #pragma once +#include + #include "math.hpp" namespace matrix @@ -187,7 +189,7 @@ Dual operator/(Scalar a, const Dual &b) template Dual sqrt(const Dual &a) { - Scalar real = sqrt(a.value); + Scalar real = std::sqrt(a.value); return Dual(real, a.derivative * (Scalar(1) / (Scalar(2) * real))); } @@ -202,14 +204,14 @@ Dual abs(const Dual &a) template Dual ceil(const Dual &a) { - return Dual(ceil(a.value)); + return Dual(std::ceil(a.value)); } // floor template Dual floor(const Dual &a) { - return Dual(floor(a.value)); + return Dual(std::floor(a.value)); } // fmod @@ -237,7 +239,7 @@ Dual min(const Dual &a, const Dual &b) template bool IsNan(Scalar a) { - return isnan(a); + return std::isnan(a); } template @@ -250,7 +252,7 @@ bool IsNan(const Dual &a) template bool IsFinite(Scalar a) { - return isfinite(a); + return std::isfinite(a); } template @@ -263,7 +265,7 @@ bool IsFinite(const Dual &a) template bool IsInf(Scalar a) { - return isinf(a); + return std::isinf(a); } template @@ -278,21 +280,21 @@ bool IsInf(const Dual &a) template Dual sin(const Dual &a) { - return Dual(sin(a.value), cos(a.value) * a.derivative); + return Dual(std::sin(a.value), std::cos(a.value) * a.derivative); } // cos template Dual cos(const Dual &a) { - return Dual(cos(a.value), -sin(a.value) * a.derivative); + return Dual(std::cos(a.value), -std::sin(a.value) * a.derivative); } // tan template Dual tan(const Dual &a) { - Scalar real = tan(a.value); + Scalar real = std::tan(a.value); return Dual(real, (Scalar(1) + real * real) * a.derivative); } @@ -300,16 +302,16 @@ Dual tan(const Dual &a) template Dual asin(const Dual &a) { - Scalar asin_d = Scalar(1) / sqrt(Scalar(1) - a.value * a.value); - return Dual(asin(a.value), asin_d * a.derivative); + Scalar asin_d = Scalar(1) / std::sqrt(Scalar(1) - a.value * a.value); + return Dual(std::asin(a.value), asin_d * a.derivative); } // acos template Dual acos(const Dual &a) { - Scalar acos_d = -Scalar(1) / sqrt(Scalar(1) - a.value * a.value); - return Dual(acos(a.value), acos_d * a.derivative); + Scalar acos_d = -Scalar(1) / std::sqrt(Scalar(1) - a.value * a.value); + return Dual(std::acos(a.value), acos_d * a.derivative); } // atan @@ -317,7 +319,7 @@ template Dual atan(const Dual &a) { Scalar atan_d = Scalar(1) / (Scalar(1) + a.value * a.value); - return Dual(atan(a.value), atan_d * a.derivative); + return Dual(std::atan(a.value), atan_d * a.derivative); } // atan2 @@ -326,7 +328,7 @@ Dual atan2(const Dual &a, const Dual &b) { // derivative is equal to that of atan(a/b), so substitute a/b into atan and simplify Scalar atan_d = Scalar(1) / (a.value * a.value + b.value * b.value); - return Dual(atan2(a.value, b.value), (a.derivative * b.value - a.value * b.derivative) * atan_d); + return Dual(std::atan2(a.value, b.value), (a.derivative * b.value - a.value * b.derivative) * atan_d); } // retrieve the derivative elements of a vector of Duals into a matrix diff --git a/src/lib/matrix/matrix/Euler.hpp b/src/lib/matrix/matrix/Euler.hpp index b1fce355ec..cb977e690e 100644 --- a/src/lib/matrix/matrix/Euler.hpp +++ b/src/lib/matrix/matrix/Euler.hpp @@ -91,19 +91,19 @@ public: */ Euler(const Dcm &dcm) { - theta() = asin(-dcm(2, 0)); + theta() = std::asin(-dcm(2, 0)); - if ((fabs(theta() - Type(M_PI / 2))) < Type(1.0e-3)) { + if ((std::fabs(theta() - Type(M_PI / 2))) < Type(1.0e-3)) { phi() = 0; - psi() = atan2(dcm(1, 2), dcm(0, 2)); + psi() = std::atan2(dcm(1, 2), dcm(0, 2)); - } else if ((fabs(theta() + Type(M_PI / 2))) < Type(1.0e-3)) { + } else if ((std::fabs(theta() + Type(M_PI / 2))) < Type(1.0e-3)) { phi() = 0; - psi() = atan2(-dcm(1, 2), -dcm(0, 2)); + psi() = std::atan2(-dcm(1, 2), -dcm(0, 2)); } else { - phi() = atan2(dcm(2, 1), dcm(2, 2)); - psi() = atan2(dcm(1, 0), dcm(0, 0)); + phi() = std::atan2(dcm(2, 1), dcm(2, 2)); + psi() = std::atan2(dcm(1, 0), dcm(0, 0)); } } diff --git a/src/lib/matrix/matrix/LeastSquaresSolver.hpp b/src/lib/matrix/matrix/LeastSquaresSolver.hpp index 0756e05ee3..036b845ec4 100644 --- a/src/lib/matrix/matrix/LeastSquaresSolver.hpp +++ b/src/lib/matrix/matrix/LeastSquaresSolver.hpp @@ -47,7 +47,7 @@ public: normx += _A(i, j) * _A(i, j); } - normx = sqrt(normx); + normx = std::sqrt(normx); Type s = _A(j, j) > 0 ? Type(-1) : Type(1); Type u1 = _A(j, j) - s * normx; diff --git a/src/lib/matrix/matrix/Matrix.hpp b/src/lib/matrix/matrix/Matrix.hpp index 3f91b27a74..87134f7f09 100644 --- a/src/lib/matrix/matrix/Matrix.hpp +++ b/src/lib/matrix/matrix/Matrix.hpp @@ -8,6 +8,7 @@ #pragma once +#include #include #include @@ -539,7 +540,7 @@ public: for (size_t i = 0; i < M; i++) { for (size_t j = 0; j < N; j++) { - r(i, j) = Type(fabs((*this)(i, j))); + r(i, j) = Type(std::fabs((*this)(i, j))); } } @@ -587,7 +588,7 @@ public: for (size_t i = 0; i < M; i++) { for (size_t j = 0; j < N; j++) { - result = result && isnan(self(i, j)); + result = result && std::isnan(self(i, j)); } } @@ -645,8 +646,8 @@ namespace typeFunction template Type min(const Type x, const Type y) { - bool x_is_nan = isnan(x); - bool y_is_nan = isnan(y); + bool x_is_nan = std::isnan(x); + bool y_is_nan = std::isnan(y); // take the non-nan value if there is one if (x_is_nan || y_is_nan) { @@ -664,8 +665,8 @@ Type min(const Type x, const Type y) template Type max(const Type x, const Type y) { - bool x_is_nan = isnan(x); - bool y_is_nan = isnan(y); + bool x_is_nan = std::isnan(x); + bool y_is_nan = std::isnan(y); // take the non-nan value if there is one if (x_is_nan || y_is_nan) { @@ -686,7 +687,7 @@ Type constrain(const Type x, const Type lower_bound, const Type upper_bound) if (lower_bound > upper_bound) { return NAN; - } else if (isnan(x)) { + } else if (std::isnan(x)) { return NAN; } else { diff --git a/src/lib/matrix/matrix/PseudoInverse.hpp b/src/lib/matrix/matrix/PseudoInverse.hpp index e4a2e6548a..37a084b351 100644 --- a/src/lib/matrix/matrix/PseudoInverse.hpp +++ b/src/lib/matrix/matrix/PseudoInverse.hpp @@ -106,7 +106,7 @@ SquareMatrix fullRankCholesky(const SquareMatrix &A, } if (L(k, r) > tol) { - L(k, r) = sqrt(L(k, r)); + L(k, r) = std::sqrt(L(k, r)); if (k < N - 1) { for (size_t i = k + 1; i < N; i++) { diff --git a/src/lib/matrix/matrix/Quaternion.hpp b/src/lib/matrix/matrix/Quaternion.hpp index ccd65d7e3e..dd8d7b5cd9 100644 --- a/src/lib/matrix/matrix/Quaternion.hpp +++ b/src/lib/matrix/matrix/Quaternion.hpp @@ -102,7 +102,7 @@ public: Type t = R.trace(); if (t > Type(0)) { - t = sqrt(Type(1) + t); + t = std::sqrt(Type(1) + t); q(0) = Type(0.5) * t; t = Type(0.5) / t; q(1) = (R(2, 1) - R(1, 2)) * t; @@ -110,7 +110,7 @@ public: q(3) = (R(1, 0) - R(0, 1)) * t; } else if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) { - t = sqrt(Type(1) + R(0, 0) - R(1, 1) - R(2, 2)); + t = std::sqrt(Type(1) + R(0, 0) - R(1, 1) - R(2, 2)); q(1) = Type(0.5) * t; t = Type(0.5) / t; q(0) = (R(2, 1) - R(1, 2)) * t; @@ -118,7 +118,7 @@ public: q(3) = (R(0, 2) + R(2, 0)) * t; } else if (R(1, 1) > R(2, 2)) { - t = sqrt(Type(1) - R(0, 0) + R(1, 1) - R(2, 2)); + t = std::sqrt(Type(1) - R(0, 0) + R(1, 1) - R(2, 2)); q(2) = Type(0.5) * t; t = Type(0.5) / t; q(0) = (R(0, 2) - R(2, 0)) * t; @@ -126,7 +126,7 @@ public: q(3) = (R(2, 1) + R(1, 2)) * t; } else { - t = sqrt(Type(1) - R(0, 0) - R(1, 1) + R(2, 2)); + t = std::sqrt(Type(1) - R(0, 0) - R(1, 1) + R(2, 2)); q(3) = Type(0.5) * t; t = Type(0.5) / t; q(0) = (R(1, 0) - R(0, 1)) * t; @@ -147,12 +147,12 @@ public: Quaternion(const Euler &euler) { Quaternion &q = *this; - Type cosPhi_2 = Type(cos(euler.phi() / Type(2))); - Type cosTheta_2 = Type(cos(euler.theta() / Type(2))); - Type cosPsi_2 = Type(cos(euler.psi() / Type(2))); - Type sinPhi_2 = Type(sin(euler.phi() / Type(2))); - Type sinTheta_2 = Type(sin(euler.theta() / Type(2))); - Type sinPsi_2 = Type(sin(euler.psi() / Type(2))); + Type cosPhi_2 = Type(std::cos(euler.phi() / Type(2))); + Type cosTheta_2 = Type(std::cos(euler.theta() / Type(2))); + Type cosPsi_2 = Type(std::cos(euler.psi() / Type(2))); + Type sinPhi_2 = Type(std::sin(euler.phi() / Type(2))); + Type sinTheta_2 = Type(std::sin(euler.theta() / Type(2))); + Type sinPsi_2 = Type(std::sin(euler.psi() / Type(2))); q(0) = cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2; q(1) = sinPhi_2 * cosTheta_2 * cosPsi_2 - @@ -179,8 +179,8 @@ public: q(1) = q(2) = q(3) = 0; } else { - Type magnitude = sin(angle / Type(2)); - q(0) = cos(angle / Type(2)); + Type magnitude = std::sin(angle / Type(2)); + q(0) = std::cos(angle / Type(2)); q(1) = axis(0) * magnitude; q(2) = axis(1) * magnitude; q(3) = axis(2) * magnitude; @@ -229,7 +229,7 @@ public: } else { // normal case, do half-way quaternion solution - q(0) = dt + sqrt(src.norm_squared() * dst.norm_squared()); + q(0) = dt + std::sqrt(src.norm_squared() * dst.norm_squared()); } q(1) = cr(0); @@ -382,8 +382,8 @@ public: cos_u = Type(1.0) - u2 * c2 + u4 * c4 - u6 * c6; } else { - sinc_u = Type(sin(u_norm) / u_norm); - cos_u = Type(cos(u_norm)); + sinc_u = Type(std::sin(u_norm) / u_norm); + cos_u = Type(std::cos(u_norm)); } Vector v = sinc_u * u; @@ -410,7 +410,7 @@ public: return Type(0.5) * (Dcm() + u_hat + (Type(1.0 / 3.0) + u_norm * u_norm / Type(45.0)) * u_hat * u_hat); } else { - return Type(0.5) * (Dcm() + u_hat + (Type(1.0) - u_norm * Type(cos(u_norm) / sin(u_norm))) / + return Type(0.5) * (Dcm() + u_hat + (Type(1.0) - u_norm * Type(std::cos(u_norm) / std::sin(u_norm))) / (u_norm * u_norm) * u_hat * u_hat); } } @@ -457,7 +457,7 @@ public: const Quaternion &q = *this; for (size_t i = 0; i < 4; i++) { - if (fabs(q(i)) > FLT_EPSILON) { + if (std::fabs(q(i)) > FLT_EPSILON) { return q * Type(matrix::sign(q(i))); } } diff --git a/src/lib/matrix/matrix/Slice.hpp b/src/lib/matrix/matrix/Slice.hpp index f730c548f9..207d21fdd9 100644 --- a/src/lib/matrix/matrix/Slice.hpp +++ b/src/lib/matrix/matrix/Slice.hpp @@ -285,7 +285,7 @@ public: Type norm() const { - return matrix::sqrt(norm_squared()); + return std::sqrt(norm_squared()); } bool longerThan(Type testVal) const diff --git a/src/lib/matrix/matrix/SparseVector.hpp b/src/lib/matrix/matrix/SparseVector.hpp index 4af242516e..40e790dfbf 100644 --- a/src/lib/matrix/matrix/SparseVector.hpp +++ b/src/lib/matrix/matrix/SparseVector.hpp @@ -176,7 +176,7 @@ public: Type norm() const { - return matrix::sqrt(norm_squared()); + return std::sqrt(norm_squared()); } bool longerThan(Type testVal) const diff --git a/src/lib/matrix/matrix/SquareMatrix.hpp b/src/lib/matrix/matrix/SquareMatrix.hpp index fd6754a1de..20ecd67f2c 100644 --- a/src/lib/matrix/matrix/SquareMatrix.hpp +++ b/src/lib/matrix/matrix/SquareMatrix.hpp @@ -8,6 +8,8 @@ #pragma once +#include // FLT_EPSILON + #include "math.hpp" namespace matrix @@ -339,12 +341,12 @@ bool inv(const SquareMatrix &A, SquareMatrix &inv, size_t rank for (size_t n = 0; n < rank; n++) { // if diagonal is zero, swap with row below - if (fabs(U(n, n)) < Type(FLT_EPSILON)) { + if (std::fabs(U(n, n)) < Type(FLT_EPSILON)) { //printf("trying pivot for row %d\n",n); for (size_t i = n + 1; i < rank; i++) { //printf("\ttrying row %d\n",i); - if (fabs(U(i, n)) > Type(FLT_EPSILON)) { + if (std::fabs(U(i, n)) > Type(FLT_EPSILON)) { //printf("swapped %d\n",i); U.swapRows(i, n); P.swapRows(i, n); @@ -364,7 +366,7 @@ bool inv(const SquareMatrix &A, SquareMatrix &inv, size_t rank #endif // failsafe, return zero matrix - if (fabs(static_cast(U(n, n))) < FLT_EPSILON) { + if (std::fabs(static_cast(U(n, n))) < FLT_EPSILON) { return false; } @@ -438,7 +440,7 @@ bool inv(const SquareMatrix &A, SquareMatrix &inv, size_t rank //check sanity of results for (size_t i = 0; i < rank; i++) { for (size_t j = 0; j < rank; j++) { - if (!is_finite(P(i, j))) { + if (!std::isfinite(P(i, j))) { return false; } } @@ -454,7 +456,7 @@ bool inv(const SquareMatrix &A, SquareMatrix &inv) { Type det = A(0, 0) * A(1, 1) - A(1, 0) * A(0, 1); - if (fabs(static_cast(det)) < FLT_EPSILON || !is_finite(det)) { + if (std::fabs(static_cast(det)) < FLT_EPSILON || !std::isfinite(det)) { return false; } @@ -473,7 +475,7 @@ bool inv(const SquareMatrix &A, SquareMatrix &inv) A(0, 1) * (A(1, 0) * A(2, 2) - A(1, 2) * A(2, 0)) + A(0, 2) * (A(1, 0) * A(2, 1) - A(1, 1) * A(2, 0)); - if (fabs(static_cast(det)) < FLT_EPSILON || !is_finite(det)) { + if (std::fabs(static_cast(det)) < FLT_EPSILON || !std::isfinite(det)) { return false; } @@ -532,7 +534,7 @@ SquareMatrix cholesky(const SquareMatrix &A) L(j, j) = 0; } else { - L(j, j) = sqrt(res); + L(j, j) = std::sqrt(res); } } else { diff --git a/src/lib/matrix/matrix/Vector.hpp b/src/lib/matrix/matrix/Vector.hpp index 336e31cef8..892be7ce0b 100644 --- a/src/lib/matrix/matrix/Vector.hpp +++ b/src/lib/matrix/matrix/Vector.hpp @@ -92,7 +92,7 @@ public: Type norm() const { const Vector &a(*this); - return Type(matrix::sqrt(a.dot(a))); + return Type(std::sqrt(a.dot(a))); } Type norm_squared() const @@ -143,7 +143,7 @@ public: Vector r; for (size_t i = 0; i < M; i++) { - r(i) = Type(matrix::sqrt(a(i))); + r(i) = Type(std::sqrt(a(i))); } return r; diff --git a/src/lib/matrix/matrix/helper_functions.hpp b/src/lib/matrix/matrix/helper_functions.hpp index e09215a19e..8af22ac53a 100644 --- a/src/lib/matrix/matrix/helper_functions.hpp +++ b/src/lib/matrix/matrix/helper_functions.hpp @@ -1,26 +1,10 @@ #pragma once -#include "math.hpp" - -#if defined (__PX4_NUTTX) || defined (__PX4_QURT) -#include -#endif +#include namespace matrix { -template -bool is_finite(Type x) -{ -#if defined (__PX4_NUTTX) - return PX4_ISFINITE(x); -#elif defined (__PX4_QURT) - return __builtin_isfinite(x); -#else - return std::isfinite(x); -#endif -} - /** * Compare if two floating point numbers are equal * @@ -35,9 +19,9 @@ bool is_finite(Type x) template bool isEqualF(const Type x, const Type y, const Type eps = Type(1e-4f)) { - return (matrix::fabs(x - y) <= eps) - || (isnan(x) && isnan(y)) - || (isinf(x) && isinf(y) && isnan(x - y)); + return (std::fabs(x - y) <= eps) + || (std::isnan(x) && std::isnan(y)) + || (std::isinf(x) && std::isinf(y) && std::isnan(x - y)); } namespace detail @@ -53,7 +37,7 @@ Floating wrap_floating(Floating x, Floating low, Floating high) const auto range = high - low; const auto inv_range = Floating(1) / range; // should evaluate at compile time, multiplies below at runtime - const auto num_wraps = floor((x - low) * inv_range); + const auto num_wraps = std::floor((x - low) * inv_range); return x - range * num_wraps; } @@ -120,7 +104,7 @@ Type wrap_pi(Type x) template Type wrap_2pi(Type x) { - return wrap(x, Type(0), Type(M_TWOPI)); + return wrap(x, Type(0), Type((2 * M_PI))); } /** diff --git a/src/lib/matrix/matrix/math.hpp b/src/lib/matrix/matrix/math.hpp index cc4e5e81d7..24a43723bf 100644 --- a/src/lib/matrix/matrix/math.hpp +++ b/src/lib/matrix/matrix/math.hpp @@ -1,11 +1,9 @@ #pragma once #include -#include "stdlib_imports.hpp" -#ifdef __PX4_QURT -#include "dspal_math.h" -#endif + #include "helper_functions.hpp" + #include "Matrix.hpp" #include "SquareMatrix.hpp" #include "Slice.hpp" diff --git a/src/lib/matrix/matrix/stdlib_imports.hpp b/src/lib/matrix/matrix/stdlib_imports.hpp deleted file mode 100644 index 94b9c79689..0000000000 --- a/src/lib/matrix/matrix/stdlib_imports.hpp +++ /dev/null @@ -1,140 +0,0 @@ -/** - * @file stdlib_imports.hpp - * - * This file is needed to shadow the C standard library math functions with ones provided by the C++ standard library. - * This way we can guarantee that unwanted functions from the C library will never creep back in unexpectedly. - * - * @author Pavel Kirienko - */ - -#pragma once - -#include -#include -#include - -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif -#ifndef M_TWOPI -#define M_TWOPI (M_PI * 2.0) -#endif - -namespace matrix -{ - -#if !defined(FLT_EPSILON) -#define FLT_EPSILON __FLT_EPSILON__ -#endif - -#if defined(__PX4_NUTTX) -/* - * NuttX has no usable C++ math library, so we need to provide the needed definitions here manually. - */ -#define MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(name) \ - inline float name(float x) { return ::name##f(x); } \ - inline double name(double x) { return ::name(x); } \ - inline long double name(long double x) { return ::name##l(x); } - -#define MATRIX_NUTTX_WRAP_MATH_FUN_BINARY(name) \ - inline float name(float x, float y) { return ::name##f(x, y); } \ - inline double name(double x, double y) { return ::name(x, y); } \ - inline long double name(long double x, long double y) { return ::name##l(x, y); } - -MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(fabs) -MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(log) -MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(log10) -MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(exp) -MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(sqrt) -MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(sin) -MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(cos) -MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(tan) -MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(asin) -MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(acos) -MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(atan) -MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(sinh) -MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(cosh) -MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(tanh) -MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(ceil) -MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(floor) - -MATRIX_NUTTX_WRAP_MATH_FUN_BINARY(pow) -MATRIX_NUTTX_WRAP_MATH_FUN_BINARY(atan2) - -#else // Not NuttX, using the C++ standard library - -using std::abs; -using std::div; -using std::fabs; -using std::fmod; -using std::exp; -using std::log; -using std::log10; -using std::pow; -using std::sqrt; -using std::sin; -using std::cos; -using std::tan; -using std::asin; -using std::acos; -using std::atan; -using std::atan2; -using std::sinh; -using std::cosh; -using std::tanh; -using std::ceil; -using std::floor; -using std::frexp; -using std::ldexp; -using std::modf; - -# if (__cplusplus >= 201103L) - -using std::remainder; -using std::remquo; -using std::fma; -using std::fmax; -using std::fmin; -using std::fdim; -using std::nan; -using std::nanf; -using std::nanl; -using std::exp2; -using std::expm1; -using std::log2; -using std::log1p; -using std::cbrt; -using std::hypot; -using std::asinh; -using std::acosh; -using std::atanh; -using std::erf; -using std::erfc; -using std::tgamma; -using std::lgamma; -using std::trunc; -using std::round; -using std::nearbyint; -using std::rint; -using std::scalbn; -using std::ilogb; -using std::logb; -using std::nextafter; -using std::copysign; -using std::fpclassify; -using std::isfinite; -using std::isinf; -using std::isnan; -using std::isnormal; -using std::signbit; -using std::isgreater; -using std::isgreaterequal; -using std::isless; -using std::islessequal; -using std::islessgreater; -using std::isunordered; - -# endif -#endif - -} diff --git a/src/lib/matrix/test/MatrixAssignmentTest.cpp b/src/lib/matrix/test/MatrixAssignmentTest.cpp index 2738f9325d..04992b5333 100644 --- a/src/lib/matrix/test/MatrixAssignmentTest.cpp +++ b/src/lib/matrix/test/MatrixAssignmentTest.cpp @@ -31,6 +31,8 @@ * ****************************************************************************/ +#include + #include #include @@ -65,7 +67,7 @@ TEST(MatrixAssignmentTest, Assignment) for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { - EXPECT_TRUE(isnan(m_nan(i, j))); + EXPECT_TRUE(std::isnan(m_nan(i, j))); } } diff --git a/src/lib/matrix/test/MatrixAttitudeTest.cpp b/src/lib/matrix/test/MatrixAttitudeTest.cpp index 5e51f1ca77..40e6dea4d2 100644 --- a/src/lib/matrix/test/MatrixAttitudeTest.cpp +++ b/src/lib/matrix/test/MatrixAttitudeTest.cpp @@ -379,14 +379,14 @@ TEST(MatrixAttitudeTest, Attitude) Vector3f rot(1.f, 0.f, 0.f); Quatf q_test; q_test.rotate(rot); - Quatf q_true(cos(1.0f / 2), sin(1.0f / 2), 0.0f, 0.0f); + Quatf q_true(std::cos(1.0f / 2), sin(1.0f / 2), 0.0f, 0.0f); EXPECT_EQ(q_test, q_true); // rotate quaternion (zero rotation) rot(0) = rot(1) = rot(2) = 0.0f; q_test = Quatf(); q_test.rotate(rot); - q_true = Quatf(cos(0.0f), sin(0.0f), 0.0f, 0.0f); + q_true = Quatf(std::cos(0.0f), sin(0.0f), 0.0f, 0.0f); EXPECT_EQ(q_test, q_true); // rotate quaternion (random non-commutating rotation) @@ -397,7 +397,7 @@ TEST(MatrixAttitudeTest, Attitude) EXPECT_EQ(q, q_true); // get rotation axis from quaternion (nonzero rotation) - q = Quatf(cos(1.0f / 2), 0.0f, sin(1.0f / 2), 0.0f); + q = Quatf(std::cos(1.0f / 2), 0.0f, sin(1.0f / 2), 0.0f); rot = AxisAnglef(q); EXPECT_FLOAT_EQ(rot(0), 0.0f); EXPECT_FLOAT_EQ(rot(1), 1.0f); @@ -417,7 +417,7 @@ TEST(MatrixAttitudeTest, Attitude) EXPECT_EQ(q, q_true); // from axis angle, with length of vector the rotation - float n = float(sqrt(4 * M_PI * M_PI / 3)); + float n = float(std::sqrt(4 * M_PI * M_PI / 3)); q = AxisAnglef(n, n, n); EXPECT_EQ(q, Quatf(-1, 0, 0, 0)); q = AxisAnglef(0, 0, 0); diff --git a/src/lib/matrix/test/MatrixDualTest.cpp b/src/lib/matrix/test/MatrixDualTest.cpp index 589d1effdb..6a34a97c8c 100644 --- a/src/lib/matrix/test/MatrixDualTest.cpp +++ b/src/lib/matrix/test/MatrixDualTest.cpp @@ -31,6 +31,8 @@ * ****************************************************************************/ +#include + #include #include #include @@ -162,7 +164,7 @@ TEST(MatrixDualTest, Dual) { // sqrt - EXPECT_FLOAT_EQ(sqrt(a).value, sqrt(a.value)); + EXPECT_FLOAT_EQ(sqrt(a).value, std::sqrt(a.value)); EXPECT_FLOAT_EQ(sqrt(a).derivative(0), 1.f / sqrt(12.f)); } @@ -202,7 +204,7 @@ TEST(MatrixDualTest, Dual) { // isnan EXPECT_FALSE(IsNan(a)); - Dual c(sqrt(-1.f), 0); + Dual c(std::sqrt(-1.f), 0); EXPECT_TRUE(IsNan(c)); } @@ -210,7 +212,7 @@ TEST(MatrixDualTest, Dual) // isfinite/isinf EXPECT_TRUE(IsFinite(a)); EXPECT_FALSE(IsInf(a)); - Dual c(sqrt(-1.f), 0); + Dual c(std::sqrt(-1.f), 0); EXPECT_FALSE(IsFinite(c)); EXPECT_FALSE(IsInf(c)); Dual d(INFINITY, 0); @@ -221,25 +223,25 @@ TEST(MatrixDualTest, Dual) { // sin/cos/tan EXPECT_FLOAT_EQ(sin(a).value, sin(a.value)); - EXPECT_FLOAT_EQ(sin(a).derivative(0), cos(a.value)); // sin'(x) = cos(x) + EXPECT_FLOAT_EQ(sin(a).derivative(0), std::cos(a.value)); // sin'(x) = cos(x) EXPECT_FLOAT_EQ(cos(a).value, cos(a.value)); - EXPECT_FLOAT_EQ(cos(a).derivative(0), -sin(a.value)); // cos'(x) = -sin(x) + EXPECT_FLOAT_EQ(cos(a).derivative(0), -std::sin(a.value)); // cos'(x) = -sin(x) EXPECT_FLOAT_EQ(tan(a).value, tan(a.value)); - EXPECT_FLOAT_EQ(tan(a).derivative(0), 1.f + tan(a.value)*tan(a.value)); // tan'(x) = 1 + tan^2(x) + EXPECT_FLOAT_EQ(tan(a).derivative(0), 1.f + std::tan(a.value)*std::tan(a.value)); // tan'(x) = 1 + tan^2(x) } { // asin/acos/atan Dual c(0.3f, 0); - EXPECT_FLOAT_EQ(asin(c).value, asin(c.value)); - EXPECT_FLOAT_EQ(asin(c).derivative(0), 1.f / sqrt(1.f - 0.3f * 0.3f)); // asin'(x) = 1/sqrt(1-x^2) + EXPECT_FLOAT_EQ(asin(c).value, std::asin(c.value)); + EXPECT_FLOAT_EQ(asin(c).derivative(0), 1.f / std::sqrt(1.f - 0.3f * 0.3f)); // asin'(x) = 1/sqrt(1-x^2) - EXPECT_FLOAT_EQ(acos(c).value, acos(c.value)); - EXPECT_FLOAT_EQ(acos(c).derivative(0), -1.f / sqrt(1.f - 0.3f * 0.3f)); // acos'(x) = -1/sqrt(1-x^2) + EXPECT_FLOAT_EQ(acos(c).value, std::acos(c.value)); + EXPECT_FLOAT_EQ(acos(c).derivative(0), -1.f / std::sqrt(1.f - 0.3f * 0.3f)); // acos'(x) = -1/sqrt(1-x^2) - EXPECT_FLOAT_EQ(atan(c).value, atan(c.value)); + EXPECT_FLOAT_EQ(atan(c).value, std::atan(c.value)); EXPECT_FLOAT_EQ(atan(c).derivative(0), 1.f / (1.f + 0.3f * 0.3f)); // tan'(x) = 1 + x^2 } diff --git a/src/lib/matrix/test/MatrixHelperTest.cpp b/src/lib/matrix/test/MatrixHelperTest.cpp index ec3062ff2f..6133f497df 100644 --- a/src/lib/matrix/test/MatrixHelperTest.cpp +++ b/src/lib/matrix/test/MatrixHelperTest.cpp @@ -81,21 +81,21 @@ TEST(MatrixHelperTest, Helper) // wrap pi EXPECT_FLOAT_EQ(wrap_pi(0.), 0.); - EXPECT_FLOAT_EQ(wrap_pi(4.), (4. - M_TWOPI)); - EXPECT_FLOAT_EQ(wrap_pi(-4.), (-4. + M_TWOPI)); + EXPECT_FLOAT_EQ(wrap_pi(4.), (4. - (2 * M_PI))); + EXPECT_FLOAT_EQ(wrap_pi(-4.), (-4. + (2 * M_PI))); EXPECT_FLOAT_EQ(wrap_pi(3.), 3.); EXPECT_FLOAT_EQ(wrap_pi(100.), (100. - 32. * M_PI)); EXPECT_FLOAT_EQ(wrap_pi(-100.), (-100. + 32. * M_PI)); EXPECT_FLOAT_EQ(wrap_pi(-101.), (-101. + 32. * M_PI)); - EXPECT_FALSE(is_finite(wrap_pi(NAN))); + EXPECT_FALSE(std::isfinite(wrap_pi(NAN))); // wrap 2pi EXPECT_FLOAT_EQ(wrap_2pi(0.), 0.); EXPECT_FLOAT_EQ(wrap_2pi(-4.), (-4. + 2. * M_PI)); EXPECT_FLOAT_EQ(wrap_2pi(3.), (3.)); - EXPECT_FLOAT_EQ(wrap_2pi(200.), (200. - 31. * M_TWOPI)); - EXPECT_FLOAT_EQ(wrap_2pi(-201.), (-201. + 32. * M_TWOPI)); - EXPECT_FALSE(is_finite(wrap_2pi(NAN))); + EXPECT_FLOAT_EQ(wrap_2pi(200.), (200. - 31. * (2 * M_PI))); + EXPECT_FLOAT_EQ(wrap_2pi(-201.), (-201. + 32. * (2 * M_PI))); + EXPECT_FALSE(std::isfinite(wrap_2pi(NAN))); // Equality checks EXPECT_TRUE(isEqualF(1., 1.)); diff --git a/src/lib/matrix/test/MatrixIntegralTest.cpp b/src/lib/matrix/test/MatrixIntegralTest.cpp index 5a7528dc85..b9624077b4 100644 --- a/src/lib/matrix/test/MatrixIntegralTest.cpp +++ b/src/lib/matrix/test/MatrixIntegralTest.cpp @@ -50,6 +50,6 @@ TEST(MatrixIntegralTest, Integral) float tf = 2; float h = 0.001f; integrate_rk4(f, y, u, t0, tf, h, y); - float v = 1 + cos(tf) - cos(t0); + float v = 1 + std::cos(tf) - std::cos(t0); EXPECT_EQ(y, (ones()*v)); } diff --git a/src/lib/matrix/test/test_data.py b/src/lib/matrix/test/test_data.py index f7d7cd6539..43bc03e324 100644 --- a/src/lib/matrix/test/test_data.py +++ b/src/lib/matrix/test/test_data.py @@ -47,7 +47,7 @@ def quat_prod(q, r): def dcm_to_euler(dcm): return array([ arctan(dcm[2,1]/ dcm[2,2]), - arctan(-dcm[2,0]/ sqrt(1 - dcm[2,0]**2)), + arctan(-dcm[2,0]/ std::sqrt(1 - dcm[2,0]**2)), arctan(dcm[1,0]/ dcm[0,0]), ]) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index 919a2be778..3a5793e582 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -41,6 +41,8 @@ #pragma once +#include + #include enum class AllocationMethod { diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 4c23a9c51d..2baf048d12 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -42,6 +42,8 @@ #ifndef EKF_COMMON_H #define EKF_COMMON_H +#include + #include namespace estimator diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 0a09c390f0..f31bf55ea8 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -784,7 +784,7 @@ bool FlightTaskAuto::isTargetModified() const { const bool xy_modified = (_target - _position_setpoint).xy().longerThan(FLT_EPSILON); const bool z_valid = PX4_ISFINITE(_position_setpoint(2)); - const bool z_modified = z_valid && fabs((_target - _position_setpoint)(2)) > FLT_EPSILON; + const bool z_modified = z_valid && std::fabs((_target - _position_setpoint)(2)) > FLT_EPSILON; return xy_modified || z_modified; } diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp index 859b86316f..bd415aae24 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp @@ -97,7 +97,7 @@ matrix::Vector3f AttitudeControl::update(const Quatf &q) const // and multiply it by the yaw setpoint rate (yawspeed_setpoint). // This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame // such that it can be added to the rates setpoint. - if (is_finite(_yawspeed_setpoint)) { + if (std::isfinite(_yawspeed_setpoint)) { rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint; } diff --git a/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp b/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp index 0b95451083..e9b6fba12f 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp @@ -31,6 +31,8 @@ * ****************************************************************************/ +#include + #include #include #include @@ -249,7 +251,7 @@ TEST(ControlMathTest, addIfNotNan) v = NAN; // both summands are NAN ControlMath::addIfNotNan(v, NAN); - EXPECT_TRUE(isnan(v)); + EXPECT_TRUE(std::isnan(v)); // regular value gets added to NAN and overwrites it ControlMath::addIfNotNan(v, 3.f); EXPECT_EQ(v, 3.f); diff --git a/src/systemcmds/tests/test_matrix.cpp b/src/systemcmds/tests/test_matrix.cpp index b7bc519040..545ec46201 100644 --- a/src/systemcmds/tests/test_matrix.cpp +++ b/src/systemcmds/tests/test_matrix.cpp @@ -98,8 +98,6 @@ bool MatrixTest::run_tests() ut_declare_test_c(test_matrix, MatrixTest) -using std::fabs; - bool MatrixTest::attitudeTests() { float eps = 1e-6; @@ -135,10 +133,10 @@ bool MatrixTest::attitudeTests() // quaternion ctor Quatf q0(1, 2, 3, 4); Quatf q(q0); - ut_test(fabs(q(0) - 1) < eps); - ut_test(fabs(q(1) - 2) < eps); - ut_test(fabs(q(2) - 3) < eps); - ut_test(fabs(q(3) - 4) < eps); + ut_test(std::fabs(q(0) - 1) < eps); + ut_test(std::fabs(q(1) - 2) < eps); + ut_test(std::fabs(q(2) - 3) < eps); + ut_test(std::fabs(q(3) - 4) < eps); // quat normalization q.normalize(); @@ -268,17 +266,17 @@ bool MatrixTest::attitudeTests() // quaternion inverse q = q_check.inversed(); - ut_test(fabs(q_check(0) - q(0)) < eps); - ut_test(fabs(q_check(1) + q(1)) < eps); - ut_test(fabs(q_check(2) + q(2)) < eps); - ut_test(fabs(q_check(3) + q(3)) < eps); + ut_test(std::fabs(q_check(0) - q(0)) < eps); + ut_test(std::fabs(q_check(1) + q(1)) < eps); + ut_test(std::fabs(q_check(2) + q(2)) < eps); + ut_test(std::fabs(q_check(3) + q(3)) < eps); q = q_check; q.invert(); - ut_test(fabs(q_check(0) - q(0)) < eps); - ut_test(fabs(q_check(1) + q(1)) < eps); - ut_test(fabs(q_check(2) + q(2)) < eps); - ut_test(fabs(q_check(3) + q(3)) < eps); + ut_test(std::fabs(q_check(0) - q(0)) < eps); + ut_test(std::fabs(q_check(1) + q(1)) < eps); + ut_test(std::fabs(q_check(2) + q(2)) < eps); + ut_test(std::fabs(q_check(3) + q(3)) < eps); // rotate quaternion (nonzero rotation) Quatf qI(1.0f, 0.0f, 0.0f, 0.0f); @@ -287,10 +285,10 @@ bool MatrixTest::attitudeTests() rot(1) = rot(2) = 0.0f; qI.rotate(rot); Quatf q_true(cosf(1.0f / 2), sinf(1.0f / 2), 0.0f, 0.0f); - ut_test(fabs(qI(0) - q_true(0)) < eps); - ut_test(fabs(qI(1) - q_true(1)) < eps); - ut_test(fabs(qI(2) - q_true(2)) < eps); - ut_test(fabs(qI(3) - q_true(3)) < eps); + ut_test(std::fabs(qI(0) - q_true(0)) < eps); + ut_test(std::fabs(qI(1) - q_true(1)) < eps); + ut_test(std::fabs(qI(2) - q_true(2)) < eps); + ut_test(std::fabs(qI(3) - q_true(3)) < eps); // rotate quaternion (zero rotation) qI = Quatf(1.0f, 0.0f, 0.0f, 0.0f); @@ -298,33 +296,33 @@ bool MatrixTest::attitudeTests() rot(1) = rot(2) = 0.0f; qI.rotate(rot); q_true = Quatf(cosf(0.0f), sinf(0.0f), 0.0f, 0.0f); - ut_test(fabs(qI(0) - q_true(0)) < eps); - ut_test(fabs(qI(1) - q_true(1)) < eps); - ut_test(fabs(qI(2) - q_true(2)) < eps); - ut_test(fabs(qI(3) - q_true(3)) < eps); + ut_test(std::fabs(qI(0) - q_true(0)) < eps); + ut_test(std::fabs(qI(1) - q_true(1)) < eps); + ut_test(std::fabs(qI(2) - q_true(2)) < eps); + ut_test(std::fabs(qI(3) - q_true(3)) < eps); // get rotation axis from quaternion (nonzero rotation) q = Quatf(cosf(1.0f / 2), 0.0f, sinf(1.0f / 2), 0.0f); rot = matrix::AxisAngle(q); - ut_test(fabs(rot(0)) < eps); - ut_test(fabs(rot(1) - 1.0f) < eps); - ut_test(fabs(rot(2)) < eps); + ut_test(std::fabs(rot(0)) < eps); + ut_test(std::fabs(rot(1) - 1.0f) < eps); + ut_test(std::fabs(rot(2)) < eps); // get rotation axis from quaternion (zero rotation) q = Quatf(1.0f, 0.0f, 0.0f, 0.0f); rot = matrix::AxisAngle(q); - ut_test(fabs(rot(0)) < eps); - ut_test(fabs(rot(1)) < eps); - ut_test(fabs(rot(2)) < eps); + ut_test(std::fabs(rot(0)) < eps); + ut_test(std::fabs(rot(1)) < eps); + ut_test(std::fabs(rot(2)) < eps); // from axis angle (zero rotation) rot(0) = rot(1) = rot(2) = 0.0f; q = Quaternion(matrix::AxisAngle(rot)); q_true = Quatf(1.0f, 0.0f, 0.0f, 0.0f); - ut_test(fabs(q(0) - q_true(0)) < eps); - ut_test(fabs(q(1) - q_true(1)) < eps); - ut_test(fabs(q(2) - q_true(2)) < eps); - ut_test(fabs(q(3) - q_true(3)) < eps); + ut_test(std::fabs(q(0) - q_true(0)) < eps); + ut_test(std::fabs(q(1) - q_true(1)) < eps); + ut_test(std::fabs(q(2) - q_true(2)) < eps); + ut_test(std::fabs(q(3) - q_true(3)) < eps); return true; } @@ -435,7 +433,7 @@ bool MatrixTest::matrixAssignmentTests() for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { - ut_test(fabs(data[i * 3 + j] - m2(i, j)) < eps); + ut_test(std::fabs(data[i * 3 + j] - m2(i, j)) < eps); } } @@ -481,13 +479,13 @@ bool MatrixTest::matrixAssignmentTests() m4.swapCols(0, 2); m4.swapRows(0, 2); ut_test(isEqual(m4, Matrix3f(data_row_02_swap))); - ut_test(fabs(m4.min() - 1) < 1e-5); + ut_test(std::fabs(m4.min() - 1) < 1e-5); Scalar s = 1; - ut_test(fabs(s - 1) < 1e-5); + ut_test(std::fabs(s - 1) < 1e-5); Matrix m5 = s; - ut_test(fabs(m5(0, 0) - s) < 1e-5); + ut_test(std::fabs(m5(0, 0) - s) < 1e-5); Matrix m6; m6.row(0) = Vector2f(1, 1); @@ -543,10 +541,10 @@ bool MatrixTest::setIdentityTests() for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { if (i == j) { - ut_test(fabs(A(i, j) - 1) < 1e-7); + ut_test(std::fabs(A(i, j) - 1) < 1e-7); } else { - ut_test(fabs(A(i, j) - 0) < 1e-7); + ut_test(std::fabs(A(i, j) - 0) < 1e-7); } } } @@ -633,9 +631,9 @@ bool MatrixTest::vectorTests() float data1[] = {1, 2, 3, 4, 5}; float data2[] = {6, 7, 8, 9, 10}; Vector v1(data1); - ut_test(fabs(v1.norm() - 7.416198487095663f) < 1e-5); + ut_test(std::fabs(v1.norm() - 7.416198487095663f) < 1e-5); Vector v2(data2); - ut_test(fabs(v1.dot(v2) - 130.0f) < 1e-5); + ut_test(std::fabs(v1.dot(v2) - 130.0f) < 1e-5); v2.normalize(); Vector v3(v2); ut_test(isEqual(v2, v3)); @@ -650,26 +648,26 @@ bool MatrixTest::vector2Tests() { Vector2f a(1, 0); Vector2f b(0, 1); - ut_test(fabs(a % b - 1.0f) < 1e-5); + ut_test(std::fabs(a % b - 1.0f) < 1e-5); Vector2f c; - ut_test(fabs(c(0) - 0) < 1e-5); - ut_test(fabs(c(1) - 0) < 1e-5); + ut_test(std::fabs(c(0) - 0) < 1e-5); + ut_test(std::fabs(c(1) - 0) < 1e-5); static Matrix d(a); // the static keywork is a workaround for an internal bug of GCC // "internal compiler error: in trunc_int_for_mode, at explow.c:55" - ut_test(fabs(d(0, 0) - 1) < 1e-5); - ut_test(fabs(d(1, 0) - 0) < 1e-5); + ut_test(std::fabs(d(0, 0) - 1) < 1e-5); + ut_test(std::fabs(d(1, 0) - 0) < 1e-5); Vector2f e(d); - ut_test(fabs(e(0) - 1) < 1e-5); - ut_test(fabs(e(1) - 0) < 1e-5); + ut_test(std::fabs(e(0) - 1) < 1e-5); + ut_test(std::fabs(e(1) - 0) < 1e-5); float data[] = {4, 5}; Vector2f f(data); - ut_test(fabs(f(0) - 4) < 1e-5); - ut_test(fabs(f(1) - 5) < 1e-5); + ut_test(std::fabs(f(0) - 4) < 1e-5); + ut_test(std::fabs(f(1) - 5) < 1e-5); return true; } @@ -699,20 +697,20 @@ bool MatrixTest::vectorAssignmentTests() static const float eps = 1e-7f; - ut_test(fabsf(v(0) - 1) < eps); - ut_test(fabsf(v(1) - 2) < eps); - ut_test(fabsf(v(2) - 3) < eps); + ut_test(std::fabs(v(0) - 1) < eps); + ut_test(std::fabs(v(1) - 2) < eps); + ut_test(std::fabs(v(2) - 3) < eps); Vector3f v2(4, 5, 6); - ut_test(fabsf(v2(0) - 4) < eps); - ut_test(fabsf(v2(1) - 5) < eps); - ut_test(fabsf(v2(2) - 6) < eps); + ut_test(std::fabs(v2(0) - 4) < eps); + ut_test(std::fabs(v2(1) - 5) < eps); + ut_test(std::fabs(v2(2) - 6) < eps); SquareMatrix m = diag(Vector3f(1, 2, 3)); - ut_test(fabsf(m(0, 0) - 1) < eps); - ut_test(fabsf(m(1, 1) - 2) < eps); - ut_test(fabsf(m(2, 2) - 3) < eps); + ut_test(std::fabs(m(0, 0) - 1) < eps); + ut_test(std::fabs(m(1, 1) - 2) < eps); + ut_test(std::fabs(m(2, 2) - 3) < eps); return true; }