aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Math: tests: test quaternion rotation is the same as vector

Browse Source
This commit is contained in:
Iampete1 2021-07-23 20:45:50 +01:00 committed by Andrew Tridgell
parent 6c5424aad6
commit f8220a8adf
1 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Math/tests/test_math.cpp
View File

@ -19,12 +19,20 @@ TEST(VectorTest, Rotations)
#define TEST_ROTATION(rotation, _x, _y, _z) { \
const float accuracy = 1.0e-6; \
Vector3f v(1, 1, 1); \
Vector3f v2 = v; \
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); \
Quaternion quat; \
quat.from_rotation(rotation); \
quat.earth_to_body(v2); \
EXPECT_NEAR(expected.length(), v.length(), accuracy); \
EXPECT_NEAR(expected.x, v2.x, accuracy); \
EXPECT_NEAR(expected.y, v2.y, accuracy); \
EXPECT_NEAR(expected.z, v2.z, accuracy); \
rotation_count++; \
}