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

Clean up and document code.

This commit is contained in:
Kyle Isom 2019-08-05 22:50:28 -07:00
parent 3a9d614010
commit 71a6f5e128
9 changed files with 517 additions and 423 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -29,6 +29,8 @@ include_directories(include)
file(GLOB_RECURSE ${PROJECT_NAME}_HEADERS include/**.h)
file(GLOB_RECURSE ${PROJECT_NAME}_SOURCES src/*.cc)
message("${PROJECT_NAME}_SOURCES -> libwrmath")
## BUILD
add_library(lib${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES})

10
docs/Doxyfile
View File

@ -135,7 +135,7 @@ ABBREVIATE_BRIEF = "The $name class" \
# description.
# The default value is: NO.
ALWAYS_DETAILED_SEC = NO
ALWAYS_DETAILED_SEC = YES
# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all
# inherited members of a class in the documentation of that class as if those
@ -226,7 +226,7 @@ SEPARATE_MEMBER_PAGES = NO
# uses this value to replace tabs by spaces in code fragments.
# Minimum value: 1, maximum value: 16, default value: 4.
TAB_SIZE = 4
TAB_SIZE = 8
# This tag can be used to specify a number of aliases that act as commands in
# the documentation. An alias has the form:
@ -995,7 +995,7 @@ USE_MDFILE_AS_MAINPAGE =
# also VERBATIM_HEADERS is set to NO.
# The default value is: NO.
SOURCE_BROWSER = NO
SOURCE_BROWSER = YES
# Setting the INLINE_SOURCES tag to YES will include the body of functions,
# classes and enums directly into the documentation.
@ -1014,7 +1014,7 @@ STRIP_CODE_COMMENTS = YES
# function all documented functions referencing it will be listed.
# The default value is: NO.
REFERENCED_BY_RELATION = NO
REFERENCED_BY_RELATION = YES
# If the REFERENCES_RELATION tag is set to YES then for each documented function
# all documented entities called/used by that function will be listed.
@ -1533,7 +1533,7 @@ FORMULA_TRANSPARENT = YES
# The default value is: NO.
# This tag requires that the tag GENERATE_HTML is set to YES.
USE_MATHJAX = NO
USE_MATHJAX = YES
# When MathJax is enabled you can set the default output format to be used for
# the MathJax output. See the MathJax site (see:

39
include/wrmath/geom/orientation.h
View File

@ -16,23 +16,38 @@ namespace wr {
namespace geom {
constexpr uint8_t Basis_i = 0;
constexpr uint8_t Basis_j = 1;
constexpr uint8_t Basis_k = 2;
/// \defgroup basis Basis vector indices.
/// The following constants are provided as a convenience for indexing two-
/// and three-dimensional vectors.
/// \ingroup basis
/// Convenience constant for the x index.
constexpr uint8_t Basis_x = 0;
/// \ingroup basis
/// Convenience constant for the y index.
constexpr uint8_t Basis_y = 1;
/// \ingroup basis
/// Convenience constant for the z index.
constexpr uint8_t Basis_z = 2;
/// @brief Basis2d provides basis vectors for Vector2ds.
static const Vector2d Basis2d[] = {
Vector2d{1, 0},
Vector2d{0, 1},
};
/// @brief Basis2d provides basis vectors for Vector2fs.
static const Vector2f Basis2f[] = {
Vector2f{1, 0},
Vector2f{0, 1},
};
/// @brief Basis2d provides basis vectors for Vector3ds.
static const Vector3d Basis3d[] = {
Vector3d{1, 0, 0},
Vector3d{0, 1, 0},
@ -40,6 +55,7 @@ static const Vector3d Basis3d[] = {
};
/// @brief Basis2d provides basis vectors for Vector3fs.
static const Vector3f Basis3f[] = {
Vector3f{1, 0, 0},
Vector3f{0, 1, 0},
@ -47,12 +63,27 @@ static const Vector3f Basis3f[] = {
};
/// Heading2f returns a compass heading for a Vector2f.
/// @param vec A vector orientation.
/// @return The compass heading of the vector in radians.
float Heading2f(Vector2f vec);
/// Heading2d returns a compass heading for a Vector2d.
/// @param vec A vector orientation.
/// @return The compass heading of the vector in radians.
double Heading2d(Vector2d vec);
/// Heading3f returns a compass heading for a Vector2f.
/// @param vec A vector orientation.
/// @return The compass heading of the vector in radians.
float Heading3f(Vector3f vec);
/// Heading3d returns a compass heading for a Vector2f.
/// @param vec A vector orientation.
/// @return The compass heading of the vector in radians.
double Heading3d(Vector3d vec);
} // namespace geom
} // namespace wr

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

@ -1,83 +1,102 @@
/// quaternion.h contains an implementation of quaternions suitable
/// for navigation in R3.
#ifndef __WRMATH_QUATERNION_H
#define __WRMATH_QUATERNION_H
#include <cassert>
#include <cmath>
#include <initializer_list>
#include <ostream>
#include <wrmath/geom/vector.h>
#include <wrmath/math.h>
/// wr contains the wntrmute robotics code.
namespace wr {
/// geom contains geometric classes and functions.
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>
/// @brief Quaternions provide a representation of orientation and rotations
/// in three dimensions.
///
/// 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
/// of the form <x, y, z, w>, where x, y, and z represent an axis of rotation in
/// R3 and w the angle, in radians, of the rotation about that axis. Where Euler
/// angles are concerned, the ZYX (or yaw, pitch, roll) sequence is used.
///
/// 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. MATHJThe 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> {0.0, 0.0, 0.0}), w(1.0)
///
/// The default Quaternion constructor returns an identity quaternion.
///
Quaternion() : v(Vector<T, 3>{0.0, 0.0, 0.0}), w(1.0)
{
wr::math::DefaultEpsilon(this->eps);
v.setEpsilon(this->eps);
this->constrainAngle();
};
/**
* A Quaternion may be initialised with a Vector<T, 3> axis 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.
*/
/// A Quaternion may be initialised with a Vector<T, 3> axis 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->constrainAngle();
wr::math::DefaultEpsilon(this->eps);
v.setEpsilon(this->eps);
};
/**
* A Quaternion may be initialised with a Vector<T, 4> comprised of the axis of rotation
* followed by the angle of rotation.
* @param vector A vector in the form <i, j, k, w>.
*/
///
/// A Quaternion may be initialised with a Vector<T, 4> comprised of
/// the axis of rotation followed by the angle of rotation.
/// @param vector A vector in the form <x, y, z, w>.
///
Quaternion(Vector<T, 4> vector) :
v(Vector<T, 3> {vector[0], vector[1], vector[2]}),
v(Vector<T, 3>{vector[0], vector[1], vector[2]}),
w(vector[3])
{
wr::math::DefaultEpsilon(this->eps);
this->constrainAngle();
wr::math::DefaultEpsilon(this->eps);
v.setEpsilon(this->eps);
}
/// A Quaternion may be constructed with an initializer list of type T, which must have
/// exactly N element.
/// @param ilst An initial set of values in the form <x, y, z, w>.
Quaternion(std::initializer_list<T> ilst)
{
auto it = ilst.begin();
/**
* Set the comparison tolerance for this quaternion.
* @param epsilon A tolerance value.
*/
this->v = Vector<T, 3>{it[0], it[1], it[2]};
this->w = it[3];
this->constrainAngle();
wr::math::DefaultEpsilon(this->eps);
v.setEpsilon(this->eps);
}
/// Set the comparison tolerance for this quaternion.
/// @param epsilon A tolerance value.
void
setEpsilon(T epsilon)
{
@ -86,10 +105,8 @@ public:
}
/**
* 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.
Vector<T, 3>
axis() const
{
@ -97,10 +114,8 @@ public:
}
/**
* Return the angle of rotation of this quaternion.
* @return the angle of rotation of this quaternion.
*/
/// Return the angle of rotation of this quaternion.
/// @return the angle of rotation of this quaternion.
T
angle() const
{
@ -108,15 +123,13 @@ public:
}
/**
* Compute the norm of a quaternion. Treating the Quaternion as a
* Vector<T, 4>, it's the same as computing the magnitude.
* @return A non-negative real number.
*/
/// Compute the norm of a quaternion. Treating the Quaternion as a
/// Vector<T, 4>, it's the same as computing the magnitude.
/// @return A non-negative real number.
T
norm() const
{
T n = 0;
T n = 0;
n += (this->v[0] * this->v[0]);
n += (this->v[1] * this->v[1]);
@ -127,58 +140,48 @@ public:
}
/**
* Compute the conjugate of a quaternion.
* @return The conjugate of this quaternion.
*/
/// Compute the conjugate of a quaternion.
/// @return The conjugate of this quaternion.
Quaternion
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.
*/
/// Compute the inverse of a quaternion.
/// @return The inverse of this quaternion.
Quaternion
inverse() const
{
T _norm = this->norm();
T _norm = this->norm();
return this->conjugate() / (_norm * _norm);
}
/**
* Determine whether this is a unit quaternion.
* @return true if this is a unit quaternion.
*/
/// 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 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.
*/
/// 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() 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.
*/
/// 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
{
@ -186,33 +189,30 @@ public:
}
/**
* 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>
*/
/// Return the Euler angles for this quaternion as a vector of
/// <yaw, pitch, roll>. Users of this function should watch out
/// for gimbal 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;
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)));
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};
return Vector<T, 3>{yaw, pitch, roll};
}
/**
* Perform quaternion addition with another quaternion.
* @param other The quaternion to be added with this one.
* @return The result of adding the two quaternions together.
*/
/// Perform quaternion addition with another quaternion.
/// @param other The quaternion to be added with this one.
/// @return The result of adding the two quaternions together.
Quaternion
operator+(const Quaternion<T> &other) const
{
@ -220,23 +220,19 @@ 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.
*/
/// 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
{
return Quaternion(this->v - other.v, this->w - other.w);
return Quaternion(this->v - other.v, this->w - other.w);
}
/**
* Perform scalar multiplication.
* @param k The scaling value.
* @return A scaled quaternion.
*/
/// Perform scalar multiplication.
/// @param k The scaling value.
/// @return A scaled quaternion.
Quaternion
operator*(const T k) const
{
@ -244,10 +240,9 @@ public:
}
/** Perform scalar division.
* @param k The scalar divisor.
* @return A scaled quaternion.
*/
/// Perform scalar division.
/// @param k The scalar divisor.
/// @return A scaled quaternion.
Quaternion
operator/(const T k) const
{
@ -255,43 +250,37 @@ public:
}
/**
* 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.
*/
/// 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);
(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.
*/
/// 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
{
T angle = (this->w * other.w) -
(this->v * other.v);
Vector<T, 3> axis = (other.v * this->w) +
(this->v * other.w) +
(this->v.cross(other.v));
T angle = (this->w * other.w) -
(this->v * other.v);
Vector<T, 3> axis = (other.v * this->w) +
(this->v * other.w) +
(this->v.cross(other.v));
return Quaternion(axis, angle);
}
/**
* Perform quaternion equality checking.
* @param other The quaternion to check equality against.
* @return True if the two quaternions are equal within their tolerance.
*/
/// 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
{
@ -300,11 +289,9 @@ public:
}
/**
* Perform quaternion inequality checking.
* @param other The quaternion to check inequality against.
* @return True if the two quaternions are unequal within their tolerance.
*/
/// 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
{
@ -312,27 +299,25 @@ 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)
/// 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)
{
outs << q.w << " + " << q.v;
return outs;
}
private:
static constexpr T minRotation = -4 * M_PI;
static constexpr T maxRotation = 4 * M_PI;
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;
Vector<T, 3> v; // axis of rotation
T w; // angle of rotation
T eps;
void
constrainAngle()
@ -347,99 +332,89 @@ private:
};
/**
* Type aliases are provided for float and double quaternions.
*/
typedef Quaternion<float> Quaternionf;
typedef Quaternion<double> Quaterniond;
///
/// \defgroup quaternion_aliases Quaternion type aliases.
/// Type aliases are provided for float and double quaternions.
///
/// \ingroup quaternion_aliases
/// Type alias for a float Quaternion.
typedef Quaternion<float> Quaternionf;
/// \ingroup quaternion_aliases
/// Type alias for a double Quaternion.
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 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.
/// @relatesalso Quaternion
Quaternionf quaternionf(Vector3f axis, float angle);
/**
* 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));
}
/// 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.
/// @relatesalso Quaternion
Quaterniond quaterniond(Vector3d axis, double angle);
/**
* 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.
/// @relatesalso Quaternion
Quaternionf quaternionf_from_euler(Vector3f euler);
/**
* 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;
/// 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.
/// @relatesalso Quaternion
Quaterniond quaterniond_from_euler(Vector3d euler);
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});
}
/// LERP computes the linear interpolation of two quaternions at some
/// fraction of the distance between them.
///
/// \tparam T
/// \param p The starting quaternion.
/// \param q The ending quaternion.
/// \param t The fraction of the distance between the two quaternions to
/// interpolate.
/// \return A Quaternion representing the linear interpolation of the
/// two quaternions.
template <typename T>
Quaternion<T> LERP(Quaternion<T> p, Quaternion<T> q, T t);
/// ShortestSLERP computes the shortest distance spherical linear
/// interpolation between two quaternions at some fraction of the
/// distance between them.
///
/// \tparam T
/// \param p The starting quaternion.
/// \param q The ending quaternion.
/// \param t The fraction of the distance between the two quaternions
/// to interpolate.
/// \return A Quaternion representing the shortest path between two
/// quaternions.
template <typename T>
Quaternion<T> ShortestSLERP(Quaternion<T> p, Quaternion<T> q, T t);
/// Run a quick self test to exercise basic functionality of the Quaternion
/// class to verify correct operation. Note that if \#NDEBUG is defined, the
/// self test is disabled.
void Quaternion_SelfTest();
// Helpful references for understanding quaternions:

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

@ -1,3 +1,4 @@
/// vector.h provides an implementation of vectors.
#ifndef __WRMATH_GEOM_VECTOR_H
#define __WRMATH_GEOM_VECTOR_H
@ -21,20 +22,18 @@ namespace wr {
namespace geom {
/**
* Vector provides a standard interface for dimensionless fixed-size
* vectors. Once instantiated, they cannot be modified. Note that while
* the type is generic, it's intended to be used with floating-point
* types. They can be indexed like arrays, and they contain an epsilon
* value that defines a tolerance for equality.
*/
/// @brief Vectors represent a direction and magnitude.
///
/// Vector provides a standard interface for dimensionless fixed-size
/// vectors. Once instantiated, they cannot be modified. Note that while
/// the type is generic, it's intended to be used with floating-point
/// types. They can be indexed like arrays, and they contain an epsilon
/// value that defines a tolerance for equality.
template <typename T, size_t N>
class Vector {
public:
/**
* The default constructor creates a unit vector for a given
* type and size.
*/
/// The default constructor creates a unit vector for a given type
/// and size.
Vector()
{
T unitLength = (T)1.0 / std::sqrt(N);
@ -45,11 +44,10 @@ public:
wr::math::DefaultEpsilon(this->epsilon);
}
/**
* If given an initializer_list, the vector is created with
* those values. There must be exactly N elements in the list.
* @param ilst An intializer list with N elements of type T.
*/
/// If given an initializer_list, the vector is created with
/// those values. There must be exactly N elements in the list.
/// @param ilst An intializer list with N elements of type T.
Vector(std::initializer_list<T> ilst)
{
assert(ilst.size() == N);
@ -59,10 +57,8 @@ public:
}
/**
* Magnitude computes the length of the vector.
* @return The length of the vector.
*/
/// Compute the length of the vector.
/// @return The length of the vector.
T magnitude() const {
T result = 0;
@ -70,16 +66,13 @@ public:
result += (this->arr[i] * this->arr[i]);
}
return std::sqrt(result);
};
}
/**
* Set the tolerance for equality checks. At a minimum, this allows
* for systemic errors in floating math arithmetic.
*
* @param eps is the maximum difference between this vector and
* another.
*/
/// Set the tolerance for equality checks. At a minimum, this allows
/// for systemic errors in floating math arithmetic.
/// @param eps is the maximum difference between this vector and
/// another.
void
setEpsilon(T eps)
{
@ -87,10 +80,8 @@ public:
}
/**
* Determine whether this is a zero vector.
* @return true if the vector is zero.
*/
/// Determine whether this is a zero vector.
/// @return true if the vector is zero.
bool
isZero() const
{
@ -103,10 +94,8 @@ public:
}
/**
* Obtain the unit vector for this vector.
* @return The unit vector
*/
/// Obtain the unit vector for this vector.
/// @return The unit vector
Vector
unitVector() const
{
@ -114,10 +103,8 @@ public:
}
/**
* Determine if this is a unit vector, e.g. if its length is 1.
* @return true if the vector is a unit vector.
*/
/// Determine if this is a unit vector, e.g. if its length is 1.
/// @return true if the vector is a unit vector.
bool
isUnitVector() const
{
@ -125,11 +112,9 @@ public:
}
/**
* Compute the angle between two other vectors.
* @param other Another vector.
* @return The angle in radians between the two vectors.
*/
/// Compute the angle between two other vectors.
/// @param other Another vector.
/// @return The angle in radians between the two vectors.
T
angle(const Vector<T, N> &other) const
{
@ -143,11 +128,9 @@ public:
}
/**
* Determine whether two vectors are parallel.
* @param other Another vector
* @return True if the angle between the vectors is zero.
*/
/// Determine whether two vectors are parallel.
/// @param other Another vector
/// @return True if the angle between the vectors is zero.
bool
isParallel(const Vector<T, N> &other) const
{
@ -164,12 +147,10 @@ public:
}
/**
* Determine if two vectors are orthogonal or perpendicular to each
* other.
* @param other Another vector
* @return True if the two vectors are orthogonal.
*/
/// Determine if two vectors are orthogonal or perpendicular to each
/// other.
/// @param other Another vector
/// @return True if the two vectors are orthogonal.
bool
isOrthogonal(const Vector<T, N> &other) const
{
@ -181,12 +162,10 @@ public:
}
/**
* Project this vector onto some basis vector.
* @param basis The basis vector to be projected onto.
* @return A vector that is the projection of this onto the basis
* vector.
*/
/// Project this vector onto some basis vector.
/// @param basis The basis vector to be projected onto.
/// @return A vector that is the projection of this onto the basis
/// vector.
Vector
projectParallel(const Vector<T, N> &basis) const
{
@ -196,13 +175,11 @@ public:
}
/**
* Project this vector perpendicularly onto some basis vector.
* This is also called the rejection of the vector.
* @param basis The basis vector to be projected onto.
* @return A vector that is the orthogonal projection of this onto
* the basis vector.
*/
/// Project this vector perpendicularly onto some basis vector.
/// This is also called the rejection of the vector.
/// @param basis The basis vector to be projected onto.
/// @return A vector that is the orthogonal projection of this onto
/// the basis vector.
Vector
projectOrthogonal(const Vector<T, N> &basis)
{
@ -211,12 +188,10 @@ public:
}
/**
* Compute the cross product of two vectors. This is only defined
* over three-dimensional vectors.
* @param other Another 3D vector.
* @return The cross product vector.
*/
/// Compute the cross product of two vectors. This is only defined
/// over three-dimensional vectors.
/// @param other Another 3D vector.
/// @return The cross product vector.
Vector
cross(const Vector<T, N> &other) const
{
@ -229,12 +204,10 @@ public:
}
/**
* Perform vector addition with another vector.
* @param other The vector to be added.
* @return A new vector that is the result of adding this and the
* other vector.
*/
/// Perform vector addition with another vector.
/// @param other The vector to be added.
/// @return A new vector that is the result of adding this and the
/// other vector.
Vector
operator+(const Vector<T, N> &other) const
{
@ -248,12 +221,10 @@ public:
}
/**
* Perform vector subtraction with another vector.
* @param other The vector to be subtracted from this vector.
* @return A new vector that is the result of subtracting the
* other vector from this one.
*/
/// Perform vector subtraction with another vector.
/// @param other The vector to be subtracted from this vector.
/// @return A new vector that is the result of subtracting the
/// other vector from this one.
Vector
operator-(const Vector<T, N> &other) const
{
@ -267,11 +238,9 @@ public:
}
/**
* Perform scalar multiplication of this vector by some scale factor.
* @param k The scaling value.
* @return A new vector that is this vector scaled by k.
*/
/// Perform scalar multiplication of this vector by some scale factor.
/// @param k The scaling value.
/// @return A new vector that is this vector scaled by k.
Vector
operator*(const T k) const
{
@ -285,11 +254,9 @@ public:
}
/**
* Perform scalar division of this vector by some scale factor.
* @param k The scaling value
* @return A new vector that is this vector scaled by 1/k.
*/
/// Perform scalar division of this vector by some scale factor.
/// @param k The scaling value
/// @return A new vector that is this vector scaled by 1/k.
Vector
operator/(const T k) const
{
@ -303,11 +270,9 @@ public:
}
/**
* Compute the dot product between two vectors.
* @param other The other vector.
* @return A scalar value that is the dot product of the two vectors.
*/
/// Compute the dot product between two vectors.
/// @param other The other vector.
/// @return A scalar value that is the dot product of the two vectors.
T
operator*(const Vector<T, N> &other) const
{
@ -321,12 +286,10 @@ public:
}
/**
* Compare two vectors for equality.
* @param other The other vector.
* @return Return true if all the components of both vectors are
* within the tolerance value.
*/
/// Compare two vectors for equality.
/// @param other The other vector.
/// @return Return true if all the components of both vectors are
/// within the tolerance value.
bool
operator==(const Vector<T, N> &other) const
{
@ -339,12 +302,10 @@ public:
}
/**
* Compare two vectors for inequality.
* @param other The other vector.
* @return Return true if any of the components of both vectors are
* not within the tolerance value.
*/
/// Compare two vectors for inequality.
/// @param other The other vector.
/// @return Return true if any of the components of both vectors are
/// not within the tolerance value.
bool
operator!=(const Vector<T, N> &other) const
{
@ -352,11 +313,9 @@ public:
}
/**
* Support array indexing into vector.
* @param i The component index.
* @return The value of the vector component at i.
*/
/// Support array indexing into vector.
/// @param i The component index.
/// @return The value of the vector component at i.
T
operator[](size_t i) const
{
@ -364,12 +323,10 @@ public:
}
/**
* Support outputting vectors in the form "<i, j, ...>".
* @param outs An output stream.
* @param vec The vector to be formatted.
* @return The output stream.
*/
/// Support outputting vectors in the form "<i, j, ...>".
/// @param outs An output stream.
/// @param vec The vector to be formatted.
/// @return The output stream.
friend std::ostream&
operator<<(std::ostream& outs, const Vector<T, N>& vec)
{
@ -390,19 +347,37 @@ private:
std::array<T, N> arr;
};
///
/// \defgroup vector_aliases Vector type aliases.
///
/**
* A number of shorthand aliases for vectors are provided. They follow
* the form of VectorNt, where N is the dimension and t is the type.
* For example, a 2D float vector is Vector2f.
*/
/// \ingroup vector_aliases
/// A number of shorthand aliases for vectors are provided. They follow
/// the form of VectorNt, where N is the dimension and t is the type.
/// For example, a 2D float vector is Vector2f.
/// \ingroup vector_aliases
/// @brief Type alias for a two-dimensional float vector.
typedef Vector<float, 2> Vector2f;
/// \ingroup vector_aliases
/// @brief Type alias for a three-dimensional float vector.
typedef Vector<float, 3> Vector3f;
/// \ingroup vector_aliases
/// @brief Type alias for a four-dimensional float vector.
typedef Vector<float, 4> Vector4f;
/// \ingroup vector_aliases
/// @brief Type alias for a two-dimensional double vector.
typedef Vector<double, 2> Vector2d;
/// \ingroup vector_aliases
/// @brief Type alias for a three-dimensional double vector.
typedef Vector<double, 3> Vector3d;
/// \ingroup vector_aliases
/// @brief Type alias for a four-dimensional double vector.
typedef Vector<double, 4> Vector4d;

