Program Listing for File UnitQuaternion.h#
↰ Return to documentation for file (include/Karana/Math/UnitQuaternion.h)
/*
* Copyright (c) 2024-2026 Karana Dynamics Pty Ltd. All rights reserved.
*
* NOTICE TO USER:
*
* This source code and/or documentation (the "Licensed Materials") is
* the confidential and proprietary information of Karana Dynamics Inc.
* Use of these Licensed Materials is governed by the terms and conditions
* of a separate software license agreement between Karana Dynamics and the
* Licensee ("License Agreement"). Unless expressly permitted under that
* agreement, any reproduction, modification, distribution, or disclosure
* of the Licensed Materials, in whole or in part, to any third party
* without the prior written consent of Karana Dynamics is strictly prohibited.
*
* THE LICENSED MATERIALS ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND.
* KARANA DYNAMICS DISCLAIMS ALL WARRANTIES, EXPRESS OR IMPLIED, INCLUDING
* BUT NOT LIMITED TO WARRANTIES OF MERCHANTABILITY, NON-INFRINGEMENT, AND
* FITNESS FOR A PARTICULAR PURPOSE.
*
* IN NO EVENT SHALL KARANA DYNAMICS BE LIABLE FOR ANY DAMAGES WHATSOEVER,
* INCLUDING BUT NOT LIMITED TO LOSS OF PROFITS, DATA, OR USE, EVEN IF
* ADVISED OF THE POSSIBILITY OF SUCH DAMAGES, WHETHER IN CONTRACT, TORT,
* OR OTHERWISE ARISING OUT OF OR IN CONNECTION WITH THE LICENSED MATERIALS.
*
* U.S. Government End Users: The Licensed Materials are a "commercial item"
* as defined at 48 C.F.R. 2.101, and are provided to the U.S. Government
* only as a commercial end item under the terms of this license.
*
* Any use of the Licensed Materials in individual or commercial software must
* include, in the user documentation and internal source code comments,
* this Notice, Disclaimer, and U.S. Government Use Provision.
*/
/**
* @file
* @brief Contains the declarations for the UnitQuaternion class.
*/
#pragma once
#include "Karana/Math/Defs.h"
#include "Karana/Math/RotationMatrix.h"
#include "Karana/Math/RotationVector.h"
#include "Karana/Math/SpatialVector.h"
#include <unsupported/Eigen/EulerAngles>
namespace Karana::Math {
using namespace Eigen;
/** Unit quaternion class */
/**
* @class UnitQuaternion
* @brief Class for unit quaternions attitude representations.
*
* See the \sref{attitude_sec} section for more discussion on attitude
* representations.
*/
class UnitQuaternion {
// Constructors
public:
/**
* @brief Constructor from 4 scalar coefficients
*
* The input 4 values are ordered to contain the vector values first,
* followed by the scalar value.
*
* @param x the first element of the vector part
* @param y the second element of the vector part
* @param z the third element of the vector part
* @param w the scalar element
* @param renormalize if true, normalize the input values
* @param skip_values_check if true, skip check that the quaternion is unit
* norm
*
*/
UnitQuaternion(double x,
double y,
double z,
double w,
bool renormalize = false,
bool skip_values_check = false);
/**
* @brief Constructor from a Eigen::Quaterniond input
*
* Note that the Eigen class is for general quaternions, not just
* unit quaternions. So we need to turn on checks by default.
*
* @param q Eigen Quaterniond.
* @param renormalize if true, normalize the input values
* @param skip_values_check if true, skip check that the quaternion is unit
* norm
*
*/
UnitQuaternion(const Quaterniond &q,
bool renormalize = false,
bool skip_values_check = false);
/**
* @brief Constructor from a RotationVector
*
* @param rv RotationVector to create this UnitQuaternion from.
*/
UnitQuaternion(const RotationVector &rv);
/**
* @brief Constructor from a 4-vector of coefficients
*
* The input 4-vector is assumed to contain the vector values first,
* followed by the scalar value.
*
* @param v the 4-vector input
* @param renormalize if true, normalize the input values
* @param skip_values_check if true, skip check that the quaternion is unit
* norm
*
*/
UnitQuaternion(const Vec4 &v, bool renormalize = false, bool skip_values_check = false);
/**
* @brief Constructor from angle/axis representation
*
* Note that angle/axis representation must be a valid one with a
* normalized vector for the axis component. Since the AngleAxis
* class does not guarantee this, we have to enable checks by
* default.
*
* @param aa the angle/axis input
*
*/
UnitQuaternion(const Eigen::AngleAxis<double> &aa);
/**
* @brief Constructor from pair of to/from 3-vectors
*
* Compute the quaternion to transform one vector to another
* one. The normalized versions of the vectors are used, the input
* vectors do not have to be unit norm. Assuming the input vectors
* are unit norm, then we will have: v2 = quat * v1.
*
* @param v1 The 'from' vector
* @param v2 The 'to' vector
*
*/
UnitQuaternion(const Vec3 &v1, const Vec3 &v2);
/**
* @brief Constructor from a rotation matrix
*
* @param rot Input rotation matrix
*
*/
UnitQuaternion(const RotationMatrix &rot);
/**
* @brief Constructor from EulerAngles
*
* @param ea Euler angles to construct this UnitQuaternion from.
*/
template <typename euler_system>
UnitQuaternion(const EulerAngles<double, euler_system> &ea)
: UnitQuaternion(
Quaterniond(ea), false /* renormalize */, false /*do not skip_values_check */) {}
/**
* @brief Get the type string of this class.
* @return "UnitQuaternion"
*/
std::string_view typeString() const;
/**
* @brief Get a string representation of the UnitQuaternion.
*
* @param prefix String prefix for each line.
* @param precision Number of digits to use for floating point values.
* @param format_type The format type to use.
* @return String with information about this UnitQuaternion.
*/
std::string dumpString(std::string_view prefix = "",
unsigned int precision = 10,
DumpFormatType format_type = DumpFormatType::DEFAULT_FLOAT) const;
/**
* @brief Print dumpString to std::cout.
*
* @param prefix String prefix for each line.
* @param precision Number of digits to use for floating point values.
* @param format_type The format type to use.
*/
void dump(std::string_view prefix = "",
unsigned int precision = 10,
DumpFormatType format_type = DumpFormatType::DEFAULT_FLOAT) const;
/**
* @brief Mark this UnitQuaternion as not ready.
*/
void makeNotReady();
/**
* @brief Check whether this UnitQuaternion is initialized.
* @return True if initialized, false otherwise.
*/
bool isReady() const;
// Copy constructor, move constructor, and assignment operators
public:
/**
* @brief Copy constructor
*
* @param other The other quaternion to copy from.
*/
UnitQuaternion(const UnitQuaternion &other);
/**
* @brief Move constructor
*
* @param other The other quaternion to move from.
*/
UnitQuaternion(UnitQuaternion &&other) noexcept;
/**
* @brief Copy assignment operator.
*
* @param other UnitQuaternion to copy from.
* @return Reference to this object.
*/
UnitQuaternion &operator=(const UnitQuaternion &other);
/**
* @brief Move assignment operator.
*
* @param other UnitQuaternion to move from.
* @return Reference to this object.
*/
UnitQuaternion &operator=(UnitQuaternion &&other) noexcept;
public:
/**
* @brief Return the native Eigen representation for the quaternion.
*
* @return the Quaterniond representation
*/
const Quaterniond &asEigen() const;
/**
* @brief Return the rotation matrix for this quaternion.
*
* The rotation matrix is recomputed if the current value is not healthy,
* else the cached value is returned.
*
* @return The rotation matrix representation of this UnitQuaternion.
*/
const RotationMatrix &toRotationMatrix() const;
/**
* @brief Convert to EulerAngles with the specified EulerSystem.
*
* @return EulerAngles for this quaternion.
*/
template <typename euler_system> EulerAngles<double, euler_system> toEulerAngles() {
return EulerAngles<double, euler_system>(toRotationMatrix());
}
/**
* @brief Check whether this quaternion is the identity quaternion
*
* @param epsilon the tolerance value
* @return true, if the quaternion is the identity quaternion; false otherwise.
*
*/
bool isIdentity(double epsilon = MATH_EPSILON) const;
/** Set the unit quaternion to the identity value */
void setIdentity();
/**
* @brief Check whether this quaternion is exactly the same as another.
*
* @param other the other quaternion
* @return true, if the quaternions are the same
*
*/
bool operator==(const UnitQuaternion &other) const;
/**
* @brief Check whether this quaternion is approximately the same as another.
*
* This method will also return true if the coefficients are
* approximately negative of all the coefficients of the other
* (since both unit quaternions then represent the same rotation).
*
* @param other the other quaternion
* @param prec the tolerance value
* @return true, if the quaternions are the same within the specified
* tolerance
*
*/
bool isApprox(const UnitQuaternion &other, double prec = MATH_EPSILON) const;
/**
* @brief Product with another quaternion
*
* @param other the other quaternion
* @return The resulting quaternion from the product
*/
UnitQuaternion operator*(const UnitQuaternion &other) const;
/**
* @brief Rotate the input 3-vector
*
* @param vec 3-vector to transform
* @return Transformed vector
*
*/
Vec3 operator*(const Vec3 &vec) const;
/**
* @brief Rotate the input spatial vector
*
* @param vec spatial vector to transform
* @return Transformed vector
*
*/
SpatialVector operator*(const SpatialVector &vec) const;
/**
* @brief Return the inverse of the quaternion
*
* @return the inverse quaternion
*
*/
UnitQuaternion inverse() const;
/**
* @brief Return the 3-vector part of the quaternion coefficients
*
* @return 3-vector coefficients.
*
*/
const Eigen::Ref<const Vec3> vec() const;
/**
* @brief Return the angle/axis representation for the quaternion.
*
* @return angle/axis representation
*
*/
Eigen::AngleAxis<double> toAngleAxis() const;
/**
* @brief Return the RotationVector representation for the quaternion.
*
* @return RotationVector representation
*
*/
RotationVector toRotationVector() const;
/**
* @brief Return the quaternion elements as a 4-vector.
*
* Note that the vector elements are in the beginning, and the
* last element is the scalar.
*
* @return 4-vector of coefficients.
*
*/
const Vec4 &toVector4() const;
/**
* @brief Apply a rotational transformation symmetrically to a 3x3 matrix.
*
* Multiply the input 3x3 matrix from the left by R, and the right
* by R transpose: R * M * R_T. Here, R denotes the 3x3 rotation
* matrix for this quaternion.
*
* @param mat 3x3 matrix to transform
* @return Transformed matrix
*
* @see inverseRotate66()
*/
Mat33 rotate(const Mat33 &mat) const;
/**
* @brief Apply a rotational transformation from the left to a 6xN matrix.
*
* Multiply the input 6xN matrix from the left as (R_block_diag) *
* M. Here, R_block_diag denotes a 6x6 matrix containing the 3x3
* rotation matrix for this quaternion repeated along the block
* diagonal.
*
* @param mat 6xN matrix to transform
* @return Transformed matrix
*
* @see rotateRightN6()
*/
Mat6n rotateLeft6N(const Mat6n &mat) const;
/**
* @brief Apply a rotational transformation from the right to a Nx6 matrix.
*
* Multiply the input Nx6 matrix from the right as M *
* (R_block_diag). Here, R_block_diag denotes a 6x6 matrix
* containing the 3x3 rotation matrix for this quaternion repeated
* along the block diagonal.
*
* @param mat Nx6 matrix to transform
* @return Transformed matrix
*
* @see rotateLeft6N()
*/
Mat rotateRightN6(const Mat &mat) const;
/**
* @brief Apply a rotational transformation symmetrically to a 6x6 matrix.
*
* Multiply the input 6x6 matrix from both sides by (R_block_diag) * M *
* (R_block_diag)_T. Here, R_block_diag denotes a 6x6 matrix containing
* the 3x3 rotation matrix for this quaternion repeated along the block
* diagonal.
*
* @param mat 6x6 matrix to transform
* @return Transformed matrix
*
* @see inverseRotate66()
*/
Mat66 rotate66(const Mat66 &mat) const;
/**
* @brief Compute the quaternion coeff rates for an angular velocity
*
* @param omega the input angular velocity
* @param oframe if true, the angular velocity is in the 'from' frame, else
* the 'to' frame
*
* @return the 4-vector with coefficient rates
*
* @see vectorRatesToOmegaMap(), omegaToVectorRatesMap(), omegaToScalarRateMap()
*
*/
Vec4 omegaToRates(const Vec3 &omega, bool oframe) const;
/**
* @brief The 3x3 matrix that maps the quaternion vector rates to the angular
* velocity
*
* @param oframe if true, the angular velocity is in the 'from' frame, else
* the 'to' frame
* @param epsilon tolerance to check if close to 180 degree rotation
*
* @return the 3x3 matrix map
*
* @see vectorRatesToOmegaMap(), omegaToVectorRatesMap(), omegaToScalarRateMap()
*
*/
Mat33 vectorRatesToOmegaMap(bool oframe, double epsilon = MATH_EPSILON) const;
/**
* @brief The 3x3 matrix that maps the angular velocity to the quaternion
* vector rates
*
* @param oframe if true, the angular velocity is in the 'from' frame, else in
* the 'to' frame
*
* @return the 3x3 matrix map
*
* @see vectorRatesToOmegaMap(), omegaToVectorRatesMap(), omegaToScalarRateMap()
*
*/
Mat33 omegaToVectorRatesMap(bool oframe) const;
/**
* @brief The 3-vector whose dot product with the angular velocity yields the
* scalar coeff rates
*
* This expression is the same whether the angular velocity is in the oframe
* or not
*
* @return the 3 vector map
*
* @see vectorRatesToOmegaMap(), omegaToVectorRatesMap(), omegaToScalarRateMap()
*
*/
Vec3 omegaToScalarRateMap() const;
/**
* @brief The 3x4 matrix that maps the coefficient rates to the angular velocity
* @param oframe if true, the angular velocity is in the 'from' frame, else the 'to' frame.
* @return the 3x4 matrix map
*/
Mat ratesToOmegaMap(bool oframe) const;
/**
* @brief The 4x3 matrix that maps the angular velocity to the coefficient rates
* @param oframe if true, the angular velocity is in the 'from' frame, else the 'to' frame
* @return the 4x3 matrix map
*/
Mat omegaToRatesMap(bool oframe) const;
/**
* @brief The 3x4 matrix that maps quaternion rates to rot vector rates
*
* @return the 3x4 matrix map
*/
Mat ratesToRotVecRatesMap() const;
private:
/** Helper method to process/check the current values
*
* @param renormalize - Renormalizes the quaternion.
* @param skip_values_check - Check that the values are unit norm.*/
void _setValues(bool renormalize = false, bool skip_values_check = false);
/** Verify that the values in the quaternion are unity norm */
void _isUnitNorm(double epsilon = MATH_EPSILON) const;
//! the Eigen quaternion with the actual data
Quaterniond _quat = Quaterniond(1.0, 0.0, 0.0, 0.0);
//! caching of the associated rotation matrix
mutable bool _cached_rot_mat_flag = false;
mutable RotationMatrix _cached_rot_mat;
// bool _is_identity = true;
};
} // namespace Karana::Math