From 3a9d614010f1bcb98fc79c8d6f2ac7e8c8c1286b Mon Sep 17 00:00:00 2001 From: Kyle Isom Date: Tue, 6 Aug 2019 01:03:30 +0000 Subject: [PATCH] quaternion <-> euler, lots of fixes. --- CMakeLists.txt | 10 +- docs/Doxyfile | 4 +- docs/sphinx/Makefile | 20 ++ docs/sphinx/api.rst | 4 + docs/sphinx/conf.py | 60 ++++++ docs/sphinx/index.rst | 21 +++ docs/sphinx/make.bat | 35 ++++ docs/{ => sphinx}/requirements.txt | 0 include/wrmath/geom/quaternion.h | 293 ++++++++++++++++++++++++++++- include/wrmath/geom/vector.h | 13 +- test/quaternion_test.cc | 190 ++++++++++++++++--- test/vector_test.cc | 8 +- 12 files changed, 611 insertions(+), 47 deletions(-) create mode 100644 docs/sphinx/Makefile create mode 100644 docs/sphinx/api.rst create mode 100644 docs/sphinx/conf.py create mode 100644 docs/sphinx/index.rst create mode 100644 docs/sphinx/make.bat rename docs/{ => sphinx}/requirements.txt (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index be28108..f0a6da9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,15 +8,19 @@ project(wrmath VERSION 0.0.1 LANGUAGES CXX) set(CMAKE_CXX_STANDARD 11) set(CMAKE_CXX_STANDARD_REQUIRED ON) -add_compile_options(-Werror -Wall -g -O0) +# Don't warn on unused functions, because this is a library and not all +# functions might be used. +add_compile_options(-Werror -Wno-unused-function -Wall -g -O0) if(DEFINED ENV{CMAKE_GCOV}) add_compile_options(-fprofile-arcs -ftest-coverage) # Need CMake 3.15+. add_link_options(-fprofile-arcs -ftest-coverage) add_custom_target(coverage COMMAND lcov -d . -t wrmath -o wrmath.info -c -i - COMMAND lcov -d . -t wrmath -o wrmath.info -c - COMMAND genhtml -o coverage-report wrmath.info) + COMMAND lcov -d . -t wrmath -o wrmath.info -c + COMMAND lcov -d . -t wrmath -o wrmath.info -r wrmath.info *gtest* + COMMAND genhtml -o coverage-report wrmath.info) +message(STATUS, "Code coverage enabled.") endif() diff --git a/docs/Doxyfile b/docs/Doxyfile index 696740a..da1ad62 100644 --- a/docs/Doxyfile +++ b/docs/Doxyfile @@ -873,7 +873,7 @@ RECURSIVE = YES # Note that relative paths are relative to the directory from which doxygen is # run. -EXCLUDE = ../build/ ../cmake-debug-build/ ../extern/ +EXCLUDE = ../build/ ../cmake-debug-build/ ../extern/ ../test/ # The EXCLUDE_SYMLINKS tag can be used to select whether or not files or # directories that are symbolic links (a Unix file system feature) are excluded @@ -1946,7 +1946,7 @@ MAN_LINKS = NO # captures the structure of the code including all documentation. # The default value is: NO. -GENERATE_XML = NO +GENERATE_XML = YES # The XML_OUTPUT tag is used to specify where the XML pages will be put. If a # relative path is entered the value of OUTPUT_DIRECTORY will be put in front of diff --git a/docs/sphinx/Makefile b/docs/sphinx/Makefile new file mode 100644 index 0000000..d4bb2cb --- /dev/null +++ b/docs/sphinx/Makefile @@ -0,0 +1,20 @@ +# Minimal makefile for Sphinx documentation +# + +# You can set these variables from the command line, and also +# from the environment for the first two. +SPHINXOPTS ?= +SPHINXBUILD ?= sphinx-build +SOURCEDIR = . +BUILDDIR = _build + +# Put it first so that "make" without argument is like "make help". +help: + @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) + +.PHONY: help Makefile + +# Catch-all target: route all unknown targets to Sphinx using the new +# "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). +%: Makefile + @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) diff --git a/docs/sphinx/api.rst b/docs/sphinx/api.rst new file mode 100644 index 0000000..8f5ac19 --- /dev/null +++ b/docs/sphinx/api.rst @@ -0,0 +1,4 @@ +wrmath API +========== + +.. doxygenindex:: diff --git a/docs/sphinx/conf.py b/docs/sphinx/conf.py new file mode 100644 index 0000000..c9dca21 --- /dev/null +++ b/docs/sphinx/conf.py @@ -0,0 +1,60 @@ +# Configuration file for the Sphinx documentation builder. +# +# This file only contains a selection of the most common options. For a full +# list see the documentation: +# http://www.sphinx-doc.org/en/master/config + +# -- Path setup -------------------------------------------------------------- + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +# +# import os +# import sys +# sys.path.insert(0, os.path.abspath('.')) + + +# -- Project information ----------------------------------------------------- + +project = 'wrmath' +copyright = '2019, K. Isom' +author = 'K. Isom' + +# The full version, including alpha/beta/rc tags +release = '0.0.1' + + +# -- General configuration --------------------------------------------------- + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = [ + 'breathe', +] + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['_templates'] + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +# This pattern also affects html_static_path and html_extra_path. +exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] + + +# -- Options for HTML output ------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +# +html_theme = 'alabaster' + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ['_static'] + +# -- Options for breathe output ---------------------------------------------- +breathe_projects = {'wrmath': '../xml/'} +breathe_default_project = 'wrmath' diff --git a/docs/sphinx/index.rst b/docs/sphinx/index.rst new file mode 100644 index 0000000..2135118 --- /dev/null +++ b/docs/sphinx/index.rst @@ -0,0 +1,21 @@ +.. wrmath documentation master file, created by + sphinx-quickstart on Mon Aug 5 19:25:35 2019. + You can adapt this file completely to your liking, but it should at least + contain the root `toctree` directive. + +Welcome to wrmath's documentation! +================================== + +.. toctree:: + :maxdepth: 2 + :caption: Contents: + + api + + +Indices and tables +================== + +* :ref:`genindex` +* :ref:`modindex` +* :ref:`search` diff --git a/docs/sphinx/make.bat b/docs/sphinx/make.bat new file mode 100644 index 0000000..2119f51 --- /dev/null +++ b/docs/sphinx/make.bat @@ -0,0 +1,35 @@ +@ECHO OFF + +pushd %~dp0 + +REM Command file for Sphinx documentation + +if "%SPHINXBUILD%" == "" ( + set SPHINXBUILD=sphinx-build +) +set SOURCEDIR=. +set BUILDDIR=_build + +if "%1" == "" goto help + +%SPHINXBUILD% >NUL 2>NUL +if errorlevel 9009 ( + echo. + echo.The 'sphinx-build' command was not found. Make sure you have Sphinx + echo.installed, then set the SPHINXBUILD environment variable to point + echo.to the full path of the 'sphinx-build' executable. Alternatively you + echo.may add the Sphinx directory to PATH. + echo. + echo.If you don't have Sphinx installed, grab it from + echo.http://sphinx-doc.org/ + exit /b 1 +) + +%SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% +goto end + +:help +%SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% + +:end +popd diff --git a/docs/requirements.txt b/docs/sphinx/requirements.txt similarity index 100% rename from docs/requirements.txt rename to docs/sphinx/requirements.txt diff --git a/include/wrmath/geom/quaternion.h b/include/wrmath/geom/quaternion.h index 37470e0..6a3c441 100644 --- a/include/wrmath/geom/quaternion.h +++ b/include/wrmath/geom/quaternion.h @@ -13,29 +13,50 @@ namespace wr { namespace geom { +/** + * Quaternions encode rotations in three-dimensional space. While technically + * a quaternion is comprised of a real element and a complex vector<3>, for + * the purposes of this library, it is modeled as a floating point 4D vector. + * + * For information on the underlying vector type, see the documentation for + * wr::geom::Vector. + * + * The constructors are primarily intended for intended operations; in practice, + * the quaternionf and quaterniond functions are more useful for constructing + * quaternions from vectors and angles. + * + * Like vectors, quaternions carry an internal tolerance value ε that is used for + * floating point comparisons. The wr::math namespace contains the default values + * used for this; generally, a tolerance of 0.0001 is considered appropriate for + * the uses of this library. The tolerance can be explicitly set with the + * setEpsilon method. + */ template class Quaternion { public: /** * The default Quaternion constructor returns an identity quaternion. */ - Quaternion() : v(Vector()), w(1.0) + Quaternion() : v(Vector {0.0, 0.0, 0.0}), w(1.0) { wr::math::DefaultEpsilon(this->eps); - this->w = std::fmod(this->w, this->maxRotation); + v.setEpsilon(this->eps); + this->constrainAngle(); }; /** * A Quaternion may be initialised with a Vector axis of rotation - * and an angle of rotation. + * and an angle of rotation. This doesn't do the angle transforms to simplify + * internal operations. * @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); + this->constrainAngle(); + v.setEpsilon(this->eps); }; /** @@ -48,9 +69,23 @@ public: w(vector[3]) { wr::math::DefaultEpsilon(this->eps); - this->w = std::fmod(this->w, this->m + this->constrainAngle(); + v.setEpsilon(this->eps); } + + /** + * Set the comparison tolerance for this quaternion. + * @param epsilon A tolerance value. + */ + void + setEpsilon(T epsilon) + { + this->eps = epsilon; + this->v.setEpsilon(epsilon); + } + + /** * Return the axis of rotation of this quaternion. * @return The axis of rotation of this quaternion. @@ -79,7 +114,7 @@ public: * @return A non-negative real number. */ T - norm() + norm() const { T n = 0; @@ -92,28 +127,91 @@ public: } + /** + * Compute the conjugate of a quaternion. + * @return The conjugate of this quaternion. + */ Quaternion - complexConj() + conjugate() const { - return Quaternion(Vector {this->v[0], this->v[1], this->v[2], this->w}) + return Quaternion(Vector {-this->v[0], -this->v[1], -this->v[2], this->w}); } + + /** + * Compute the inverse of a quaternion. + * @return The inverse of this quaternion. + */ + Quaternion + inverse() const + { + T _norm = this->norm(); + + return this->conjugate() / (_norm * _norm); + } + + + /** + * Determine whether this is a unit quaternion. + * @return true if this is a unit quaternion. + */ + bool + isUnitQuaternion() const + { + return wr::math::WithinTolerance(this->norm(), (T)1.0, this->eps); + } + + /** * Return the quaternion as a Vector, with the axis of rotation * followed by the angle of rotation. * @return A vector representation of the quaternion. */ Vector - asVector() + asVector() const { return Vector {this->v[0], this->v[1], this->v[2], this->w}; } + /** + * Rotate vector v about this quaternion. + * @param v The vector to be rotated. + * @return The rotated vector. + */ + Vector + rotate(Vector v) const + { + return (this->conjugate() * v * (*this)).axis(); + } + + + /** + * Return the Euler angles for this quaternion as a vector of + * . Users of this function should watch out + * for gimball lock. + * @return A vector containing + */ + Vector + euler() const + { + T yaw, pitch, roll; + T a = this->w, a2 = a * a; + T b = this->v[0], b2 = b * b; + T c = this->v[1], c2 = c * c; + T d = this->v[2], d2 = d * d; + + yaw = std::atan2(2 * ((a*b) + (c * d)), a2 - b2 - c2 + d2); + pitch = std::asin(2 * ((b*d) - (a*c))); + roll = std::atan2(2 * ((a * d) + (b * c)), a2 + b2 - c2 - d2); + + return Vector {yaw, pitch, roll}; + } + /** * Perform quaternion addition with another quaternion. * @param other The quaternion to be added with this one. - * @return + * @return The result of adding the two quaternions together. */ Quaternion operator+(const Quaternion &other) const @@ -122,6 +220,11 @@ public: } + /** + * Perform quaternion subtraction with another quaternion. + * @param other The quaternion to be subtracted from this one. + * @return The result of subtracting the other quaternion from this one. + */ Quaternion operator-(const Quaternion &other) const { @@ -129,6 +232,49 @@ public: } + /** + * Perform scalar multiplication. + * @param k The scaling value. + * @return A scaled quaternion. + */ + Quaternion + operator*(const T k) const + { + return Quaternion(this->v * k, this->w * k); + } + + + /** Perform scalar division. + * @param k The scalar divisor. + * @return A scaled quaternion. + */ + Quaternion + operator/(const T k) const + { + return Quaternion(this->v / k, this->w / k); + } + + + /** + * Perform quaternion Hamilton multiplication with a three- + * dimensional vector; this is done by treating the vector + * as a pure quaternion (e.g. with an angle of rotation of 0). + * @param vector The vector to multiply with this quaternion. + * @return The Hamilton product of the quaternion and vector. + */ + Quaternion + operator*(const Vector &vector) const + { + return Quaternion(vector * this->w + this->v.cross(vector), + (T)0.0); + } + + + /** + * Perform quaternion Hamilton multiplication. + * @param other The other quaternion to multiply with this one. + * @result The Hamilton product of the two quaternions. + */ Quaternion operator*(const Quaternion &other) const { @@ -141,6 +287,11 @@ public: } + /** + * Perform quaternion equality checking. + * @param other The quaternion to check equality against. + * @return True if the two quaternions are equal within their tolerance. + */ bool operator==(const Quaternion &other) const { @@ -149,6 +300,11 @@ public: } + /** + * Perform quaternion inequality checking. + * @param other The quaternion to check inequality against. + * @return True if the two quaternions are unequal within their tolerance. + */ bool operator!=(const Quaternion &other) const { @@ -156,6 +312,13 @@ public: } + /** + * Support stream output of a quaternion in the form `a + `. + * TODO: improve the formatting. + * @param outs An output stream + * @param q A quaternion + * @return The output stream + */ friend std::ostream& operator<<(std::ostream& outs, const Quaternion& q) { @@ -164,18 +327,128 @@ public: } private: + static constexpr T minRotation = -4 * M_PI; static constexpr T maxRotation = 4 * M_PI; Vector v; // axis of rotation T w; // angle of rotation T eps; + + void + constrainAngle() + { + if (this->w < 0.0) { + this->w = std::fmod(this->w, this->minRotation); + } + else { + this->w = std::fmod(this->w, this->maxRotation); + } + } }; +/** + * Type aliases are provided for float and double quaternions. + */ typedef Quaternion Quaternionf; typedef Quaternion Quaterniond; +/** + * Return a float quaternion scaled appropriately from a vector and angle, + * e.g. angle = cos(angle / 2), axis.unitVector() * sin(angle / 2). + * @param axis The axis of rotation. + * @param angle The angle of rotation. + * @return A quaternion. + */ +static Quaternionf +quaternionf(Vector3f axis, float angle) +{ + return Quaternionf(axis.unitVector() * std::sin(angle / 2.0), + std::cos(angle / 2.0)); +} + + +/** + * Return a double quaternion scaled appropriately from a vector and angle, + * e.g. angle = cos(angle / 2), axis.unitVector() * sin(angle / 2). + * @param axis The axis of rotation. + * @param angle The angle of rotation. + * @return A quaternion. + */ +static Quaterniond +quaterniond(Vector3d axis, double angle) +{ + return Quaterniond(axis.unitVector() * std::sin(angle / 2.0), + std::cos(angle / 2.0)); +} + + +/** + * Given a vector of Euler angles in ZYX sequence (e.g. yaw, pitch, roll), + * return a quaternion. + * @param euler A vector Euler angle in ZYX sequence. + * @return A Quaternion representation of the orientation represented + * by the Euler angles. + */ +static Quaternionf +quaternionf_from_euler(Vector3f euler) +{ + float x, y, z, w; + euler = euler / 2.0; + + float cos_yaw = std::cos(euler[0]); + float cos_pitch = std::cos(euler[1]); + float cos_roll = std::cos(euler[2]); + float sin_yaw = std::sin(euler[0]); + float sin_pitch = std::sin(euler[1]); + float sin_roll = std::sin(euler[2]); + + x = (sin_yaw * cos_pitch * cos_roll) + (cos_yaw * sin_pitch * sin_roll); + y = (sin_yaw * cos_pitch * sin_roll) - (cos_yaw * sin_pitch * cos_roll); + z = (cos_yaw * cos_pitch * sin_roll) + (sin_yaw * sin_pitch * cos_roll); + w = (cos_yaw * cos_pitch * cos_roll) - (sin_yaw * sin_pitch * sin_roll); + + return Quaternionf(Vector4f {x, y, z, w}); +} + + +/** + * Given a vector of Euler angles in ZYX sequence (e.g. yaw, pitch, roll), + * return a quaternion. + * @param euler A vector Euler angle in ZYX sequence. + * @return A Quaternion representation of the orientation represented + * by the Euler angles. + */ +static Quaterniond +quaterniond_from_euler(Vector3d euler) +{ + double x, y, z, w; + euler = euler / 2.0; + + double cos_yaw = std::cos(euler[0]); + double cos_pitch = std::cos(euler[1]); + double cos_roll = std::cos(euler[2]); + double sin_yaw = std::sin(euler[0]); + double sin_pitch = std::sin(euler[1]); + double sin_roll = std::sin(euler[2]); + + x = (sin_yaw * cos_pitch * cos_roll) + (cos_yaw * sin_pitch * sin_roll); + y = (sin_yaw * cos_pitch * sin_roll) - (cos_yaw * sin_pitch * cos_roll); + z = (cos_yaw * cos_pitch * sin_roll) + (sin_yaw * sin_pitch * cos_roll); + w = (cos_yaw * cos_pitch * cos_roll) - (sin_yaw * sin_pitch * sin_roll); + + return Quaterniond(Vector4d {x, y, z, w}); +} + + +// Helpful references for understanding quaternions: +// + "Intro to Quaternions" - https://www.youtube.com/watch?v=fKIss4EV6ME +// 15 minutes into this video I had a more intuitive understanding. +// + "Quaternions and Rotations" - http://graphics.stanford.edu/courses/cs348a-17-winter/Papers/quaternion.pdf +// + "Understanding Quaternions" - http://www.chrobotics.com/library/understanding-quaternions + + } // namespace geom } // namespace wr diff --git a/include/wrmath/geom/vector.h b/include/wrmath/geom/vector.h index 5169513..c11c913 100644 --- a/include/wrmath/geom/vector.h +++ b/include/wrmath/geom/vector.h @@ -12,6 +12,11 @@ #include +// This implementation is essentially a C++ translation of a Python library +// I wrote for Coursera's "Linear Algebra for Machine Learning" course. Many +// of the test vectors come from quiz questions in the class. + + namespace wr { namespace geom { @@ -27,15 +32,17 @@ template class Vector { public: /** - * The default constructor creates a zero vector for a given + * The default constructor creates a unit vector for a given * type and size. */ Vector() { - wr::math::DefaultEpsilon(this->epsilon); + T unitLength = (T)1.0 / std::sqrt(N); for (size_t i = 0; i < N; i++) { - this->arr[i] = 0.0; + this->arr[i] = unitLength; } + + wr::math::DefaultEpsilon(this->epsilon); } /** diff --git a/test/quaternion_test.cc b/test/quaternion_test.cc index 4bcd967..56329af 100644 --- a/test/quaternion_test.cc +++ b/test/quaternion_test.cc @@ -1,3 +1,4 @@ +#include #include #include #include @@ -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; diff --git a/test/vector_test.cc b/test/vector_test.cc index 6fd2d10..fdd0ff8 100644 --- a/test/vector_test.cc +++ b/test/vector_test.cc @@ -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));