diff --git a/test/quaternion_test.cc b/test/quaternion_test.cc index f419893..9771f23 100644 --- a/test/quaternion_test.cc +++ b/test/quaternion_test.cc @@ -111,10 +111,10 @@ TEST(Quaterniond, ShortestSLERP) { // Our starting point is an orientation that is yawed 45° - our // orientation is pointed π/4 radians in the X axis. - geom::Quaterniond p = geom::Quaterniond {0.382683, 0, 0, 0.92388}; + geom::Quaterniond p {0.382683, 0, 0, 0.92388}; // Our ending point is an orientation that is yawed -45° - or // pointed -π/4 radians in the X axis. - geom::Quaterniond q = geom::Quaterniond {-0.382683, 0, 0, 0.92388}; + geom::Quaterniond q {-0.382683, 0, 0, 0.92388}; // The halfway point should be oriented midway about the X axis. It turns // out this is an identity quaternion. geom::Quaterniond r; @@ -125,6 +125,25 @@ TEST(Quaterniond, ShortestSLERP) } +TEST(Quaterniond, ShortestSLERP2) +{ + // Start with an orientation pointing forward, all Euler angles + // set to 0. + geom::Quaterniond start {0.0, 0.0, 0.0, 1.0}; + // The goal is to end up face up, or 90º pitch (still facing forward). + geom::Quaterniond end {0, -0.707107, 0, 0.707107}; + // Halfway to the endpoint should be a 45º pitch. + geom::Quaterniond halfway {0, -0.382683, 0, 0.92388 }; + // 2/3 of the way should be 60º pitch. + geom::Quaterniond twoThirds {0, -0.5, 0, 0.866025}; + + EXPECT_EQ(ShortestSLERP(start, end, 0.0), start); + EXPECT_EQ(ShortestSLERP(start, end, 1.0), end); + EXPECT_EQ(ShortestSLERP(start, end, 0.5), halfway); + EXPECT_EQ(ShortestSLERP(start, end, 2.0/3.0), twoThirds); +} + + TEST(Quaterniond, Unit) { geom::Quaterniond q(geom::Vector4d{0.5773502691896258, 0.5773502691896258, 0.5773502691896258, 0.0}); @@ -228,6 +247,43 @@ TEST(Quaternionf, Rotate) } +TEST(Quaternionf, ShortestSLERP) +{ + // Our starting point is an orientation that is yawed 45° - our + // orientation is pointed π/4 radians in the X axis. + geom::Quaternionf p {0.382683, 0, 0, 0.92388}; + // Our ending point is an orientation that is yawed -45° - or + // pointed -π/4 radians in the X axis. + geom::Quaternionf q {-0.382683, 0, 0, 0.92388}; + // The halfway point should be oriented midway about the X axis. It turns + // out this is an identity quaternion. + geom::Quaternionf r; + + EXPECT_EQ(geom::ShortestSLERP(p, q, (float)0.0), p); + EXPECT_EQ(geom::ShortestSLERP(p, q, (float)1.0), q); + EXPECT_EQ(geom::ShortestSLERP(p, q, (float)0.5), r); +} + + +TEST(Quaternionf, ShortestSLERP2) +{ + // Start with an orientation pointing forward, all Euler angles + // set to 0. + geom::Quaternionf start {0.0, 0.0, 0.0, 1.0}; + // The goal is to end up face up, or 90º pitch (still facing forward). + geom::Quaternionf end {0, -0.707107, 0, 0.707107}; + // Halfway to the endpoint should be a 45º pitch. + geom::Quaternionf halfway {0, -0.382683, 0, 0.92388 }; + // 2/3 of the way should be 60º pitch. + geom::Quaternionf twoThirds {0, -0.5, 0, 0.866025}; + + EXPECT_EQ(ShortestSLERP(start, end, (float)0.0), start); + EXPECT_EQ(ShortestSLERP(start, end, (float)1.0), end); + EXPECT_EQ(ShortestSLERP(start, end, (float)0.5), halfway); + EXPECT_EQ(ShortestSLERP(start, end, (float)(2.0/3.0)), twoThirds); +} + + TEST(Quaternionf, Unit) { geom::Quaternionf q(geom::Vector4f{0.5773502691896258, 0.5773502691896258, 0.5773502691896258, 0.0});