quaternion <-> euler, lots of fixes.

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

View File

@ -8,15 +8,19 @@ project(wrmath VERSION 0.0.1 LANGUAGES CXX)
set(CMAKE_CXX_STANDARD 11) set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED ON) 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}) if(DEFINED ENV{CMAKE_GCOV})
add_compile_options(-fprofile-arcs -ftest-coverage) add_compile_options(-fprofile-arcs -ftest-coverage)
# Need CMake 3.15+. # Need CMake 3.15+.
add_link_options(-fprofile-arcs -ftest-coverage) add_link_options(-fprofile-arcs -ftest-coverage)
add_custom_target(coverage COMMAND lcov -d . -t wrmath -o wrmath.info -c -i 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() endif()

View File

@ -873,7 +873,7 @@ RECURSIVE = YES
# Note that relative paths are relative to the directory from which doxygen is # Note that relative paths are relative to the directory from which doxygen is
# run. # 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 # 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 # 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. # captures the structure of the code including all documentation.
# The default value is: NO. # 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 # 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 # relative path is entered the value of OUTPUT_DIRECTORY will be put in front of

20
docs/sphinx/Makefile Normal file
View File

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

4
docs/sphinx/api.rst Normal file
View File

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

60
docs/sphinx/conf.py Normal file
View File

@ -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'

21
docs/sphinx/index.rst Normal file
View File

@ -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`

35
docs/sphinx/make.bat Normal file
View File

@ -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

View File

@ -13,29 +13,50 @@ namespace wr {
namespace geom { 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> template <typename T>
class Quaternion { class Quaternion {
public: public:
/** /**
* The default Quaternion constructor returns an identity quaternion. * 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); 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 * 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 _axis A three-dimensional vector of the same type as the Quaternion.
* @param _angle The angle of rotation about the axis of rotation. * @param _angle The angle of rotation about the axis of rotation.
*/ */
Quaternion(Vector<T, 3> _axis, T _angle) : v(_axis), w(_angle) Quaternion(Vector<T, 3> _axis, T _angle) : v(_axis), w(_angle)
{ {
wr::math::DefaultEpsilon(this->eps); 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]) w(vector[3])
{ {
wr::math::DefaultEpsilon(this->eps); 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.
* @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. * @return A non-negative real number.
*/ */
T T
norm() norm() const
{ {
T n = 0; T n = 0;
@ -92,28 +127,91 @@ public:
} }
/**
* Compute the conjugate of a quaternion.
* @return The conjugate of this quaternion.
*/
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 * Return the quaternion as a Vector<T, 4>, with the axis of rotation
* followed by the angle of rotation. * followed by the angle of rotation.
* @return A vector representation of the quaternion. * @return A vector representation of the quaternion.
*/ */
Vector<T, 4> Vector<T, 4>
asVector() asVector() const
{ {
return Vector<T, 4> {this->v[0], this->v[1], this->v[2], this->w}; 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. * Perform quaternion addition with another quaternion.
* @param other The quaternion to be added with this one. * @param other The quaternion to be added with this one.
* @return * @return The result of adding the two quaternions together.
*/ */
Quaternion Quaternion
operator+(const Quaternion<T> &other) const 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 Quaternion
operator-(const Quaternion<T> &other) const 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 Quaternion
operator*(const Quaternion<T> &other) const 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 bool
operator==(const Quaternion<T> &other) const 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 bool
operator!=(const Quaternion<T> &other) const 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& friend std::ostream&
operator<<(std::ostream& outs, const Quaternion<T>& q) operator<<(std::ostream& outs, const Quaternion<T>& q)
{ {
@ -164,18 +327,128 @@ public:
} }
private: private:
static constexpr T minRotation = -4 * M_PI;
static constexpr T maxRotation = 4 * M_PI; static constexpr T maxRotation = 4 * M_PI;
Vector<T, 3> v; // axis of rotation Vector<T, 3> v; // axis of rotation
T w; // angle of rotation T w; // angle of rotation
T eps; 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<float> Quaternionf;
typedef Quaternion<double> Quaterniond; 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 geom
} // namespace wr } // namespace wr

View File

@ -12,6 +12,11 @@
#include <wrmath/math.h> #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 wr {
namespace geom { namespace geom {
@ -27,15 +32,17 @@ template <typename T, size_t N>
class Vector { class Vector {
public: public:
/** /**
* The default constructor creates a zero vector for a given * The default constructor creates a unit vector for a given
* type and size. * type and size.
*/ */
Vector() Vector()
{ {
wr::math::DefaultEpsilon(this->epsilon); T unitLength = (T)1.0 / std::sqrt(N);
for (size_t i = 0; i < N; i++) { for (size_t i = 0; i < N; i++) {
this->arr[i] = 0.0; this->arr[i] = unitLength;
} }
wr::math::DefaultEpsilon(this->epsilon);
} }
/** /**

View File

@ -1,3 +1,4 @@
#include <cmath>
#include <sstream> #include <sstream>
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <wrmath/geom/quaternion.h> #include <wrmath/geom/quaternion.h>
@ -8,9 +9,9 @@ using namespace wr;
TEST(Quaterniond, Addition) 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(p + q, expected);
EXPECT_EQ(expected - q, p); 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) 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; double norm = 12.57016663729933;
EXPECT_DOUBLE_EQ(p.norm(), norm); EXPECT_DOUBLE_EQ(p.norm(), norm);
@ -30,28 +67,64 @@ TEST(Quaterniond, Norm)
TEST(Quaterniond, Product) 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); 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) 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(p + q, expected);
EXPECT_EQ(expected - q, p); 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) 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; float norm = 12.57016663729933;
EXPECT_FLOAT_EQ(p.norm(), norm); EXPECT_FLOAT_EQ(p.norm(), norm);
@ -71,28 +182,53 @@ TEST(Quaternionf, Norm)
TEST(Quaternionf, Product) 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); 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) TEST(QuaternionMiscellaneous, SanityChecks)
{ {
geom::Vector4d q {1.0, 2.0, 3.0, 4.0};
geom::Vector3d v {1.0, 2.0, 3.0}; geom::Vector3d v {1.0, 2.0, 3.0};
double w = 4.0; double w = 4.0;
geom::Quaterniond p(v, w); geom::Quaterniond p(q);
EXPECT_EQ(p.axis(), v); EXPECT_EQ(p.axis(), v);
EXPECT_DOUBLE_EQ(p.angle(), w); EXPECT_DOUBLE_EQ(p.angle(), w);
@ -101,8 +237,8 @@ TEST(QuaternionMiscellaneous, SanityChecks)
TEST(QuaternionMiscellaneous, OutputStream) 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; stringstream ss;
ss << p; ss << p;

View File

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