From 71a6f5e1284b985c91089a5def532892bfe9dbe3 Mon Sep 17 00:00:00 2001 From: Kyle Isom Date: Mon, 5 Aug 2019 22:50:28 -0700 Subject: [PATCH] Clean up and document code. --- CMakeLists.txt | 2 + docs/Doxyfile | 10 +- include/wrmath/geom/orientation.h | 39 ++- include/wrmath/geom/quaternion.h | 465 ++++++++++++++---------------- include/wrmath/geom/vector.h | 235 +++++++-------- include/wrmath/math.h | 59 ++-- src/orientation.cc | 6 +- src/quaternion.cc | 108 +++++++ test/quaternion_test.cc | 16 + 9 files changed, 517 insertions(+), 423 deletions(-) create mode 100644 src/quaternion.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index f0a6da9..b1b3685 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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}) diff --git a/docs/Doxyfile b/docs/Doxyfile index da1ad62..b7eb437 100644 --- a/docs/Doxyfile +++ b/docs/Doxyfile @@ -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: diff --git a/include/wrmath/geom/orientation.h b/include/wrmath/geom/orientation.h index 831ffa6..d0449fa 100644 --- a/include/wrmath/geom/orientation.h +++ b/include/wrmath/geom/orientation.h @@ -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 diff --git a/include/wrmath/geom/quaternion.h b/include/wrmath/geom/quaternion.h index 6a3c441..044afa7 100644 --- a/include/wrmath/geom/quaternion.h +++ b/include/wrmath/geom/quaternion.h @@ -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 #include +#include #include #include #include - +/// 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 +/// @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 , 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 class Quaternion { public: - /** - * The default Quaternion constructor returns an identity quaternion. - */ - Quaternion() : v(Vector {0.0, 0.0, 0.0}), w(1.0) + /// + /// The default Quaternion constructor returns an identity quaternion. + /// + Quaternion() : v(Vector{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 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 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 _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 comprised of the axis of rotation - * followed by the angle of rotation. - * @param vector A vector in the form . - */ + /// + /// A Quaternion may be initialised with a Vector comprised of + /// the axis of rotation followed by the angle of rotation. + /// @param vector A vector in the form . + /// Quaternion(Vector vector) : - v(Vector {vector[0], vector[1], vector[2]}), + v(Vector{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 . + Quaternion(std::initializer_list ilst) + { + auto it = ilst.begin(); - /** - * Set the comparison tolerance for this quaternion. - * @param epsilon A tolerance value. - */ + this->v = Vector{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 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, 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, 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 {-this->v[0], -this->v[1], -this->v[2], this->w}); + return Quaternion(Vector{-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, with the axis of rotation - * followed by the angle of rotation. - * @return A vector representation of the quaternion. - */ + /// Return the quaternion as a Vector, with the axis of rotation + /// followed by the angle of rotation. + /// @return A vector representation of the quaternion. Vector asVector() const { - return Vector {this->v[0], this->v[1], this->v[2], this->w}; + return Vector{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 rotate(Vector v) const { @@ -186,33 +189,30 @@ public: } - /** - * Return the Euler angles for this quaternion as a vector of - * . Users of this function should watch out - * for gimball lock. - * @return A vector containing - */ + /// Return the Euler angles for this quaternion as a vector of + /// . Users of this function should watch out + /// for gimbal lock. + /// @return A vector containing Vector 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 {yaw, pitch, roll}; + + return Vector{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 &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 &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 &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 &other) const { - T angle = (this->w * other.w) - - (this->v * other.v); - Vector 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 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 &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 &other) const { @@ -312,27 +299,25 @@ public: } - /** - * Support stream output of a quaternion in the form `a + `. - * 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& q) + /// Support stream output of a quaternion in the form `a + `. + /// \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 &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 v; // axis of rotation - T w; // angle of rotation - T eps; + Vector 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 Quaternionf; -typedef Quaternion 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 Quaternionf; + +/// \ingroup quaternion_aliases +/// Type alias for a double Quaternion. +typedef Quaternion 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 +Quaternion LERP(Quaternion p, Quaternion 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 +Quaternion ShortestSLERP(Quaternion p, Quaternion 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: diff --git a/include/wrmath/geom/vector.h b/include/wrmath/geom/vector.h index c11c913..4339803 100644 --- a/include/wrmath/geom/vector.h +++ b/include/wrmath/geom/vector.h @@ -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 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 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 &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 &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 &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 &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 &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 &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 &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 &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 &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 &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 &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 "". - * @param outs An output stream. - * @param vec The vector to be formatted. - * @return The output stream. - */ + /// Support outputting vectors in the form "". + /// @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& vec) { @@ -390,19 +347,37 @@ private: std::array 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 Vector2f; + +/// \ingroup vector_aliases +/// @brief Type alias for a three-dimensional float vector. typedef Vector Vector3f; + +/// \ingroup vector_aliases +/// @brief Type alias for a four-dimensional float vector. typedef Vector Vector4f; +/// \ingroup vector_aliases +/// @brief Type alias for a two-dimensional double vector. typedef Vector Vector2d; + +/// \ingroup vector_aliases +/// @brief Type alias for a three-dimensional double vector. typedef Vector Vector3d; + +/// \ingroup vector_aliases +/// @brief Type alias for a four-dimensional double vector. typedef Vector Vector4d; diff --git a/include/wrmath/math.h b/include/wrmath/math.h index b88fc3b..84bb9a7 100644 --- a/include/wrmath/math.h +++ b/include/wrmath/math.h @@ -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 static T WithinTolerance(T a, T b, T epsilon) diff --git a/src/orientation.cc b/src/orientation.cc index 3d33bde..0a8462e 100644 --- a/src/orientation.cc +++ b/src/orientation.cc @@ -1,4 +1,4 @@ -#include +#include #include @@ -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]); } diff --git a/src/quaternion.cc b/src/quaternion.cc new file mode 100644 index 0000000..5a568cb --- /dev/null +++ b/src/quaternion.cc @@ -0,0 +1,108 @@ +#include + + +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 +Quaternion +LERP(Quaternion p, Quaternion q, T t) +{ + return p + (q - p) * t; +} + + +template +Quaternion +ShortestSLERP(Quaternion p, Quaternion 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 diff --git a/test/quaternion_test.cc b/test/quaternion_test.cc index 56329af..d935eb9 100644 --- a/test/quaternion_test.cc +++ b/test/quaternion_test.cc @@ -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) {