NuttX carry minimal c++ cmath (replacing Matrix stdlib_imports.hpp)

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 <cstdlib>
+#include <inttypes.h>
+
+#include <math.h>
+
+#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)
+
+}

src/drivers/cyphal/Actuators/EscClient.hpp

@@ -58,6 +58,7 @@
 #include <lib/mixer_module/mixer_module.hpp>
 
 // UDRAL Specification Messages
+using std::isfinite;
 #include <reg/udral/service/actuator/common/sp/Vector31_0_1.h>
 #include <reg/udral/service/common/Readiness_0_1.h>
 

src/drivers/cyphal/Publishers/Publisher.hpp

@@ -46,6 +46,8 @@
 #include <lib/parameters/param.h>
 #include <containers/List.hpp>
 
+#include <cmath>
+using std::isfinite;
 #include <uavcan/_register/Access_1_0.h>
 
 #include "../CanardHandle.hpp"

src/drivers/uavcan/sensors/gnss.cpp

@@ -282,11 +282,11 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
 	if (!msg.ecef_position_velocity.empty()) {
 		heading = msg.ecef_position_velocity[0].velocity_xyz[0];
 
-		if (!isnan(msg.ecef_position_velocity[0].velocity_xyz[1])) {
+		if (!std::isnan(msg.ecef_position_velocity[0].velocity_xyz[1])) {
 			heading_offset = msg.ecef_position_velocity[0].velocity_xyz[1];
 		}
 
-		if (!isnan(msg.ecef_position_velocity[0].velocity_xyz[2])) {
+		if (!std::isnan(msg.ecef_position_velocity[0].velocity_xyz[2])) {
 			heading_accuracy = msg.ecef_position_velocity[0].velocity_xyz[2];
 		}
 	}

src/drivers/uavcannode/Publishers/GnssFix2.hpp

@@ -33,6 +33,8 @@
 
 #pragma once
 
+#include <cmath>
+
 #include "UavcanPublisherBase.hpp"
 
 #include <uavcan/equipment/gnss/Fix2.hpp>
@@ -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;
 				}
 

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)) {
+		if (std::fabs(mag) >= Type(1e-10)) {
-			v = q.imag() * Type(Type(2) * atan2(mag, q(0)) / mag);
+			v = q.imag() * Type(Type(2) * std::atan2(mag, q(0)) / mag);
 
 		} else {
 			v = q.imag() * Type(Type(2) * Type(sign(q(0))));

src/lib/matrix/matrix/Dcm.hpp

@@ -123,12 +123,12 @@ public:
 	Dcm(const Euler<Type> &euler)
 	{
 		Dcm &dcm = *this;
-		Type cosPhi = Type(cos(euler.phi()));
+		Type cosPhi = Type(std::cos(euler.phi()));
-		Type sinPhi = Type(sin(euler.phi()));
+		Type sinPhi = Type(std::sin(euler.phi()));
-		Type cosThe = Type(cos(euler.theta()));
+		Type cosThe = Type(std::cos(euler.theta()));
-		Type sinThe = Type(sin(euler.theta()));
+		Type sinThe = Type(std::sin(euler.theta()));
-		Type cosPsi = Type(cos(euler.psi()));
+		Type cosPsi = Type(std::cos(euler.psi()));
-		Type sinPsi = Type(sin(euler.psi()));
+		Type sinPsi = Type(std::sin(euler.psi()));
 
 		dcm(0, 0) = cosThe * cosPsi;
 		dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;

src/lib/matrix/matrix/Dual.hpp

@@ -13,6 +13,8 @@
 
 #pragma once
 
+#include <cmath>
+
 #include "math.hpp"
 
 namespace matrix
@@ -187,7 +189,7 @@ Dual<Scalar, N> operator/(Scalar a, const Dual<Scalar, N> &b)
 template <typename Scalar, size_t N>
 Dual<Scalar, N> sqrt(const Dual<Scalar, N> &a)
 {
-	Scalar real = sqrt(a.value);
+	Scalar real = std::sqrt(a.value);
 	return Dual<Scalar, N>(real, a.derivative * (Scalar(1) / (Scalar(2) * real)));
 }
 
@@ -202,14 +204,14 @@ Dual<Scalar, N> abs(const Dual<Scalar, N> &a)
 template <typename Scalar, size_t N>
 Dual<Scalar, N> ceil(const Dual<Scalar, N> &a)
 {
-	return Dual<Scalar, N>(ceil(a.value));
+	return Dual<Scalar, N>(std::ceil(a.value));
 }
 
 // floor
 template <typename Scalar, size_t N>
 Dual<Scalar, N> floor(const Dual<Scalar, N> &a)
 {
-	return Dual<Scalar, N>(floor(a.value));
+	return Dual<Scalar, N>(std::floor(a.value));
 }
 
 // fmod
@@ -237,7 +239,7 @@ Dual<Scalar, N> min(const Dual<Scalar, N> &a, const Dual<Scalar, N> &b)
 template <typename Scalar>
 bool IsNan(Scalar a)
 {
-	return isnan(a);
+	return std::isnan(a);
 }
 
 template <typename Scalar, size_t N>
@@ -250,7 +252,7 @@ bool IsNan(const Dual<Scalar, N> &a)
 template <typename Scalar>
 bool IsFinite(Scalar a)
 {
-	return isfinite(a);
+	return std::isfinite(a);
 }
 
 template <typename Scalar, size_t N>
@@ -263,7 +265,7 @@ bool IsFinite(const Dual<Scalar, N> &a)
 template <typename Scalar>
 bool IsInf(Scalar a)
 {
-	return isinf(a);
+	return std::isinf(a);
 }
 
 template <typename Scalar, size_t N>
@@ -278,21 +280,21 @@ bool IsInf(const Dual<Scalar, N> &a)
 template <typename Scalar, size_t N>
 Dual<Scalar, N> sin(const Dual<Scalar, N> &a)
 {
-	return Dual<Scalar, N>(sin(a.value), cos(a.value) * a.derivative);
+	return Dual<Scalar, N>(std::sin(a.value), std::cos(a.value) * a.derivative);
 }
 
 // cos
 template <typename Scalar, size_t N>
 Dual<Scalar, N> cos(const Dual<Scalar, N> &a)
 {
-	return Dual<Scalar, N>(cos(a.value), -sin(a.value) * a.derivative);
+	return Dual<Scalar, N>(std::cos(a.value), -std::sin(a.value) * a.derivative);
 }
 
 // tan
 template <typename Scalar, size_t N>
 Dual<Scalar, N> tan(const Dual<Scalar, N> &a)
 {
-	Scalar real = tan(a.value);
+	Scalar real = std::tan(a.value);
 	return Dual<Scalar, N>(real, (Scalar(1) + real * real) * a.derivative);
 }
 
@@ -300,16 +302,16 @@ Dual<Scalar, N> tan(const Dual<Scalar, N> &a)
 template <typename Scalar, size_t N>
 Dual<Scalar, N> asin(const Dual<Scalar, N> &a)
 {
-	Scalar asin_d = Scalar(1) / sqrt(Scalar(1) - a.value * a.value);
+	Scalar asin_d = Scalar(1) / std::sqrt(Scalar(1) - a.value * a.value);
-	return Dual<Scalar, N>(asin(a.value), asin_d * a.derivative);
+	return Dual<Scalar, N>(std::asin(a.value), asin_d * a.derivative);
 }
 
 // acos
 template <typename Scalar, size_t N>
 Dual<Scalar, N> acos(const Dual<Scalar, N> &a)
 {
-	Scalar acos_d = -Scalar(1) / sqrt(Scalar(1) - a.value * a.value);
+	Scalar acos_d = -Scalar(1) / std::sqrt(Scalar(1) - a.value * a.value);
-	return Dual<Scalar, N>(acos(a.value), acos_d * a.derivative);
+	return Dual<Scalar, N>(std::acos(a.value), acos_d * a.derivative);
 }
 
 // atan
@@ -317,7 +319,7 @@ template <typename Scalar, size_t N>
 Dual<Scalar, N> atan(const Dual<Scalar, N> &a)
 {
 	Scalar atan_d = Scalar(1) / (Scalar(1) + a.value * a.value);
-	return Dual<Scalar, N>(atan(a.value), atan_d * a.derivative);
+	return Dual<Scalar, N>(std::atan(a.value), atan_d * a.derivative);
 }
 
 // atan2
@@ -326,7 +328,7 @@ Dual<Scalar, N> atan2(const Dual<Scalar, N> &a, const Dual<Scalar, N> &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<Scalar, N>(atan2(a.value, b.value), (a.derivative * b.value - a.value * b.derivative) * atan_d);
+	return Dual<Scalar, N>(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

src/lib/matrix/matrix/Euler.hpp

@@ -91,19 +91,19 @@ public:
 	*/
 	Euler(const Dcm<Type> &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));
+			phi() = std::atan2(dcm(2, 1), dcm(2, 2));
-			psi() = atan2(dcm(1, 0), dcm(0, 0));
+			psi() = std::atan2(dcm(1, 0), dcm(0, 0));
 		}
 	}
 

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;
 

src/lib/matrix/matrix/Matrix.hpp

@@ -8,6 +8,7 @@
 
 #pragma once
 
+#include <cmath>
 #include <cstdio>
 #include <cstring>
 
@@ -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<typename Type>
 Type min(const Type x, const Type y)
 {
-	bool x_is_nan = isnan(x);
+	bool x_is_nan = std::isnan(x);
-	bool y_is_nan = isnan(y);
+	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<typename Type>
 Type max(const Type x, const Type y)
 {
-	bool x_is_nan = isnan(x);
+	bool x_is_nan = std::isnan(x);
-	bool y_is_nan = isnan(y);
+	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 {

src/lib/matrix/matrix/PseudoInverse.hpp

@@ -106,7 +106,7 @@ SquareMatrix<Type, N> fullRankCholesky(const SquareMatrix<Type, N> &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++) {

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<Type> &euler)
 	{
 		Quaternion &q = *this;
-		Type cosPhi_2 = Type(cos(euler.phi() / Type(2)));
+		Type cosPhi_2 = Type(std::cos(euler.phi() / Type(2)));
-		Type cosTheta_2 = Type(cos(euler.theta() / Type(2)));
+		Type cosTheta_2 = Type(std::cos(euler.theta() / Type(2)));
-		Type cosPsi_2 = Type(cos(euler.psi() / Type(2)));
+		Type cosPsi_2 = Type(std::cos(euler.psi() / Type(2)));
-		Type sinPhi_2 = Type(sin(euler.phi() / Type(2)));
+		Type sinPhi_2 = Type(std::sin(euler.phi() / Type(2)));
-		Type sinTheta_2 = Type(sin(euler.theta() / Type(2)));
+		Type sinTheta_2 = Type(std::sin(euler.theta() / Type(2)));
-		Type sinPsi_2 = Type(sin(euler.psi() / 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));
+			Type magnitude = std::sin(angle / Type(2));
-			q(0) = cos(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);
+			sinc_u = Type(std::sin(u_norm) / u_norm);
-			cos_u = Type(cos(u_norm));
+			cos_u = Type(std::cos(u_norm));
 		}
 
 		Vector<Type, 3> v = sinc_u * u;
@@ -410,7 +410,7 @@ public:
 			return Type(0.5) * (Dcm<Type>() + u_hat + (Type(1.0 / 3.0) + u_norm * u_norm / Type(45.0)) * u_hat * u_hat);
 
 		} else {
-			return Type(0.5) * (Dcm<Type>() + u_hat + (Type(1.0) - u_norm * Type(cos(u_norm) / sin(u_norm))) /
+			return Type(0.5) * (Dcm<Type>() + 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)));
 			}
 		}

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

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

src/lib/matrix/matrix/SquareMatrix.hpp

@@ -8,6 +8,8 @@
 
 #pragma once
 
+#include <float.h> // FLT_EPSILON
+
 #include "math.hpp"
 
 namespace matrix
@@ -339,12 +341,12 @@ bool inv(const SquareMatrix<Type, M> &A, SquareMatrix<Type, M> &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<Type, M> &A, SquareMatrix<Type, M> &inv, size_t rank
 #endif
 
 		// failsafe, return zero matrix
-		if (fabs(static_cast<float>(U(n, n))) < FLT_EPSILON) {
+		if (std::fabs(static_cast<float>(U(n, n))) < FLT_EPSILON) {
 			return false;
 		}
 
@@ -438,7 +440,7 @@ bool inv(const SquareMatrix<Type, M> &A, SquareMatrix<Type, M> &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<Type, 2> &A, SquareMatrix<Type, 2> &inv)
 {
 	Type det = A(0, 0) * A(1, 1) - A(1, 0) * A(0, 1);
 
-	if (fabs(static_cast<float>(det)) < FLT_EPSILON || !is_finite(det)) {
+	if (std::fabs(static_cast<float>(det)) < FLT_EPSILON || !std::isfinite(det)) {
 		return false;
 	}
 
@@ -473,7 +475,7 @@ bool inv(const SquareMatrix<Type, 3> &A, SquareMatrix<Type, 3> &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<float>(det)) < FLT_EPSILON || !is_finite(det)) {
+	if (std::fabs(static_cast<float>(det)) < FLT_EPSILON || !std::isfinite(det)) {
 		return false;
 	}
 
@@ -532,7 +534,7 @@ SquareMatrix <Type, M> cholesky(const SquareMatrix<Type, M> &A)
 					L(j, j) = 0;
 
 				} else {
-					L(j, j) = sqrt(res);
+					L(j, j) = std::sqrt(res);
 				}
 
 			} else {

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;

src/lib/matrix/matrix/helper_functions.hpp

@@ -1,26 +1,10 @@
 #pragma once
 
-#include "math.hpp"
+#include <cmath>
-
-#if defined (__PX4_NUTTX) || defined (__PX4_QURT)
-#include <px4_defines.h>
-#endif
 
 namespace matrix
 {
 
-template<typename Type>
-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<typename Type>
 bool isEqualF(const Type x, const Type y, const Type eps = Type(1e-4f))
 {
-	return (matrix::fabs(x - y) <= eps)
+	return (std::fabs(x - y) <= eps)
-	       || (isnan(x) && isnan(y))
+	       || (std::isnan(x) && std::isnan(y))
-	       || (isinf(x) && isinf(y) && isnan(x - 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<typename Type>
 Type wrap_2pi(Type x)
 {
-	return wrap(x, Type(0), Type(M_TWOPI));
+	return wrap(x, Type(0), Type((2 * M_PI)));
 }
 
 /**

src/lib/matrix/matrix/math.hpp

@@ -1,11 +1,9 @@
 #pragma once
 
 #include <assert.h>
-#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"

src/lib/matrix/matrix/stdlib_imports.hpp

@@ -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 <pavel.kirienko@zubax.com>
- */
-
-#pragma once
-
-#include <cmath>
-#include <cstdlib>
-#include <inttypes.h>
-
-#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
-
-}

src/lib/matrix/test/MatrixAssignmentTest.cpp

@@ -31,6 +31,8 @@
  *
  ****************************************************************************/
 
+#include <cmath>
+
 #include <gtest/gtest.h>
 #include <matrix/math.hpp>
 
@@ -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)));
 		}
 	}
 

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);

src/lib/matrix/test/MatrixDualTest.cpp

@@ -31,6 +31,8 @@
  *
  ****************************************************************************/
 
+#include <cmath>
+
 #include <gtest/gtest.h>
 #include <matrix/math.hpp>
 #include <iostream>
@@ -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<float, 1> c(sqrt(-1.f), 0);
+		Dual<float, 1> 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<float, 1> c(sqrt(-1.f), 0);
+		Dual<float, 1> c(std::sqrt(-1.f), 0);
 		EXPECT_FALSE(IsFinite(c));
 		EXPECT_FALSE(IsInf(c));
 		Dual<float, 1> 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<float, 1> c(0.3f, 0);
-		EXPECT_FLOAT_EQ(asin(c).value, asin(c.value));
+		EXPECT_FLOAT_EQ(asin(c).value, std::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).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).value, std::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).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
 	}
 

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. - (2 * M_PI)));
-	EXPECT_FLOAT_EQ(wrap_pi(-4.), (-4. + M_TWOPI));
+	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(200.), (200. - 31. * (2 * M_PI)));
-	EXPECT_FLOAT_EQ(wrap_2pi(-201.), (-201. + 32. * M_TWOPI));
+	EXPECT_FLOAT_EQ(wrap_2pi(-201.), (-201. + 32. * (2 * M_PI)));
-	EXPECT_FALSE(is_finite(wrap_2pi(NAN)));
+	EXPECT_FALSE(std::isfinite(wrap_2pi(NAN)));
 
 	// Equality checks
 	EXPECT_TRUE(isEqualF(1., 1.));

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<float, 6, 1>()*v));
 }

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]),
     ])
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -41,6 +41,8 @@
 
 #pragma once
 
