2 #ifndef TEST_TEST_HELPERS_H_ 3 #define TEST_TEST_HELPERS_H_ 5 #include <gtest/gtest.h> 12 T invNorm = 1 /
sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
14 if (fabs(max) < fabs(q1)) max = q1;
15 if (fabs(max) < fabs(q2)) max = q2;
16 if (fabs(max) < fabs(q3)) max = q3;
17 if (max < 0) invNorm *= -1.0;
29 return std::abs((q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3) - 1.0
f) < 10e-6;
33 static inline bool quat_equal(T q0, T q1, T q2, T q3, T qr0, T qr1, T qr2, T qr3) {
37 return (fabs(q0 - qr0) <
MAX_DIFF) &&
44 static inline bool quat_eq_ex_z(T q0, T q1, T q2, T q3, T qr0, T qr1, T qr2, T qr3) {
63 #define ASSERT_IS_NORMALIZED_(q0, q1, q2, q3) ASSERT_TRUE(is_normalized(q0, q1, q2, q3)) << "q0: " << q0 << ", q1: " << q1 << ", q2: " << q2 << ", q3: " << q3; 64 #define ASSERT_IS_NORMALIZED(...) ASSERT_IS_NORMALIZED_(__VA_ARGS__) 66 #define ASSERT_QUAT_EQUAL_(q0, q1, q2, q3, qr0, qr1, qr2, qr3) ASSERT_TRUE(quat_equal(q0, q1, q2, q3, qr0, qr1, qr2, qr3)) << "q0: " << q0 << ", q1: " << q1 << ", q2: " << q2 << ", q3: " << q3; 67 #define ASSERT_QUAT_EQUAL(...) ASSERT_QUAT_EQUAL_(__VA_ARGS__) 69 #define ASSERT_QUAT_EQUAL_EX_Z_(q0, q1, q2, q3, qr0, qr1, qr2, qr3) ASSERT_TRUE(quat_eq_ex_z(q0, q1, q2, q3, qr0, qr1, qr2, qr3)) << "q0: " << q0 << ", q1: " << q1 << ", q2: " << q2 << ", q3: " << q3; 70 #define ASSERT_QUAT_EQUAL_EX_Z(...) ASSERT_QUAT_EQUAL_EX_Z_(__VA_ARGS__) 74 #define AM_EAST_NORTH_UP 0.0, 0.0, 9.81, 0.0, 0.0005, -0.0005 75 #define AM_NORTH_EAST_DOWN 0.0, 0.0, -9.81, 0.0005, 0.0, 0.0005 76 #define AM_NORTH_WEST_UP 0.0, 0.0, 9.81, 0.0005, 0.0, -0.0005 77 #define AM_SOUTH_UP_WEST 0.0, 9.81, 0.0, -0.0005, -0.0005, 0.0 78 #define AM_SOUTH_EAST_UP 0.0, 0.0, 9.81, -0.0005, 0.0, -0.0005 79 #define AM_WEST_NORTH_DOWN 0.0, 0.0, -9.81, 0.0, 0.0005, 0.0005 82 #define AM_WEST_NORTH_DOWN_RSD 0.12, 0.29, -9.83, 6.363e-06, 1.0908e-05, 4.2723e-05 83 #define AM_NE_NW_UP_RSD 0.20, 0.55, 9.779, 8.484e-06, 8.181e-06, -4.3329e-05 86 #define ACCEL_ONLY(ax,ay,az, mx,my,mz) ax, ay, az 90 #define QUAT_IDENTITY 1.0, 0.0, 0.0, 0.0 91 #define QUAT_MZ_90 0.707107, 0.0, 0.0, -0.707107 92 #define QUAT_X_180 0.0, 1.0, 0.0, 0.0 93 #define QUAT_XMYMZ_120 0.5, 0.5, -0.5, -0.5 94 #define QUAT_WEST_NORTH_DOWN_RSD_NWU 0.01, 0.86, 0.50, 0.012 95 #define QUAT_WEST_NORTH_DOWN_RSD_ENU 0.0, -0.25, -0.97, -0.02 96 #define QUAT_WEST_NORTH_DOWN_RSD_NED 0.86, -0.01, 0.01, -0.50 97 #define QUAT_NE_NW_UP_RSD_NWU 0.91, 0.03, -0.02, -0.41 98 #define QUAT_NE_NW_UP_RSD_ENU 0.93, 0.03, 0.0, 0.35 99 #define QUAT_NE_NW_UP_RSD_NED 0.021, -0.91, -0.41, 0.02
TF2SIMD_FORCE_INLINE const tf2Scalar & getW() const
Quaternion inverse() const
static bool quat_equal(T q0, T q1, T q2, T q3, T qr0, T qr1, T qr2, T qr3)
static bool quat_eq_ex_z(T q0, T q1, T q2, T q3, T qr0, T qr1, T qr2, T qr3)
static bool is_normalized(T q0, T q1, T q2, T q3)
static void normalize_quaternion(T &q0, T &q1, T &q2, T &q3)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
double max(double a, double b)