quaternion <-> euler, lots of fixes.
This commit is contained in:
parent
a175afd49f
commit
3a9d614010
|
|
@ -8,7 +8,9 @@ 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)
|
||||
|
|
@ -16,7 +18,9 @@ add_compile_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
|
||||
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()
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
wrmath API
|
||||
==========
|
||||
|
||||
.. doxygenindex::
|
||||
|
|
@ -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'
|
||||
|
|
@ -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`
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
|
|
|
|||
Loading…
Reference in New Issue