59
include/wrmath/math.h
View File

@ -1,3 +1,4 @@
/// math.h provides certain useful mathematical functions.
#ifndef __WRMATH_UTIL_MATH_H
#define __WRMATH_UTIL_MATH_H
@ -9,56 +10,42 @@ namespace wr {
namespace math {
/**
* Convert radians to degrees.
* @param rads the angle in radians
* @return the angle in degrees.
*/
/// Convert radians to degrees.
/// @param rads the angle in radians
/// @return the angle in degrees.
float RadiansToDegreesF(float rads);
/**
* Convert radians to degrees.
* @param rads the angle in radians
* @return the angle in degrees.
*/
/// Convert radians to degrees.
/// @param rads the angle in radians
/// @return the angle in degrees.
double RadiansToDegreesD(double rads);
/**
* Convert degrees to radians.
* @param degrees the angle in degrees
* @return the angle in radians.
*/
/// Convert degrees to radians.
/// @param degrees the angle in degrees
/// @return the angle in radians.
float DegreesToRadiansF(float degrees);
/**
* Convert degrees to radians.
* @param degrees the angle in degrees
* @return the angle in radians.
*/
/// Convert degrees to radians.
/// @param degrees the angle in degrees
/// @return the angle in radians.
double DegreesToRadiansD(double degrees);
/**
* Get the default epsilon value.
* @param epsilon The variable to store the epsilon value in.
*/
/// Get the default epsilon value.
/// @param epsilon The variable to store the epsilon value in.
void DefaultEpsilon(double &epsilon);
/**
* Get the default epsilon value.
* @param epsilon The variable to store the epsilon value in.
*/
/// Get the default epsilon value.
/// @param epsilon The variable to store the epsilon value in.
void DefaultEpsilon(float &epsilon);
/**
* Return whether the two values of type T are equal to within some tolerance.
* @tparam T The type of value
* @param a A value of type T used as the left-hand side of an equality check.
* @param b A value of type T used as the right-hand side of an equality check.
* @param epsilon The tolerance value.
* @return Whether the two values are "close enough" to be considered equal.
*/
/// Return whether the two values of type T are equal to within some tolerance.
/// @tparam T The type of value
/// @param a A value of type T used as the left-hand side of an equality check.
/// @param b A value of type T used as the right-hand side of an equality check.
/// @param epsilon The tolerance value.
/// @return Whether the two values are "close enough" to be considered equal.
template <typename T>
static T
WithinTolerance(T a, T b, T epsilon)

6
src/orientation.cc
View File

@ -1,4 +1,4 @@
#include <cmath>
#include <wrmath/geom/vector.h>
#include <wrmath/geom/orientation.h>
@ -9,7 +9,7 @@ namespace geom {
float
Heading2f(Vector2f vec)
{
return vec.angle(Basis2f[Basis_i]);
return vec.angle(Basis2f[Basis_x]);
}
@ -24,7 +24,7 @@ Heading3f(Vector3f vec)
double
Heading2d(Vector2d vec)
{
return vec.angle(Basis2d[Basis_i]);
return vec.angle(Basis2d[Basis_x]);
}

108
src/quaternion.cc Normal file
View File

@ -0,0 +1,108 @@
#include <wrmath/geom/quaternion.h>
namespace wr {
namespace geom {
Quaternionf
quaternionf(Vector3f axis, float angle)
{
return Quaternionf(axis.unitVector() * std::sin(angle / 2.0),
std::cos(angle / 2.0));
}
Quaterniond
quaterniond(Vector3d axis, double angle)
{
return Quaterniond(axis.unitVector() * std::sin(angle / 2.0),
std::cos(angle / 2.0));
}
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});
}
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});
}
template <typename T>
Quaternion<T>
LERP(Quaternion<T> p, Quaternion<T> q, T t)
{
return p + (q - p) * t;
}
template <typename T>
Quaternion<T>
ShortestSLERP(Quaternion<T> p, Quaternion<T> q, T t)
{
T innerProduct = p.dot(q);
T sign = innerProduct >= 0.0 ? -1.0 : 1.0;
T acip = std::acos(innerProduct);
return (p * std::sin((T)1.0 - t) * acip + p * sign * std::sin(t * acip)) / std::sin(acip);
}
void
Quaternion_SelfTest()
{
#ifndef NDEBUG
Vector3f v {1.0, 0.0, 0.0};
Vector3f yAxis {0.0, 1.0, 0.0};
float angle = M_PI / 2;
Quaternionf p = quaternionf(yAxis, angle);
Quaternionf q;
Vector3f vr {0.0, 0.0, 1.0};
assert(p.isUnitQuaternion());
assert(p.rotate(v) == vr);
assert(p * q == p);
#endif
}
} // namespace geom
} // namespace wr

16
test/quaternion_test.cc
View File

@ -7,6 +7,12 @@ using namespace std;
using namespace wr;
TEST(Quaternion, SelfTest)
{
geom::Quaternion_SelfTest();
}
TEST(Quaterniond, Addition)
{
geom::Quaterniond p(geom::Vector4d {1.0, -2.0, 1.0, 3.0});
@ -250,6 +256,16 @@ TEST(QuaternionMiscellaneous, OutputStream)
}
TEST(QuaternionMiscellanous, InitializerConstructor)
{
geom::Quaternionf p {1.0, 1.0, 1.0, 1.0};
geom::Quaternionf q(geom::Vector4f {1.0, 1.0, 1.0, 1.0});
EXPECT_EQ(p, q);
EXPECT_FLOAT_EQ(p.norm(), 2.0);
}
int
main(int argc, char **argv)
{