From de1c4e6109208393fbdec70a42594038fda239d4 Mon Sep 17 00:00:00 2001 From: Kyle Isom Date: Mon, 5 Aug 2019 09:46:08 -0700 Subject: [PATCH] More work on quaternions. --- include/wrmath/geom/quaternion.h | 81 ++++++++++++++++++++++++++++++-- test/quaternion_test.cc | 78 ++++++++++++++++++++++++++++++ test/vector_test.cc | 40 ++++------------ 3 files changed, 164 insertions(+), 35 deletions(-) diff --git a/include/wrmath/geom/quaternion.h b/include/wrmath/geom/quaternion.h index 33ca3f9..8e86df9 100644 --- a/include/wrmath/geom/quaternion.h +++ b/include/wrmath/geom/quaternion.h @@ -4,7 +4,7 @@ #include #include -#include +#include #include #include @@ -16,30 +16,93 @@ namespace geom { template class Quaternion { public: - // The default constructor returns the identity quaternion. + /** + * The default Quaternion constructor returns an identity quaternion. + */ Quaternion() : v(Vector()), w(1.0) { wr::math::DefaultEpsilon(this->eps); + this->w = std::fmod(this->w, this->maxRotation); }; - Quaternion(Vector axis, T angle) : v(axis), w(angle) + /** + * A Quaternion may be initialised with a Vector axis of rotation + * and an angle of rotation. + * @param _axis A three-dimensional vector of the same type as the Quaternion. + * @param _angle The angle of rotation about the axis of rotation. + */ + Quaternion(Vector _axis, T _angle) : v(_axis), w(_angle) { wr::math::DefaultEpsilon(this->eps); + this->w = std::fmod(this->w, this->maxRotation); }; + /** + * Return the axis of rotation of this quaternion. + * @return The axis of rotation of this quaternion. + */ + Vector + axis() const + { + return this->v; + } + + + /** + * Return the angle of rotation of this quaternion. + * @return the angle of rotation of this quaternion. + */ + T + angle() const + { + return this->w; + } + + + /** + * Compute the norm of a quaternion. Treating the Quaternion as a + * Vector, it's the same as computing the magnitude. + * @return A non-negative real number. + */ + T + norm() + { + T n = 0; + + n += (this->v[0] * this->v[0]); + n += (this->v[1] * this->v[1]); + n += (this->v[2] * this->v[2]); + n += (this->w * this->w); + + return std::sqrt(n); + } + + + Vector + asVector() + { + return Vector {this->v[0], this->v[1], this->v[2], this->w}; + } + + + /** + * Perform quaternion addition with another quaternion. + * @param other The quaternion to be added with this one. + * @return + */ Quaternion operator+(const Quaternion &other) const { - return Quaternion(this->v + other.v, (this->w + other.w) % this->maxRotation); + return Quaternion(this->v + other.v, this->w + other.w); } Quaternion operator-(const Quaternion &other) const { - return Quaternion(this->v - other.v, (this->w - other.w) % this->maxRotation); + return Quaternion(this->v - other.v, this->w - other.w); } @@ -69,6 +132,14 @@ public: return !(*this == other); } + + friend std::ostream& + operator<<(std::ostream& outs, const Quaternion& q) + { + outs << q.w << " + " << q.v; + return outs; + } + private: static constexpr T maxRotation = 4 * M_PI; diff --git a/test/quaternion_test.cc b/test/quaternion_test.cc index 9b0ee18..94713aa 100644 --- a/test/quaternion_test.cc +++ b/test/quaternion_test.cc @@ -1,3 +1,4 @@ +#include #include #include @@ -6,6 +7,28 @@ 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); + + EXPECT_EQ(p + q, expected); + EXPECT_EQ(expected - q, p); + EXPECT_NE(expected - q, q); // exercise != +} + + +TEST(Quaterniond, Norm) +{ + geom::Quaterniond p(geom::Vector3d {0.9899139811480784, 9.387110042325054, 6.161341707794767}, + 5.563199889674063); + double norm = 12.57016663729933; + + EXPECT_DOUBLE_EQ(p.norm(), 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); @@ -15,6 +38,61 @@ TEST(Quaterniond, Addition) } +TEST(Quaterniond, Identity) +{ + geom::Quaterniond p(geom::Vector3d {1.0, -2.0, 1.0}, 3.0); + geom::Quaterniond q; + + EXPECT_EQ(p * q, p); +} + + +TEST(Quaternionf, Norm) +{ + geom::Quaternionf p(geom::Vector3f {0.9899139811480784, 9.387110042325054, 6.161341707794767}, + 5.563199889674063); + float norm = 12.57016663729933; + + EXPECT_DOUBLE_EQ(p.norm(), 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); + + EXPECT_EQ(p * q, expected); +} + + +TEST(QuaternionMiscellaneous, SanityChecks) +{ + geom::Vector3d v {1.0, 2.0, 3.0}; + double w = 4.0; + geom::Quaterniond p(v, w); + + EXPECT_EQ(p.axis(), v); + EXPECT_DOUBLE_EQ(p.angle(), w); +} + + +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); + stringstream ss; + + ss << p; + EXPECT_EQ(ss.str(), "4 + <1, 2, 3>"); + ss.str(""); + + ss << q; + EXPECT_EQ(ss.str(), "4 + <1, 2, 3>"); +} + + int main(int argc, char **argv) { diff --git a/test/vector_test.cc b/test/vector_test.cc index e880558..6fd2d10 100644 --- a/test/vector_test.cc +++ b/test/vector_test.cc @@ -26,26 +26,6 @@ TEST(Vector3Miscellaneous, ExtractionOperator3f) } -TEST(Vector3Miscellaneous, ExtractionOperator4d) -{ - geom::Vector4d vec {1.0, 2.0, 3.0, 4.0}; - stringstream vecBuffer; - - vecBuffer << vec; - EXPECT_EQ(vecBuffer.str(), "<1, 2, 3, 4>"); -} - - -TEST(Vector3Miscellaneous, ExtractionOperator4f) -{ - geom::Vector4f vec {1.0, 2.0, 3.0, 4.0}; - stringstream vecBuffer; - - vecBuffer << vec; - EXPECT_EQ(vecBuffer.str(), "<1, 2, 3, 4>"); -} - - TEST(Vector3Miscellaneous, SetEpsilon) { geom::Vector3f a {1.0, 1.0, 1.0}; @@ -172,12 +152,12 @@ TEST(Vector3FloatTests, ParallelOrthogonalVectors) } -TEST(Vector4FloatTests, Projections) +TEST(Vector3FloatTests, Projections) { - geom::Vector4f a {3.009, -6.172, 3.692, -2.510}; - geom::Vector4f b {6.404, -9.144, 2.759, 8.718}; - geom::Vector4f c {1.9685, -2.8108, 0.8481, 2.6798}; - geom::Vector4f d {1.0405, -3.3612, 2.8439, -5.1898}; + geom::Vector3f a {4.866769214609107, 6.2356222686140566, 9.140878417029711}; + geom::Vector3f b {6.135533104801077, 8.757851406697895, 0.6738031370548048}; + geom::Vector3f c {4.843812341655318, 6.9140509888133055, 0.5319465962229454}; + geom::Vector3f d {0.02295687295378901, -0.6784287201992489, 8.608931820806765}; ASSERT_EQ(a.projectParallel(b), c); ASSERT_EQ(a.projectOrthogonal(b), d); @@ -313,12 +293,12 @@ TEST(Vector3DoubleTests, ParallelOrthogonalVectors) } -TEST(Vector4DoubleTests, Projections) +TEST(Vector3DoubleTests, Projections) { - geom::Vector4d a {3.009, -6.172, 3.692, -2.510}; - geom::Vector4d b {6.404, -9.144, 2.759, 8.718}; - geom::Vector4d c {1.9685, -2.8108, 0.8481, 2.6798}; - geom::Vector4d d {1.0405, -3.3612, 2.8439, -5.1898}; + geom::Vector3d a {4.866769214609107, 6.2356222686140566, 9.140878417029711}; + geom::Vector3d b {6.135533104801077, 8.757851406697895, 0.6738031370548048}; + geom::Vector3d c {4.843812341655318, 6.9140509888133055, 0.5319465962229454}; + geom::Vector3d d {0.02295687295378901, -0.6784287201992489, 8.608931820806765}; ASSERT_EQ(a.projectParallel(b), c); ASSERT_EQ(a.projectOrthogonal(b), d);