mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-14 12:48:31 -04:00
180359d6dd
For ROTATION_ROLL_90_PITCH_68_YAW_293 consider the angles as 90, 68.8 and 293.3 degrees to pre-calculate rotation. This matches the rotation matrix used in code. While at it, check not only the values are close enough but also the length of the vector.
67 lines
2.8 KiB
C++
67 lines
2.8 KiB
C++
#include <AP_gtest.h>
|
|
|
|
#include <AP_Math/AP_Math.h>
|
|
|
|
#define SQRT_2 1.4142135623730951f
|
|
|
|
TEST(VectorTest, Rotations)
|
|
{
|
|
unsigned rotation_count = 0;
|
|
|
|
#define TEST_ROTATION(rotation, _x, _y, _z) { \
|
|
const float accuracy = 1.0e-6; \
|
|
Vector3f v(1, 1, 1); \
|
|
v.rotate(rotation); \
|
|
Vector3f expected(_x, _y, _z); \
|
|
EXPECT_NEAR(expected.length(), v.length(), accuracy); \
|
|
EXPECT_FLOAT_EQ(expected.x, v.x); \
|
|
EXPECT_FLOAT_EQ(expected.y, v.y); \
|
|
EXPECT_FLOAT_EQ(expected.z, v.z); \
|
|
rotation_count++; \
|
|
}
|
|
|
|
TEST_ROTATION(ROTATION_NONE, 1, 1, 1);
|
|
TEST_ROTATION(ROTATION_YAW_45, 0, SQRT_2, 1);
|
|
TEST_ROTATION(ROTATION_YAW_90, -1, 1, 1);
|
|
TEST_ROTATION(ROTATION_YAW_135, -SQRT_2, 0, 1);
|
|
TEST_ROTATION(ROTATION_YAW_180, -1, -1, 1);
|
|
TEST_ROTATION(ROTATION_YAW_225, 0, -SQRT_2, 1);
|
|
TEST_ROTATION(ROTATION_YAW_270, 1, -1, 1);
|
|
TEST_ROTATION(ROTATION_YAW_315, SQRT_2, 0, 1);
|
|
TEST_ROTATION(ROTATION_ROLL_180, 1, -1, -1);
|
|
TEST_ROTATION(ROTATION_ROLL_180_YAW_45, SQRT_2, 0, -1);
|
|
TEST_ROTATION(ROTATION_ROLL_180_YAW_90, 1, 1, -1);
|
|
TEST_ROTATION(ROTATION_ROLL_180_YAW_135, 0, SQRT_2, -1);
|
|
TEST_ROTATION(ROTATION_PITCH_180, -1, 1, -1);
|
|
TEST_ROTATION(ROTATION_ROLL_180_YAW_225, -SQRT_2, 0, -1);
|
|
TEST_ROTATION(ROTATION_ROLL_180_YAW_270, -1, -1, -1);
|
|
TEST_ROTATION(ROTATION_ROLL_180_YAW_315, 0, -SQRT_2, -1);
|
|
TEST_ROTATION(ROTATION_ROLL_90, 1, -1, 1);
|
|
TEST_ROTATION(ROTATION_ROLL_90_YAW_45, SQRT_2, 0, 1);
|
|
TEST_ROTATION(ROTATION_ROLL_90_YAW_90, 1, 1, 1);
|
|
TEST_ROTATION(ROTATION_ROLL_90_YAW_135, 0, SQRT_2, 1);
|
|
TEST_ROTATION(ROTATION_ROLL_270, 1, 1, -1);
|
|
TEST_ROTATION(ROTATION_ROLL_270_YAW_45, 0, SQRT_2, -1);
|
|
TEST_ROTATION(ROTATION_ROLL_270_YAW_90, -1, 1, -1);
|
|
TEST_ROTATION(ROTATION_ROLL_270_YAW_135, -SQRT_2, 0, -1);
|
|
TEST_ROTATION(ROTATION_PITCH_90, 1, 1, -1);
|
|
TEST_ROTATION(ROTATION_PITCH_270, -1, 1, 1);
|
|
TEST_ROTATION(ROTATION_PITCH_180_YAW_90, -1, -1, -1);
|
|
TEST_ROTATION(ROTATION_PITCH_180_YAW_270, 1, 1, -1);
|
|
TEST_ROTATION(ROTATION_ROLL_90_PITCH_90, 1, -1, -1);
|
|
TEST_ROTATION(ROTATION_ROLL_180_PITCH_90, -1, -1, -1);
|
|
TEST_ROTATION(ROTATION_ROLL_270_PITCH_90, -1, 1, -1);
|
|
TEST_ROTATION(ROTATION_ROLL_90_PITCH_180, -1, -1, -1);
|
|
TEST_ROTATION(ROTATION_ROLL_270_PITCH_180, -1, 1, 1);
|
|
TEST_ROTATION(ROTATION_ROLL_90_PITCH_270, -1, -1, 1);
|
|
TEST_ROTATION(ROTATION_ROLL_180_PITCH_270, 1, -1, 1);
|
|
TEST_ROTATION(ROTATION_ROLL_270_PITCH_270, 1, 1, 1);
|
|
TEST_ROTATION(ROTATION_ROLL_90_PITCH_180_YAW_90, 1, -1, -1);
|
|
TEST_ROTATION(ROTATION_ROLL_90_YAW_270, -1, -1, 1);
|
|
TEST_ROTATION(ROTATION_ROLL_90_PITCH_68_YAW_293, -0.4066309f, -1.5839677f, -0.5706992f);
|
|
|
|
EXPECT_EQ(ROTATION_MAX, rotation_count) << "All rotations are expect to be tested";
|
|
}
|
|
|
|
AP_GTEST_MAIN()
|