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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
CMakeLists.txt
View File

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

4
docs/Doxyfile
View File

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

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

0
docs/requirements.txt → docs/sphinx/requirements.txt
View File

293
include/wrmath/geom/quaternion.h
View File

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

13
include/wrmath/geom/vector.h
View File

@ -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);
}
/**

190
test/quaternion_test.cc
View File

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

8
test/vector_test.cc
View File

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