You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
66 lines
2.8 KiB
66 lines
2.8 KiB
#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()
|
|
|