kyle/wrmath
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

quaternion <-> euler, lots of fixes.

Browse Source
This commit is contained in:
Kyle Isom
2019-08-06 01:03:30 +00:00
parent a175afd49f
commit 3a9d614010
12 changed files with 611 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

190
test/quaternion_test.cc
View File

@@ -1,3 +1,4 @@
#include <cmath>
#include <sstream>
#include <gtest/gtest.h>
#include <wrmath/geom/quaternion.h>
@@ -8,9 +9,9 @@ using namespace wr;
TEST(Quaterniond, Addition)
{
geom::Quaterniond p(geom::Vector3d {1.0, -2.0, 1.0}, 3.0);
geom::Quaterniond q(geom::Vector3d {-1.0, 2.0, 3.0}, 2.0);
geom::Quaterniond expected(geom::Vector3d{0.0, 0.0, 4.0}, 5.0);
geom::Quaterniond p(geom::Vector4d {1.0, -2.0, 1.0, 3.0});
geom::Quaterniond q(geom::Vector4d {-1.0, 2.0, 3.0, 2.0});
geom::Quaterniond expected(geom::Vector4d{0.0, 0.0, 4.0, 5.0});
EXPECT_EQ(p + q, expected);
EXPECT_EQ(expected - q, p);
@@ -18,10 +19,46 @@ TEST(Quaterniond, Addition)
}
TEST(Quaterniond, Conjugate)
{
geom::Quaterniond p(geom::Vector4d {3.0, 4.0, 5.0, 2.0});
geom::Quaterniond q(geom::Vector4d {-3.0, -4.0, -5.0, 2.0});
EXPECT_EQ(p.conjugate(), q);
}
TEST(Quaterniond, Euler)
{
geom::Quaterniond p = geom::quaterniond(geom::Vector3d{5.037992718099102, 6.212303632611285, 1.7056797335843106}, M_PI/4.0);
geom::Quaterniond q = geom::quaterniond_from_euler(p.euler());
EXPECT_EQ(p, q);
}
TEST(Quaterniond, Identity)
{
geom::Quaterniond p(geom::Vector4d {1.0, -2.0, 1.0, 3.0});
geom::Quaterniond q;
EXPECT_EQ(p * q, p);
}
TEST(Quaterniond, Inverse)
{
geom::Quaterniond p(geom::Vector4d {3.0, 4.0, 5.0, 2.0});
geom::Quaterniond q(geom::Vector4d {-0.05556, -0.07407, -0.09259, 0.03704 });
EXPECT_EQ(p.inverse(), q);
}
TEST(Quaterniond, Norm)
{
geom::Quaterniond p(geom::Vector3d {0.9899139811480784, 9.387110042325054, 6.161341707794767},
5.563199889674063);
geom::Quaterniond p(geom::Vector4d {0.9899139811480784, 9.387110042325054, 6.161341707794767,
5.563199889674063});
double norm = 12.57016663729933;
EXPECT_DOUBLE_EQ(p.norm(), norm);
@@ -30,28 +67,64 @@ TEST(Quaterniond, Norm)
TEST(Quaterniond, Product)
{
geom::Quaterniond p(geom::Vector3d {1.0, -2.0, 1.0}, 3.0);
geom::Quaterniond q(geom::Vector3d {-1.0, 2.0, 3.0}, 2.0);
geom::Quaterniond expected(geom::Vector3d{-9.0, -2.0, 11.0}, 8.0);
geom::Quaterniond p(geom::Vector4d {1.0, -2.0, 1.0, 3.0});
geom::Quaterniond q(geom::Vector4d {-1.0, 2.0, 3.0, 2.0});
geom::Quaterniond expected(geom::Vector4d{-9.0, -2.0, 11.0, 8.0});
EXPECT_EQ(p * q, expected);
}
TEST(Quaterniond, Identity)
TEST(Quaterniond, Rotate)
{
geom::Quaterniond p(geom::Vector3d {1.0, -2.0, 1.0}, 3.0);
geom::Quaterniond q;
// This test aims to rotate a vector v using a quaternion.
// c.f. https://math.stackexchange.com/questions/40164/how-do-you-rotate-a-vector-by-a-unit-quaternion
// If we assume a standard IMU frame of reference following the
// right hand rule:
// + The x axis points toward magnetic north
// + The y axis points toward magnentic west
// + The z axis points toward the sky
// Given a vector pointing due north, rotating by 90º about
// the y-axis should leave us pointing toward the sky.
EXPECT_EQ(p * q, p);
geom::Vector3d v {1.0, 0.0, 0.0}; // a vector pointed north
geom::Vector3d yAxis {0.0, 1.0, 0.0}; // a vector representing the y axis.
double angle = M_PI / 2; // 90º rotation
// A quaternion representing a 90º rotation about the y axis.
geom::Quaterniond p = geom::quaterniond(yAxis, angle);
geom::Vector3d vr {0.0, 0.0, 1.0}; // expected rotated vector.
// A rotation quaternion should be a unit quaternion.
EXPECT_TRUE(p.isUnitQuaternion());
EXPECT_EQ(p.rotate(v), vr);
}
TEST(Quaterniond, Unit)
{
geom::Quaterniond q(geom::Vector4d{0.5773502691896258, 0.5773502691896258, 0.5773502691896258, 0.0});
EXPECT_TRUE(q.isUnitQuaternion());
}
TEST(Quaterniond, UtilityCreator)
{
geom::Vector3d v {1.0, 1.0, 1.0};
double w = M_PI;
geom::Quaterniond p = geom::quaterniond(v, w);
geom::Quaterniond q(geom::Vector4d{0.5773502691896258, 0.5773502691896258, 0.5773502691896258, 0.0});
EXPECT_EQ(p, q);
}
TEST(Quaternionf, Addition)
{
geom::Quaternionf p(geom::Vector3f {1.0, -2.0, 1.0}, 3.0);
geom::Quaternionf q(geom::Vector3f {-1.0, 2.0, 3.0}, 2.0);
geom::Quaternionf expected(geom::Vector3f{0.0, 0.0, 4.0}, 5.0);
geom::Quaternionf p(geom::Vector4f {1.0, -2.0, 1.0, 3.0});
geom::Quaternionf q(geom::Vector4f {-1.0, 2.0, 3.0, 2.0});
geom::Quaternionf expected(geom::Vector4f{0.0, 0.0, 4.0, 5.0});
EXPECT_EQ(p + q, expected);
EXPECT_EQ(expected - q, p);
@@ -59,10 +132,48 @@ TEST(Quaternionf, Addition)
}
TEST(Quaternionf, Conjugate)
{
geom::Quaternionf p(geom::Vector4f {3.0, 4.0, 5.0, 2.0});
geom::Quaternionf q(geom::Vector4f {-3.0, -4.0, -5.0, 2.0});
EXPECT_EQ(p.conjugate(), q);
}
TEST(Quaternionf, Euler)
{
geom::Quaternionf p = geom::quaternionf(geom::Vector3f{5.037992718099102, 6.212303632611285, 1.7056797335843106}, M_PI/4.0);
geom::Quaternionf q = geom::quaternionf_from_euler(p.euler());
EXPECT_EQ(p, q);
}
TEST(Quaternionf, Identity)
{
geom::Quaternionf p(geom::Vector4f {1.0, -2.0, 1.0, 3.0});
geom::Quaternionf q;
EXPECT_EQ(p * q, p);
}
TEST(Quaternionf, Inverse)
{
geom::Quaternionf p(geom::Vector4f {3.0, 4.0, 5.0, 2.0});
geom::Quaternionf q(geom::Vector4f {-0.05556, -0.07407, -0.09259, 0.03704 });
EXPECT_EQ(p.inverse(), q);
}
TEST(Quaternionf, Norm)
{
geom::Quaternionf p(geom::Vector3f {0.9899139811480784, 9.387110042325054, 6.161341707794767},
5.563199889674063);
geom::Quaternionf p(geom::Vector4f {0.9899139811480784,
9.387110042325054,
6.161341707794767,
5.563199889674063});
float norm = 12.57016663729933;
EXPECT_FLOAT_EQ(p.norm(), norm);
@@ -71,28 +182,53 @@ TEST(Quaternionf, Norm)
TEST(Quaternionf, Product)
{
geom::Quaternionf p(geom::Vector3f {1.0, -2.0, 1.0}, 3.0);
geom::Quaternionf q(geom::Vector3f {-1.0, 2.0, 3.0}, 2.0);
geom::Quaternionf expected(geom::Vector3f{-9.0, -2.0, 11.0}, 8.0);
geom::Quaternionf p(geom::Vector4f {1.0, -2.0, 1.0, 3.0});
geom::Quaternionf q(geom::Vector4f {-1.0, 2.0, 3.0, 2.0});
geom::Quaternionf expected(geom::Vector4f{-9.0, -2.0, 11.0, 8.0});
EXPECT_EQ(p * q, expected);
}
TEST(Quaternionf, Identity)
TEST(Quaternionf, Rotate)
{
geom::Quaternionf p(geom::Vector3f {1.0, -2.0, 1.0}, 3.0);
geom::Quaternionf q;
geom::Vector3f v {1.0, 0.0, 0.0};
geom::Vector3f yAxis {0.0, 1.0, 0.0};
float angle = M_PI / 2;
EXPECT_EQ(p * q, p);
geom::Quaternionf p = geom::quaternionf(yAxis, angle);
geom::Vector3f vr {0.0, 0.0, 1.0};
EXPECT_TRUE(p.isUnitQuaternion());
EXPECT_EQ(p.rotate(v), vr);
}
TEST(Quaternionf, Unit)
{
geom::Quaternionf q(geom::Vector4f{0.5773502691896258, 0.5773502691896258, 0.5773502691896258, 0.0});
EXPECT_TRUE(q.isUnitQuaternion());
}
TEST(Quaternionf, UtilityCreator)
{
geom::Vector3f v {1.0, 1.0, 1.0};
float w = M_PI;
geom::Quaternionf p = geom::quaternionf(v, w);
geom::Quaternionf q(geom::Vector4f{0.5773502691896258, 0.5773502691896258, 0.5773502691896258, 0.0});
EXPECT_EQ(p, q);
}
TEST(QuaternionMiscellaneous, SanityChecks)
{
geom::Vector4d q {1.0, 2.0, 3.0, 4.0};
geom::Vector3d v {1.0, 2.0, 3.0};
double w = 4.0;
geom::Quaterniond p(v, w);
geom::Quaterniond p(q);
EXPECT_EQ(p.axis(), v);
EXPECT_DOUBLE_EQ(p.angle(), w);
@@ -101,8 +237,8 @@ TEST(QuaternionMiscellaneous, SanityChecks)
TEST(QuaternionMiscellaneous, OutputStream)
{
geom::Quaternionf p(geom::Vector3f {1.0, 2.0, 3.0}, 4.0);
geom::Quaterniond q(geom::Vector3d {1.0, 2.0, 3.0}, 4.0);
geom::Quaternionf p(geom::Vector4f {1.0, 2.0, 3.0, 4.0});
geom::Quaterniond q(geom::Vector4d {1.0, 2.0, 3.0, 4.0});
stringstream ss;
ss << p;

8
test/vector_test.cc
View File

@@ -109,10 +109,12 @@ TEST(Vector3FloatTests, UnitVector)
// Test values randomly generated and calculated with numpy.
geom::Vector3f vec3 {5.320264018493507, 5.6541812891273935, 1.9233435162644652};
geom::Vector3f unit {0.6651669556972103, 0.7069150218815566, 0.24046636539587804};
geom::Vector3f unit2;
EXPECT_EQ(vec3.unitVector(), unit);
EXPECT_FALSE(vec3.isUnitVector());
EXPECT_TRUE(unit.isUnitVector());
EXPECT_TRUE(unit2.isUnitVector());
}
@@ -135,7 +137,7 @@ TEST(Vector3FloatTests, ParallelOrthogonalVectors)
geom::Vector3f d {-1.821, 1.072, -2.94};
geom::Vector3f e {-2.0, 1.0, 3.0};
geom::Vector3f f {-6.0, 3.0, 9.0};
geom::Vector3f zeroVector;
geom::Vector3f zeroVector {0.0, 0.0, 0.0};
EXPECT_FALSE(a.isParallel(b));
EXPECT_FALSE(a.isOrthogonal(b));
@@ -249,10 +251,12 @@ TEST(Vector3DoubleTests, UnitVector)
// Test values randomly generated and calculated with numpy.
geom::Vector3d vec3 {5.320264018493507, 5.6541812891273935, 1.9233435162644652};
geom::Vector3d unit {0.6651669556972103, 0.7069150218815566, 0.24046636539587804};
geom::Vector3d unit2;
EXPECT_EQ(vec3.unitVector(), unit);
EXPECT_FALSE(vec3.isUnitVector());
EXPECT_TRUE(unit.isUnitVector());
EXPECT_TRUE(unit2.isUnitVector());
}
@@ -276,7 +280,7 @@ TEST(Vector3DoubleTests, ParallelOrthogonalVectors)
geom::Vector3d d {-1.821, 1.072, -2.94};
geom::Vector3d e {-2.0, 1.0, 3.0};
geom::Vector3d f {-6.0, 3.0, 9.0};
geom::Vector3d zeroVector;
geom::Vector3d zeroVector {0.0, 0.0, 0.0};
EXPECT_FALSE(a.isParallel(b));
EXPECT_FALSE(a.isOrthogonal(b));