quaternion <-> euler, lots of fixes.

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 lcov -d . -t wrmath -o wrmath.info -c
-						   COMMAND genhtml -o coverage-report wrmath.info)
+			   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()
 
 

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

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)

docs/sphinx/api.rst

@@ -0,0 +1,4 @@
+wrmath API
+==========
+
+.. doxygenindex::

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'

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`

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

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 <typename T>
 class Quaternion {
 public:
 	/**
 	 * The default Quaternion constructor returns an identity quaternion.
 	 */
-	Quaternion() : v(Vector<T, 3>()), w(1.0)
+	Quaternion() : v(Vector<T, 3> {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<T, 3> 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<T, 3> _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<T, 4> {this->v[0], this->v[1], this->v[2], this->w})
+		return Quaternion(Vector<T, 4> {-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<T, 4>, with the axis of rotation
 	 * followed by the angle of rotation.
 	 * @return A vector representation of the quaternion.
 	 */
 	Vector<T, 4>
-	asVector()
+	asVector() const
 	{
 		return Vector<T, 4> {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<T, 3>
+	rotate(Vector<T, 3> v) const
+	{
+		return (this->conjugate() * v * (*this)).axis();
+	}
+
+
+	/**
+	 * Return the Euler angles for this quaternion as a vector of
+	 * <yaw, pitch, roll>. Users of this function should watch out
+	 * for gimball lock.
+	 * @return A vector<T, 3> containing <yaw, pitch, roll>
+	 */
+	Vector<T, 3>
+	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<T, 3> {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<T> &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<T> &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<T, 3> &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<T> &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<T> &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<T> &other) const
 	{
@@ -156,6 +312,13 @@ public:
 	}
 
 
+	/**
+	 * Support stream output of a quaternion in the form `a + <i, j, k>`.
+	 * 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<T>& q)
 	{
@@ -164,18 +327,128 @@ public:
 	}
 
 private:
+	static constexpr T	minRotation = -4 * M_PI;
 	static constexpr T	maxRotation = 4 * M_PI;
 
 	Vector<T, 3>	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<float>	Quaternionf;
 typedef Quaternion<double>	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
 

include/wrmath/geom/vector.h

@@ -12,6 +12,11 @@
 #include <wrmath/math.h>
 
 
+// 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 <typename T, size_t N>
 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);
 	}
 
 	/**

test/quaternion_test.cc

@@ -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	p(geom::Vector4d {1.0, -2.0, 1.0, 3.0});
-	geom::Quaterniond	q(geom::Vector3d {-1.0, 2.0, 3.0}, 2.0);
+	geom::Quaterniond	q(geom::Vector4d {-1.0, 2.0, 3.0, 2.0});
-	geom::Quaterniond	expected(geom::Vector3d{0.0, 0.0, 4.0}, 5.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},
+	geom::Quaterniond	p(geom::Vector4d {0.9899139811480784, 9.387110042325054, 6.161341707794767,
-			   5.563199889674063);
+						  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	p(geom::Vector4d {1.0, -2.0, 1.0, 3.0});
-	geom::Quaterniond	q(geom::Vector3d {-1.0, 2.0, 3.0}, 2.0);
+	geom::Quaterniond	q(geom::Vector4d {-1.0, 2.0, 3.0, 2.0});
-	geom::Quaterniond	expected(geom::Vector3d{-9.0, -2.0, 11.0}, 8.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);
+	// This test aims to rotate a vector v using a quaternion.
-	geom::Quaterniond	q;
+	// 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	p(geom::Vector4f {1.0, -2.0, 1.0, 3.0});
-	geom::Quaternionf	q(geom::Vector3f {-1.0, 2.0, 3.0}, 2.0);
+	geom::Quaternionf	q(geom::Vector4f {-1.0, 2.0, 3.0, 2.0});
-	geom::Quaternionf	expected(geom::Vector3f{0.0, 0.0, 4.0}, 5.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},
+	geom::Quaternionf	p(geom::Vector4f {0.9899139811480784,
-				   5.563199889674063);
+						  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	p(geom::Vector4f {1.0, -2.0, 1.0, 3.0});
-	geom::Quaternionf	q(geom::Vector3f {-1.0, 2.0, 3.0}, 2.0);
+	geom::Quaternionf	q(geom::Vector4f {-1.0, 2.0, 3.0, 2.0});
-	geom::Quaternionf	expected(geom::Vector3f{-9.0, -2.0, 11.0}, 8.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::Vector3f		v {1.0, 0.0, 0.0};
-	geom::Quaternionf	q;
+	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::Quaternionf	p(geom::Vector4f {1.0, 2.0, 3.0, 4.0});
-	geom::Quaterniond	q(geom::Vector3d {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;

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));