5 #if defined (__PX4_NUTTX) || defined (__PX4_QURT) 6 #include <px4_defines.h> 12 template<
typename Type>
14 #if defined (__PX4_NUTTX) 15 return PX4_ISFINITE(x);
16 #elif defined (__PX4_QURT) 17 return __builtin_isfinite(x);
19 return std::isfinite(x);
34 template<
typename Type>
35 bool isEqualF(
const Type x,
const Type y,
const Type eps = 1e-4
f)
37 return (matrix::fabs(x - y) <= eps)
38 || (isnan(x) && isnan(y))
39 || (isinf(x) && isinf(y) && isnan(x - y));
50 template<
typename Type>
51 Type
wrap(Type x, Type low, Type high) {
53 if (low <= x && x < high) {
57 const Type range = high - low;
58 const Type inv_range = Type(1) / range;
59 const Type num_wraps =
floor((x - low) * inv_range);
60 return x - range * num_wraps;
66 template<
typename Type>
75 template<
typename Type>
Type wrap(Type x, Type low, Type high)
Wrap value to stay in range [low, high)
Type wrap_2pi(Type x)
Wrap value in range [0, 2π)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Type wrap_pi(Type x)
Wrap value in range [-π, π)
bool isEqualF(const Type x, const Type y, const Type eps=1e-4f)
Compare if two floating point numbers are equal.
Dual< Scalar, N > floor(const Dual< Scalar, N > &a)