dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

This commit is contained in:
Daniel Agar 2022-07-11 09:33:18 -04:00
parent fe22167512
commit a73efd9c4f
32 changed files with 297 additions and 323 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

114
platforms/nuttx/NuttX/include/cxx/cmath Normal file
View File

@ -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)
}

1
src/drivers/cyphal/Actuators/EscClient.hpp
View File

@ -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>

2
src/drivers/cyphal/Publishers/Publisher.hpp
View File

@ -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"

4
src/drivers/uavcan/sensors/gnss.cpp
View File

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

8
src/drivers/uavcannode/Publishers/GnssFix2.hpp
View File

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

4
src/lib/matrix/matrix/AxisAngle.hpp
View File

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

12
src/lib/matrix/matrix/Dcm.hpp
View File

@ -123,12 +123,12 @@ public:
Dcm(const Euler<Type> &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;

32
src/lib/matrix/matrix/Dual.hpp
View File

@ -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);
return Dual<Scalar, N>(asin(a.value), asin_d * a.derivative);
Scalar asin_d = Scalar(1) / std::sqrt(Scalar(1) - a.value * a.value);
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);
return Dual<Scalar, N>(acos(a.value), acos_d * a.derivative);
Scalar acos_d = -Scalar(1) / std::sqrt(Scalar(1) - a.value * a.value);
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

14
src/lib/matrix/matrix/Euler.hpp
View File

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

2
src/lib/matrix/matrix/LeastSquaresSolver.hpp
View File

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

15
src/lib/matrix/matrix/Matrix.hpp
View File

@ -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 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<typename Type>
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 {

2
src/lib/matrix/matrix/PseudoInverse.hpp
View File

@ -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++) {

34
src/lib/matrix/matrix/Quaternion.hpp
View File

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

2
src/lib/matrix/matrix/Slice.hpp
View File

@ -285,7 +285,7 @@ public:
Type norm() const
{
return matrix::sqrt(norm_squared());
return std::sqrt(norm_squared());
}
bool longerThan(Type testVal) const

2
src/lib/matrix/matrix/SparseVector.hpp
View File

@ -176,7 +176,7 @@ public:
Type norm() const
{
return matrix::sqrt(norm_squared());
return std::sqrt(norm_squared());
}
bool longerThan(Type testVal) const

16
src/lib/matrix/matrix/SquareMatrix.hpp
View File

@ -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 {

4
src/lib/matrix/matrix/Vector.hpp
View File

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

28
src/lib/matrix/matrix/helper_functions.hpp
View File

@ -1,26 +1,10 @@
#pragma once
#include "math.hpp"
#if defined (__PX4_NUTTX) || defined (__PX4_QURT)
#include <px4_defines.h>
#endif
#include <cmath>
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)
|| (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<typename Type>
Type wrap_2pi(Type x)
{
return wrap(x, Type(0), Type(M_TWOPI));
return wrap(x, Type(0), Type((2 * M_PI)));
}
/**

6
src/lib/matrix/matrix/math.hpp
View File

@ -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"

140
src/lib/matrix/matrix/stdlib_imports.hpp
View File

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

4
src/lib/matrix/test/MatrixAssignmentTest.cpp
View File

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

8
src/lib/matrix/test/MatrixAttitudeTest.cpp
View File

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

24
src/lib/matrix/test/MatrixDualTest.cpp
View File

@ -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).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
}

12
src/lib/matrix/test/MatrixHelperTest.cpp
View File

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

2
src/lib/matrix/test/MatrixIntegralTest.cpp
View File

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

2
src/lib/matrix/test/test_data.py
View File

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

2
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp
View File

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

2
src/modules/ekf2/EKF/common.h
View File

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

2
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
View File

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

2
src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp
View File

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

4
src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp
View File

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

114
src/systemcmds/tests/test_matrix.cpp
View File

@ -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<float>(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<float>(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<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(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<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(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<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(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<float, 3> 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;
}