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}_HEADERS include/**.h)
file(GLOB_RECURSE ${PROJECT_NAME}_SOURCES src/*.cc) file(GLOB_RECURSE ${PROJECT_NAME}_SOURCES src/*.cc)
message("${PROJECT_NAME}_SOURCES -> libwrmath")
## BUILD ## BUILD
add_library(lib${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES}) add_library(lib${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES})

10
docs/Doxyfile
View File

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

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

@ -16,23 +16,38 @@ namespace wr {
namespace geom { namespace geom {
constexpr uint8_t Basis_i = 0; /// \defgroup basis Basis vector indices.
constexpr uint8_t Basis_j = 1; /// The following constants are provided as a convenience for indexing two-
constexpr uint8_t Basis_k = 2; /// 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[] = { static const Vector2d Basis2d[] = {
Vector2d{1, 0}, Vector2d{1, 0},
Vector2d{0, 1}, Vector2d{0, 1},
}; };
/// @brief Basis2d provides basis vectors for Vector2fs.
static const Vector2f Basis2f[] = { static const Vector2f Basis2f[] = {
Vector2f{1, 0}, Vector2f{1, 0},
Vector2f{0, 1}, Vector2f{0, 1},
}; };
/// @brief Basis2d provides basis vectors for Vector3ds.
static const Vector3d Basis3d[] = { static const Vector3d Basis3d[] = {
Vector3d{1, 0, 0}, Vector3d{1, 0, 0},
Vector3d{0, 1, 0}, Vector3d{0, 1, 0},
@ -40,6 +55,7 @@ static const Vector3d Basis3d[] = {
}; };
/// @brief Basis2d provides basis vectors for Vector3fs.
static const Vector3f Basis3f[] = { static const Vector3f Basis3f[] = {
Vector3f{1, 0, 0}, Vector3f{1, 0, 0},
Vector3f{0, 1, 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); 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); 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); 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); double Heading3d(Vector3d vec);
} // namespace geom } // namespace geom
} // namespace wr } // namespace wr

419
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 #ifndef __WRMATH_QUATERNION_H
#define __WRMATH_QUATERNION_H #define __WRMATH_QUATERNION_H
#include <cassert> #include <cassert>
#include <cmath> #include <cmath>
#include <initializer_list>
#include <ostream> #include <ostream>
#include <wrmath/geom/vector.h> #include <wrmath/geom/vector.h>
#include <wrmath/math.h> #include <wrmath/math.h>
/// wr contains the wntrmute robotics code.
namespace wr { namespace wr {
/// geom contains geometric classes and functions.
namespace geom { namespace geom {
/** /// @brief Quaternions provide a representation of orientation and rotations
* Quaternions encode rotations in three-dimensional space. While technically /// in three dimensions.
* 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. /// Quaternions encode rotations in three-dimensional space. While technically
* /// a quaternion is comprised of a real element and a complex vector<3>, for
* For information on the underlying vector type, see the documentation for /// the purposes of this library, it is modeled as a floating point 4D vector
* wr::geom::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
* The constructors are primarily intended for intended operations; in practice, /// angles are concerned, the ZYX (or yaw, pitch, roll) sequence is used.
* the quaternionf and quaterniond functions are more useful for constructing ///
* quaternions from vectors and angles. /// For information on the underlying vector type, see the documentation for
* /// wr::geom::Vector.
* Like vectors, quaternions carry an internal tolerance value ε that is used for ///
* floating point comparisons. The wr::math namespace contains the default values /// The constructors are primarily intended for intended operations; in practice,
* used for this; generally, a tolerance of 0.0001 is considered appropriate for /// the quaternionf() and quaterniond() functions are more useful for constructing
* the uses of this library. The tolerance can be explicitly set with the /// quaternions from vectors and angles.
* setEpsilon method. ///
*/ /// Like vectors, quaternions carry an internal tolerance value ε that is used for
template <typename T> /// 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 { class Quaternion {
public: public:
/** ///
* The default Quaternion constructor returns an identity quaternion. /// The default Quaternion constructor returns an identity quaternion.
*/ ///
Quaternion() : v(Vector<T, 3> {0.0, 0.0, 0.0}), w(1.0) Quaternion() : v(Vector<T, 3>{0.0, 0.0, 0.0}), w(1.0)
{ {
wr::math::DefaultEpsilon(this->eps); wr::math::DefaultEpsilon(this->eps);
v.setEpsilon(this->eps); v.setEpsilon(this->eps);
this->constrainAngle();
}; };
/** /// A Quaternion may be initialised with a Vector<T, 3> axis of rotation
* A Quaternion may be initialised with a Vector<T, 3> axis of rotation /// and an angle of rotation. This doesn't do the angle transforms to simplify
* and an angle of rotation. This doesn't do the angle transforms to simplify /// internal operations.
* internal operations. /// @param _axis A three-dimensional vector of the same type as the Quaternion.
* @param _axis A three-dimensional vector of the same type as the Quaternion. /// @param _angle The angle of rotation about the axis of rotation.
* @param _angle The angle of rotation about the axis of rotation.
*/
Quaternion(Vector<T, 3> _axis, T _angle) : v(_axis), w(_angle) Quaternion(Vector<T, 3> _axis, T _angle) : v(_axis), w(_angle)
{ {
wr::math::DefaultEpsilon(this->eps);
this->constrainAngle(); this->constrainAngle();
wr::math::DefaultEpsilon(this->eps);
v.setEpsilon(this->eps); v.setEpsilon(this->eps);
}; };
/** ///
* A Quaternion may be initialised with a Vector<T, 4> comprised of the axis of rotation /// A Quaternion may be initialised with a Vector<T, 4> comprised of
* followed by the angle of rotation. /// the axis of rotation followed by the angle of rotation.
* @param vector A vector in the form <i, j, k, w>. /// @param vector A vector in the form <x, y, z, w>.
*/ ///
Quaternion(Vector<T, 4> vector) : 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]) w(vector[3])
{ {
wr::math::DefaultEpsilon(this->eps);
this->constrainAngle(); this->constrainAngle();
wr::math::DefaultEpsilon(this->eps);
v.setEpsilon(this->eps); v.setEpsilon(this->eps);
} }
/** /// A Quaternion may be constructed with an initializer list of type T, which must have
* Set the comparison tolerance for this quaternion. /// exactly N element.
* @param epsilon A tolerance value. /// @param ilst An initial set of values in the form <x, y, z, w>.
*/ Quaternion(std::initializer_list<T> ilst)
{
auto it = ilst.begin();
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 void
setEpsilon(T epsilon) 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> Vector<T, 3>
axis() const 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 T
angle() const angle() const
{ {
@ -108,11 +123,9 @@ public:
} }
/** /// Compute the norm of a quaternion. Treating the Quaternion as a
* Compute the norm of a quaternion. Treating the Quaternion as a /// Vector<T, 4>, it's the same as computing the magnitude.
* Vector<T, 4>, it's the same as computing the magnitude. /// @return A non-negative real number.
* @return A non-negative real number.
*/
T T
norm() const norm() const
{ {
@ -127,21 +140,17 @@ public:
} }
/** /// Compute the conjugate of a quaternion.
* Compute the conjugate of a quaternion. /// @return The conjugate of this quaternion.
* @return The conjugate of this quaternion.
*/
Quaternion Quaternion
conjugate() const 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.
* Compute the inverse of a quaternion. /// @return The inverse of this quaternion.
* @return The inverse of this quaternion.
*/
Quaternion Quaternion
inverse() const inverse() const
{ {
@ -151,34 +160,28 @@ public:
} }
/** /// Determine whether this is a unit quaternion.
* Determine whether this is a unit quaternion. /// @return true if this is a unit quaternion.
* @return true if this is a unit quaternion.
*/
bool bool
isUnitQuaternion() const 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
* Return the quaternion as a Vector<T, 4>, with the axis of rotation /// followed by the angle of rotation.
* followed by the angle of rotation. /// @return A vector representation of the quaternion.
* @return A vector representation of the quaternion.
*/
Vector<T, 4> Vector<T, 4>
asVector() const 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.
* Rotate vector v about this quaternion. /// @param v The vector to be rotated.
* @param v The vector to be rotated. /// @return The rotated vector.
* @return The rotated vector.
*/
Vector<T, 3> Vector<T, 3>
rotate(Vector<T, 3> v) const rotate(Vector<T, 3> v) const
{ {
@ -186,12 +189,10 @@ public:
} }
/** /// Return the Euler angles for this quaternion as a vector of
* Return the Euler angles for this quaternion as a vector of /// <yaw, pitch, roll>. Users of this function should watch out
* <yaw, pitch, roll>. Users of this function should watch out /// for gimbal lock.
* for gimball lock. /// @return A vector<T, 3> containing <yaw, pitch, roll>
* @return A vector<T, 3> containing <yaw, pitch, roll>
*/
Vector<T, 3> Vector<T, 3>
euler() const euler() const
{ {
@ -201,18 +202,17 @@ public:
T c = this->v[1], c2 = c * c; T c = this->v[1], c2 = c * c;
T d = this->v[2], d2 = d * d; T d = this->v[2], d2 = d * d;
yaw = std::atan2(2 * ((a*b) + (c * d)), a2 - b2 - c2 + d2); yaw = std::atan2(2 * ((a * b) + (c * d)), a2 - b2 - c2 + d2);
pitch = std::asin(2 * ((b*d) - (a*c))); pitch = std::asin(2 * ((b * d) - (a * c)));
roll = std::atan2(2 * ((a * d) + (b * c)), a2 + b2 - c2 - d2); 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. /// Perform quaternion addition with another quaternion.
* @param other The quaternion to be added with this one. /// @param other The quaternion to be added with this one.
* @return The result of adding the two quaternions together. /// @return The result of adding the two quaternions together.
*/
Quaternion Quaternion
operator+(const Quaternion<T> &other) const operator+(const Quaternion<T> &other) const
{ {
@ -220,11 +220,9 @@ public:
} }
/** /// Perform quaternion subtraction with another quaternion.
* Perform quaternion subtraction with another quaternion. /// @param other The quaternion to be subtracted from this one.
* @param other The quaternion to be subtracted from this one. /// @return The result of subtracting the other quaternion from this one.
* @return The result of subtracting the other quaternion from this one.
*/
Quaternion Quaternion
operator-(const Quaternion<T> &other) const operator-(const Quaternion<T> &other) const
{ {
@ -232,11 +230,9 @@ public:
} }
/** /// Perform scalar multiplication.
* Perform scalar multiplication. /// @param k The scaling value.
* @param k The scaling value. /// @return A scaled quaternion.
* @return A scaled quaternion.
*/
Quaternion Quaternion
operator*(const T k) const operator*(const T k) const
{ {
@ -244,10 +240,9 @@ public:
} }
/** Perform scalar division. /// Perform scalar division.
* @param k The scalar divisor. /// @param k The scalar divisor.
* @return A scaled quaternion. /// @return A scaled quaternion.
*/
Quaternion Quaternion
operator/(const T k) const operator/(const T k) const
{ {
@ -255,26 +250,22 @@ public:
} }
/** /// Perform quaternion Hamilton multiplication with a three-
* Perform quaternion Hamilton multiplication with a three- /// dimensional vector; this is done by treating the vector
* dimensional vector; this is done by treating the vector /// as a pure quaternion (e.g. with an angle of rotation of 0).
* as a pure quaternion (e.g. with an angle of rotation of 0). /// @param vector The vector to multiply with this quaternion.
* @param vector The vector to multiply with this quaternion. /// @return The Hamilton product of the quaternion and vector.
* @return The Hamilton product of the quaternion and vector.
*/
Quaternion Quaternion
operator*(const Vector<T, 3> &vector) const operator*(const Vector<T, 3> &vector) const
{ {
return Quaternion(vector * this->w + this->v.cross(vector), return Quaternion(vector * this->w + this->v.cross(vector),
(T)0.0); (T) 0.0);
} }
/** /// Perform quaternion Hamilton multiplication.
* Perform quaternion Hamilton multiplication. /// @param other The other quaternion to multiply with this one.
* @param other The other quaternion to multiply with this one. /// @result The Hamilton product of the two quaternions.
* @result The Hamilton product of the two quaternions.
*/
Quaternion Quaternion
operator*(const Quaternion<T> &other) const operator*(const Quaternion<T> &other) const
{ {
@ -287,11 +278,9 @@ public:
} }
/** /// Perform quaternion equality checking.
* Perform quaternion equality checking. /// @param other The quaternion to check equality against.
* @param other The quaternion to check equality against. /// @return True if the two quaternions are equal within their tolerance.
* @return True if the two quaternions are equal within their tolerance.
*/
bool bool
operator==(const Quaternion<T> &other) const operator==(const Quaternion<T> &other) const
{ {
@ -300,11 +289,9 @@ public:
} }
/** /// Perform quaternion inequality checking.
* Perform quaternion inequality checking. /// @param other The quaternion to check inequality against.
* @param other The quaternion to check inequality against. /// @return True if the two quaternions are unequal within their tolerance.
* @return True if the two quaternions are unequal within their tolerance.
*/
bool bool
operator!=(const Quaternion<T> &other) const operator!=(const Quaternion<T> &other) const
{ {
@ -312,15 +299,13 @@ public:
} }
/** /// Support stream output of a quaternion in the form `a + <i, j, k>`.
* Support stream output of a quaternion in the form `a + <i, j, k>`. /// \todo improve the formatting.
* TODO: improve the formatting. /// @param outs An output stream
* @param outs An output stream /// @param q A quaternion
* @param q A quaternion /// @return The output stream
* @return The output stream friend std::ostream &
*/ operator<<(std::ostream &outs, const Quaternion<T> &q)
friend std::ostream&
operator<<(std::ostream& outs, const Quaternion<T>& q)
{ {
outs << q.w << " + " << q.v; outs << q.w << " + " << q.v;
return outs; return outs;
@ -347,99 +332,89 @@ private:
}; };
/** ///
* Type aliases are provided for float and double quaternions. /// \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; typedef Quaternion<float> Quaternionf;
/// \ingroup quaternion_aliases
/// Type alias for a double Quaternion.
typedef Quaternion<double> Quaterniond; typedef Quaternion<double> Quaterniond;
/** /// Return a float quaternion scaled appropriately from a vector and angle,
* Return a float quaternion scaled appropriately from a vector and angle, /// e.g. angle = cos(angle / 2), axis.unitVector() * sin(angle / 2).
* e.g. angle = cos(angle / 2), axis.unitVector() * sin(angle / 2). /// @param axis The axis of rotation.
* @param axis The axis of rotation. /// @param angle The angle of rotation.
* @param angle The angle of rotation. /// @return A quaternion.
* @return A quaternion. /// @relatesalso Quaternion
*/ Quaternionf quaternionf(Vector3f axis, float angle);
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,
* Return a double quaternion scaled appropriately from a vector and angle, /// e.g. angle = cos(angle / 2), axis.unitVector() * sin(angle / 2).
* e.g. angle = cos(angle / 2), axis.unitVector() * sin(angle / 2). /// @param axis The axis of rotation.
* @param axis The axis of rotation. /// @param angle The angle of rotation.
* @param angle The angle of rotation. /// @return A quaternion.
* @return A quaternion. /// @relatesalso Quaternion
*/ Quaterniond quaterniond(Vector3d axis, double angle);
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),
* Given a vector of Euler angles in ZYX sequence (e.g. yaw, pitch, roll), /// return a quaternion.
* return a quaternion. /// @param euler A vector Euler angle in ZYX sequence.
* @param euler A vector Euler angle in ZYX sequence. /// @return A Quaternion representation of the orientation represented
* @return A Quaternion representation of the orientation represented /// by the Euler angles.
* by the Euler angles. /// @relatesalso Quaternion
*/ Quaternionf quaternionf_from_euler(Vector3f euler);
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),
* Given a vector of Euler angles in ZYX sequence (e.g. yaw, pitch, roll), /// return a quaternion.
* return a quaternion. /// @param euler A vector Euler angle in ZYX sequence.
* @param euler A vector Euler angle in ZYX sequence. /// @return A Quaternion representation of the orientation represented
* @return A Quaternion representation of the orientation represented /// by the Euler angles.
* by the Euler angles. /// @relatesalso Quaternion
*/ Quaterniond quaterniond_from_euler(Vector3d euler);
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); /// LERP computes the linear interpolation of two quaternions at some
y = (sin_yaw * cos_pitch * sin_roll) - (cos_yaw * sin_pitch * cos_roll); /// fraction of the distance between them.
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); /// \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);
return Quaterniond(Vector4d {x, y, z, w});
} /// 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: // 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 #ifndef __WRMATH_GEOM_VECTOR_H
#define __WRMATH_GEOM_VECTOR_H #define __WRMATH_GEOM_VECTOR_H
@ -21,20 +22,18 @@ namespace wr {
namespace geom { namespace geom {
/** /// @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 /// Vector provides a standard interface for dimensionless fixed-size
* the type is generic, it's intended to be used with floating-point /// vectors. Once instantiated, they cannot be modified. Note that while
* types. They can be indexed like arrays, and they contain an epsilon /// the type is generic, it's intended to be used with floating-point
* value that defines a tolerance for equality. /// 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> template <typename T, size_t N>
class Vector { class Vector {
public: public:
/** /// The default constructor creates a unit vector for a given type
* The default constructor creates a unit vector for a given /// and size.
* type and size.
*/
Vector() Vector()
{ {
T unitLength = (T)1.0 / std::sqrt(N); T unitLength = (T)1.0 / std::sqrt(N);
@ -45,11 +44,10 @@ public:
wr::math::DefaultEpsilon(this->epsilon); wr::math::DefaultEpsilon(this->epsilon);
} }
/**
* If given an initializer_list, the vector is created with /// If given an initializer_list, the vector is created with
* those values. There must be exactly N elements in the list. /// those values. There must be exactly N elements in the list.
* @param ilst An intializer list with N elements of type T. /// @param ilst An intializer list with N elements of type T.
*/
Vector(std::initializer_list<T> ilst) Vector(std::initializer_list<T> ilst)
{ {
assert(ilst.size() == N); assert(ilst.size() == N);
@ -59,10 +57,8 @@ public:
} }
/** /// Compute the length of the vector.
* Magnitude computes the length of the vector. /// @return The length of the vector.
* @return The length of the vector.
*/
T magnitude() const { T magnitude() const {
T result = 0; T result = 0;
@ -70,16 +66,13 @@ public:
result += (this->arr[i] * this->arr[i]); result += (this->arr[i] * this->arr[i]);
} }
return std::sqrt(result); return std::sqrt(result);
}; }
/** /// Set the tolerance for equality checks. At a minimum, this allows
* Set the tolerance for equality checks. At a minimum, this allows /// for systemic errors in floating math arithmetic.
* for systemic errors in floating math arithmetic. /// @param eps is the maximum difference between this vector and
* /// another.
* @param eps is the maximum difference between this vector and
* another.
*/
void void
setEpsilon(T eps) setEpsilon(T eps)
{ {
@ -87,10 +80,8 @@ public:
} }
/** /// Determine whether this is a zero vector.
* Determine whether this is a zero vector. /// @return true if the vector is zero.
* @return true if the vector is zero.
*/
bool bool
isZero() const isZero() const
{ {
@ -103,10 +94,8 @@ public:
} }
/** /// Obtain the unit vector for this vector.
* Obtain the unit vector for this vector. /// @return The unit vector
* @return The unit vector
*/
Vector Vector
unitVector() const unitVector() const
{ {
@ -114,10 +103,8 @@ public:
} }
/** /// Determine if this is a unit vector, e.g. if its length is 1.
* Determine if this is a unit vector, e.g. if its length is 1. /// @return true if the vector is a unit vector.
* @return true if the vector is a unit vector.
*/
bool bool
isUnitVector() const isUnitVector() const
{ {
@ -125,11 +112,9 @@ public:
} }
/** /// Compute the angle between two other vectors.
* Compute the angle between two other vectors. /// @param other Another vector.
* @param other Another vector. /// @return The angle in radians between the two vectors.
* @return The angle in radians between the two vectors.
*/
T T
angle(const Vector<T, N> &other) const angle(const Vector<T, N> &other) const
{ {
@ -143,11 +128,9 @@ public:
} }
/** /// Determine whether two vectors are parallel.
* Determine whether two vectors are parallel. /// @param other Another vector
* @param other Another vector /// @return True if the angle between the vectors is zero.
* @return True if the angle between the vectors is zero.
*/
bool bool
isParallel(const Vector<T, N> &other) const isParallel(const Vector<T, N> &other) const
{ {
@ -164,12 +147,10 @@ public:
} }
/** /// Determine if two vectors are orthogonal or perpendicular to each
* Determine if two vectors are orthogonal or perpendicular to each /// other.
* other. /// @param other Another vector
* @param other Another vector /// @return True if the two vectors are orthogonal.
* @return True if the two vectors are orthogonal.
*/
bool bool
isOrthogonal(const Vector<T, N> &other) const isOrthogonal(const Vector<T, N> &other) const
{ {
@ -181,12 +162,10 @@ public:
} }
/** /// Project this vector onto some basis vector.
* Project this vector onto some basis vector. /// @param basis The basis vector to be projected onto.
* @param basis The basis vector to be projected onto. /// @return A vector that is the projection of this onto the basis
* @return A vector that is the projection of this onto the basis /// vector.
* vector.
*/
Vector Vector
projectParallel(const Vector<T, N> &basis) const projectParallel(const Vector<T, N> &basis) const
{ {
@ -196,13 +175,11 @@ public:
} }
/** /// Project this vector perpendicularly onto some basis vector.
* Project this vector perpendicularly onto some basis vector. /// This is also called the rejection of the vector.
* This is also called the rejection of the vector. /// @param basis The basis vector to be projected onto.
* @param basis The basis vector to be projected onto. /// @return A vector that is the orthogonal projection of this onto
* @return A vector that is the orthogonal projection of this onto /// the basis vector.
* the basis vector.
*/
Vector Vector
projectOrthogonal(const Vector<T, N> &basis) projectOrthogonal(const Vector<T, N> &basis)
{ {
@ -211,12 +188,10 @@ public:
} }
/** /// Compute the cross product of two vectors. This is only defined
* Compute the cross product of two vectors. This is only defined /// over three-dimensional vectors.
* over three-dimensional vectors. /// @param other Another 3D vector.
* @param other Another 3D vector. /// @return The cross product vector.
* @return The cross product vector.
*/
Vector Vector
cross(const Vector<T, N> &other) const cross(const Vector<T, N> &other) const
{ {
@ -229,12 +204,10 @@ public:
} }
/** /// Perform vector addition with another vector.
* Perform vector addition with another vector. /// @param other The vector to be added.
* @param other The vector to be added. /// @return A new vector that is the result of adding this and the
* @return A new vector that is the result of adding this and the /// other vector.
* other vector.
*/
Vector Vector
operator+(const Vector<T, N> &other) const operator+(const Vector<T, N> &other) const
{ {
@ -248,12 +221,10 @@ public:
} }
/** /// Perform vector subtraction with another vector.
* Perform vector subtraction with another vector. /// @param other The vector to be subtracted from this vector.
* @param other The vector to be subtracted from this vector. /// @return A new vector that is the result of subtracting the
* @return A new vector that is the result of subtracting the /// other vector from this one.
* other vector from this one.
*/
Vector Vector
operator-(const Vector<T, N> &other) const operator-(const Vector<T, N> &other) const
{ {
@ -267,11 +238,9 @@ public:
} }
/** /// Perform scalar multiplication of this vector by some scale factor.
* Perform scalar multiplication of this vector by some scale factor. /// @param k The scaling value.
* @param k The scaling value. /// @return A new vector that is this vector scaled by k.
* @return A new vector that is this vector scaled by k.
*/
Vector Vector
operator*(const T k) const operator*(const T k) const
{ {
@ -285,11 +254,9 @@ public:
} }
/** /// Perform scalar division of this vector by some scale factor.
* Perform scalar division of this vector by some scale factor. /// @param k The scaling value
* @param k The scaling value /// @return A new vector that is this vector scaled by 1/k.
* @return A new vector that is this vector scaled by 1/k.
*/
Vector Vector
operator/(const T k) const operator/(const T k) const
{ {
@ -303,11 +270,9 @@ public:
} }
/** /// Compute the dot product between two vectors.
* Compute the dot product between two vectors. /// @param other The other vector.
* @param other The other vector. /// @return A scalar value that is the dot product of the two vectors.
* @return A scalar value that is the dot product of the two vectors.
*/
T T
operator*(const Vector<T, N> &other) const operator*(const Vector<T, N> &other) const
{ {
@ -321,12 +286,10 @@ public:
} }
/** /// Compare two vectors for equality.
* Compare two vectors for equality. /// @param other The other vector.
* @param other The other vector. /// @return Return true if all the components of both vectors are
* @return Return true if all the components of both vectors are /// within the tolerance value.
* within the tolerance value.
*/
bool bool
operator==(const Vector<T, N> &other) const operator==(const Vector<T, N> &other) const
{ {
@ -339,12 +302,10 @@ public:
} }
/** /// Compare two vectors for inequality.
* Compare two vectors for inequality. /// @param other The other vector.
* @param other The other vector. /// @return Return true if any of the components of both vectors are
* @return Return true if any of the components of both vectors are /// not within the tolerance value.
* not within the tolerance value.
*/
bool bool
operator!=(const Vector<T, N> &other) const operator!=(const Vector<T, N> &other) const
{ {
@ -352,11 +313,9 @@ public:
} }
/** /// Support array indexing into vector.
* Support array indexing into vector. /// @param i The component index.
* @param i The component index. /// @return The value of the vector component at i.
* @return The value of the vector component at i.
*/
T T
operator[](size_t i) const operator[](size_t i) const
{ {
@ -364,12 +323,10 @@ public:
} }
/** /// Support outputting vectors in the form "<i, j, ...>".
* Support outputting vectors in the form "<i, j, ...>". /// @param outs An output stream.
* @param outs An output stream. /// @param vec The vector to be formatted.
* @param vec The vector to be formatted. /// @return The output stream.
* @return The output stream.
*/
friend std::ostream& friend std::ostream&
operator<<(std::ostream& outs, const Vector<T, N>& vec) operator<<(std::ostream& outs, const Vector<T, N>& vec)
{ {
@ -390,19 +347,37 @@ private:
std::array<T, N> arr; std::array<T, N> arr;
}; };
///
/// \defgroup vector_aliases Vector type aliases.
///
/** /// \ingroup vector_aliases
* A number of shorthand aliases for vectors are provided. They follow /// 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. /// the form of VectorNt, where N is the dimension and t is the type.
* For example, a 2D float vector is Vector2f. /// 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; typedef Vector<float, 2> Vector2f;
/// \ingroup vector_aliases
/// @brief Type alias for a three-dimensional float vector.
typedef Vector<float, 3> Vector3f; typedef Vector<float, 3> Vector3f;
/// \ingroup vector_aliases
/// @brief Type alias for a four-dimensional float vector.
typedef Vector<float, 4> Vector4f; typedef Vector<float, 4> Vector4f;
/// \ingroup vector_aliases
/// @brief Type alias for a two-dimensional double vector.
typedef Vector<double, 2> Vector2d; typedef Vector<double, 2> Vector2d;
/// \ingroup vector_aliases
/// @brief Type alias for a three-dimensional double vector.
typedef Vector<double, 3> Vector3d; typedef Vector<double, 3> Vector3d;
/// \ingroup vector_aliases
/// @brief Type alias for a four-dimensional double vector.
typedef Vector<double, 4> Vector4d; 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 #ifndef __WRMATH_UTIL_MATH_H
#define __WRMATH_UTIL_MATH_H #define __WRMATH_UTIL_MATH_H
@ -9,56 +10,42 @@ namespace wr {
namespace math { namespace math {
/** /// Convert radians to degrees.
* Convert radians to degrees. /// @param rads the angle in radians
* @param rads the angle in radians /// @return the angle in degrees.
* @return the angle in degrees.
*/
float RadiansToDegreesF(float rads); float RadiansToDegreesF(float rads);
/** /// Convert radians to degrees.
* Convert radians to degrees. /// @param rads the angle in radians
* @param rads the angle in radians /// @return the angle in degrees.
* @return the angle in degrees.
*/
double RadiansToDegreesD(double rads); double RadiansToDegreesD(double rads);
/** /// Convert degrees to radians.
* Convert degrees to radians. /// @param degrees the angle in degrees
* @param degrees the angle in degrees /// @return the angle in radians.
* @return the angle in radians.
*/
float DegreesToRadiansF(float degrees); float DegreesToRadiansF(float degrees);
/** /// Convert degrees to radians.
* Convert degrees to radians. /// @param degrees the angle in degrees
* @param degrees the angle in degrees /// @return the angle in radians.
* @return the angle in radians.
*/
double DegreesToRadiansD(double degrees); double DegreesToRadiansD(double degrees);
/** /// Get the default epsilon value.
* Get the default epsilon value. /// @param epsilon The variable to store the epsilon value in.
* @param epsilon The variable to store the epsilon value in.
*/
void DefaultEpsilon(double &epsilon); void DefaultEpsilon(double &epsilon);
/** /// Get the default epsilon value.
* Get the default epsilon value. /// @param epsilon The variable to store the epsilon value in.
* @param epsilon The variable to store the epsilon value in.
*/
void DefaultEpsilon(float &epsilon); void DefaultEpsilon(float &epsilon);
/** /// Return whether the two values of type T are equal to within some tolerance.
* Return whether the two values of type T are equal to within some tolerance. /// @tparam T The type of value
* @tparam T The type of value /// @param a A value of type T used as the left-hand side of an equality check.
* @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 b A value of type T used as the right-hand side of an equality check. /// @param epsilon The tolerance value.
* @param epsilon The tolerance value. /// @return Whether the two values are "close enough" to be considered equal.
* @return Whether the two values are "close enough" to be considered equal.
*/
template <typename T> template <typename T>
static T static T
WithinTolerance(T a, T b, T epsilon) 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> #include <wrmath/geom/orientation.h>
@ -9,7 +9,7 @@ namespace geom {
float float
Heading2f(Vector2f vec) Heading2f(Vector2f vec)
{ {
return vec.angle(Basis2f[Basis_i]); return vec.angle(Basis2f[Basis_x]);
} }
@ -24,7 +24,7 @@ Heading3f(Vector3f vec)
double double
Heading2d(Vector2d vec) 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; using namespace wr;
TEST(Quaternion, SelfTest)
{
geom::Quaternion_SelfTest();
}
TEST(Quaterniond, Addition) TEST(Quaterniond, Addition)
{ {
geom::Quaterniond p(geom::Vector4d {1.0, -2.0, 1.0, 3.0}); 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 int
main(int argc, char **argv) main(int argc, char **argv)
{ {