+#include <cstdint>
+
 #include <matrix/matrix/math.hpp>
 
 enum class AllocationMethod {

src/modules/ekf2/EKF/common.h

@@ -42,6 +42,8 @@
 #ifndef EKF_COMMON_H
 #define EKF_COMMON_H
 
+#include <cstdint>
+
 #include <matrix/math.hpp>
 
 namespace estimator

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;
 }

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;
 	}
 

src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp

@@ -31,6 +31,8 @@
  *
  ****************************************************************************/
 
+#include <cmath>
+
 #include <gtest/gtest.h>
 #include <ControlMath.hpp>
 #include <px4_platform_common/defines.h>
@@ -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);

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(std::fabs(q(0) - 1) < eps);
-	ut_test(fabs(q(1) - 2) < eps);
+	ut_test(std::fabs(q(1) - 2) < eps);
-	ut_test(fabs(q(2) - 3) < eps);
+	ut_test(std::fabs(q(2) - 3) < eps);
-	ut_test(fabs(q(3) - 4) < 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(std::fabs(q_check(0) - q(0)) < eps);
-	ut_test(fabs(q_check(1) + q(1)) < eps);
+	ut_test(std::fabs(q_check(1) + q(1)) < eps);
-	ut_test(fabs(q_check(2) + q(2)) < eps);
+	ut_test(std::fabs(q_check(2) + q(2)) < eps);
-	ut_test(fabs(q_check(3) + q(3)) < 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(std::fabs(q_check(0) - q(0)) < eps);
-	ut_test(fabs(q_check(1) + q(1)) < eps);
+	ut_test(std::fabs(q_check(1) + q(1)) < eps);
-	ut_test(fabs(q_check(2) + q(2)) < eps);
+	ut_test(std::fabs(q_check(2) + q(2)) < eps);
-	ut_test(fabs(q_check(3) + q(3)) < 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(std::fabs(qI(0) - q_true(0)) < eps);
-	ut_test(fabs(qI(1) - q_true(1)) < eps);
+	ut_test(std::fabs(qI(1) - q_true(1)) < eps);
-	ut_test(fabs(qI(2) - q_true(2)) < eps);
+	ut_test(std::fabs(qI(2) - q_true(2)) < eps);
-	ut_test(fabs(qI(3) - q_true(3)) < 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(std::fabs(qI(0) - q_true(0)) < eps);
-	ut_test(fabs(qI(1) - q_true(1)) < eps);
+	ut_test(std::fabs(qI(1) - q_true(1)) < eps);
-	ut_test(fabs(qI(2) - q_true(2)) < eps);
+	ut_test(std::fabs(qI(2) - q_true(2)) < eps);
-	ut_test(fabs(qI(3) - q_true(3)) < 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<float>(q);
-	ut_test(fabs(rot(0)) < eps);
+	ut_test(std::fabs(rot(0)) < eps);
-	ut_test(fabs(rot(1) - 1.0f) < eps);
+	ut_test(std::fabs(rot(1) - 1.0f) < eps);
-	ut_test(fabs(rot(2)) < 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<float>(q);
-	ut_test(fabs(rot(0)) < eps);
+	ut_test(std::fabs(rot(0)) < eps);
-	ut_test(fabs(rot(1)) < eps);
+	ut_test(std::fabs(rot(1)) < eps);
-	ut_test(fabs(rot(2)) < eps);
+	ut_test(std::fabs(rot(2)) < eps);
 
 	// from axis angle (zero rotation)
 	rot(0) = rot(1) = rot(2) = 0.0f;
 	q = Quaternion<float>(matrix::AxisAngle<float>(rot));
 	q_true = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
-	ut_test(fabs(q(0) - q_true(0)) < eps);
+	ut_test(std::fabs(q(0) - q_true(0)) < eps);
-	ut_test(fabs(q(1) - q_true(1)) < eps);
+	ut_test(std::fabs(q(1) - q_true(1)) < eps);
-	ut_test(fabs(q(2) - q_true(2)) < eps);
+	ut_test(std::fabs(q(2) - q_true(2)) < eps);
-	ut_test(fabs(q(3) - q_true(3)) < 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<float> s = 1;
-	ut_test(fabs(s - 1) < 1e-5);
+	ut_test(std::fabs(s - 1) < 1e-5);
 
 	Matrix<float, 1, 1> m5 = s;
-	ut_test(fabs(m5(0, 0) - s) < 1e-5);
+	ut_test(std::fabs(m5(0, 0) - s) < 1e-5);
 
 	Matrix<float, 2, 2> 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<float, 5> v1(data1);
-	ut_test(fabs(v1.norm() - 7.416198487095663f) < 1e-5);
+	ut_test(std::fabs(v1.norm() - 7.416198487095663f) < 1e-5);
 	Vector<float, 5> 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<float, 5> 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(std::fabs(c(0) - 0) < 1e-5);
-	ut_test(fabs(c(1) - 0) < 1e-5);
+	ut_test(std::fabs(c(1) - 0) < 1e-5);
 
 	static Matrix<float, 2, 1> 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(std::fabs(d(0, 0) - 1) < 1e-5);
-	ut_test(fabs(d(1, 0) - 0) < 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(std::fabs(e(0) - 1) < 1e-5);
-	ut_test(fabs(e(1) - 0) < 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(std::fabs(f(0) - 4) < 1e-5);
-	ut_test(fabs(f(1) - 5) < 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(std::fabs(v(0) - 1) < eps);
-	ut_test(fabsf(v(1) - 2) < eps);
+	ut_test(std::fabs(v(1) - 2) < eps);
-	ut_test(fabsf(v(2) - 3) < eps);
+	ut_test(std::fabs(v(2) - 3) < eps);
 
 	Vector3f v2(4, 5, 6);
 
-	ut_test(fabsf(v2(0) - 4) < eps);
+	ut_test(std::fabs(v2(0) - 4) < eps);
-	ut_test(fabsf(v2(1) - 5) < eps);
+	ut_test(std::fabs(v2(1) - 5) < eps);
-	ut_test(fabsf(v2(2) - 6) < eps);
+	ut_test(std::fabs(v2(2) - 6) < eps);
 
 	SquareMatrix<float, 3> m = diag(Vector3f(1, 2, 3));
-	ut_test(fabsf(m(0, 0) - 1) < eps);
+	ut_test(std::fabs(m(0, 0) - 1) < eps);
-	ut_test(fabsf(m(1, 1) - 2) < eps);
+	ut_test(std::fabs(m(1, 1) - 2) < eps);
-	ut_test(fabsf(m(2, 2) - 3) < eps);
+	ut_test(std::fabs(m(2, 2) - 3) < eps);
 
 	return true;
 }