Program Listing for File HomTran.h#
↰ Return to documentation for file (include/Karana/Math/HomTran.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 HomTran class.
*/
#pragma once
#include "Karana/Math/Defs.h"
#include "Karana/Math/SpatialVector.h"
#include "Karana/Math/UnitQuaternion.h"
namespace Karana::Math {
using namespace Eigen;
/**
* @class HomTran
*
* @brief Represents a 3D homogeneous transformation for defining
* frame poses. It uses a unit quaternion for the attitude and a
* 3-vector for the translation.
*
* Homogeneous transforms are a member of the SE(3) Lie group. They
* are used to define the relative pose of pairs for frames. See
* the \sref{definition of a homogeneous transform,homtranDef_anchor} for
* the the definition of a homogeneous transform. Also, see the
* \sref{attitude_sec} section for more discussion on attitude
* representations.
*/
class HomTran {
public:
/** @brief Default constructor */
HomTran();
/**
* @brief Construct a homogeneous transform with pure translation.
* @param vec Translation vector.
* @param epsilon Tolerance for checking whether translation is zero.
*/
HomTran(const Vec3 &vec, double epsilon = MATH_EPSILON);
/**
* @brief Construct a homogeneous transform with pure rotation.
* @param q Unit quaternion representing rotation.
* @param epsilon Tolerance for checking whether the quaternion is identity.
*/
HomTran(const UnitQuaternion &q, double epsilon = MATH_EPSILON);
/**
* @brief Construct a general homogeneous transform with rotation and translation.
* @param q Unit quaternion representing rotation.
* @param vec Translation vector.
* @param epsilon Tolerance for checking whether translation is zero and unit quaternion is
* identity.
*/
HomTran(const UnitQuaternion &q, const Vec3 &vec, double epsilon = MATH_EPSILON);
/**
* @brief Copy constructor
* @param T HomTran to copy
*/
HomTran(const HomTran &T);
/**
* @brief Copy assignment operator
* @param T HomTran to copy
* @return A copy of T
*/
HomTran &operator=(const HomTran &T);
/**
* @brief Move constructor
* @param T HomTran to move
*/
HomTran(HomTran &&T) noexcept;
/**
* @brief Move assignment operator
* @param T HomTran to move
* @return The moved T.
*/
HomTran &operator=(HomTran &&T) noexcept;
/**
* @brief Equality operator.
* @param other Another HomTran instance.
* @return True if both transforms are equal.
*/
bool operator==(const HomTran &other) const;
/**
* @brief Check if two transforms are approximately equal.
* @param other The other HomTran to compare with.
* @param prec Tolerance for comparison.
* @return True if approximately equal.
*/
bool isApprox(const HomTran &other, double prec = MATH_EPSILON) const;
/**
* @brief Get a string representing the type.
* @return The string "HomTran".
*/
std::string_view typeString() const;
/**
* @brief Set the translation vector.
* @param vec The translation vector.
* @param epsilon Tolerance for checking whether the translation is zero.
*/
void setTranslation(const Vec3 &vec, double epsilon = MATH_EPSILON);
/**
* @brief Mark the transform as not ready.
*/
void makeNotReady();
/**
* @brief Check whether the transform has been initialized.
* @return True if initialized.
*/
bool isReady() const;
/**
* @brief Get a string representation of the transform.
*
* @param prefix Optional 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 dump of the transform.
*/
std::string dumpString(std::string_view prefix = "",
unsigned int precision = 6,
DumpFormatType format_type = DumpFormatType::DEFAULT_FLOAT) const;
/**
* @brief Print transform information to standard output.
*
* @param prefix Optional 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 Set the unit quaternion (rotation part).
* @param q Unit quaternion.
* @param epsilon Tolerance for checking whether the unit quaternion is identity.
*/
void setUnitQuaternion(const UnitQuaternion &q, double epsilon = MATH_EPSILON);
/**
* @brief Get the 4x4 homogeneous transformation matrix representation.
* @return The transformation matrix.
*/
Mat44 getMatrix() const;
/**
* @brief Get the translation vector.
* @return Translation vector.
*/
const Vec3 &getTranslation() const;
/**
* @brief Get the unit quaternion.
* @return Unit quaternion.
*/
const UnitQuaternion &getUnitQuaternion() const;
/**
* @brief Convert to a 6D vector [rotation_vector; translation].
* @return The 6D vector representation.
*/
Vec6 toVector6() const;
/**
* @brief Set the transform to identity.
*/
void setIdentity();
/**
* @brief Check if transform is identity.
* @return True if identity.
*/
bool isIdentity() const;
/**
* @brief Check if transform has translation.
* @return True if translation is non-zero.
*/
bool hasTranslation() const;
/**
* @brief Check if transform has rotation.
* @return True if the unit quaternion is non-identity.
*/
bool hasRotation() const;
/**
* @brief Multiply two transforms.
* @param T The other transform to multiply with.
* @return Product transform.
*/
HomTran operator*(const HomTran &T) const;
/**
* @brief Apply transform to a 3D vector.
* @param v Input vector.
* @return Transformed vector.
*/
Vec3 operator*(const Vec3 &v) const;
/**
* @brief Get the inverse of this transform.
* @return Inverse transform.
*/
HomTran inverse() const;
/**
* @brief Apply the "phi" rigid body transform to a spatial vector.
*
* See \sref{rigid body transformation matrix,phiDef} for a
* definition of the transformation matrix.
*
* @param V Spatial vector.
* @return Transformed spatial vector.
* @see phiStar(), phiMatrix()
*/
SpatialVector phi(const SpatialVector &V) const;
/**
* @brief Apply "phi" rigid body transform to a 6D vector.
*
* See \sref{rigid body transformation matrix,phiDef} for a
* definition of the transformation matrix.
*
* @param V Input 6-vector.
* @return Transformed 6-vector.
* @see phiStar(), phiMatrix()
*/
SpatialVector phiVec(const Vec6 &V) const;
/**
* @brief Apply phi transform to a 6xN matrix.
*
* See \sref{rigid body transformation matrix,phiDef} for a
* definition of the transformation matrix.
*
* @param m Input matrix.
* @return Transformed matrix.
* @see phiStar(), phiMatrix()
*/
Mat6n phiMat(const Mat6n &m) const;
/**
* @brief Compute phi * m * phi^* symmetric rigid body transformation to a 6x6 matrix.
*
* See \sref{rigid body transformation matrix,phiDef} for a
* definition of the transformation matrix.
*
* @param m Input matrix.
* @return Transformed symmetric matrix.
* @see phiStar(), phiMatrix()
*/
Mat66 phiSymmetric(const Mat66 &m) const;
/**
* @brief Apply the phi^* dual rigid body transformation to a spatial vector.
*
* See \sref{rigid body transformation matrix,phiDef} for a
* definition of the transformation matrix.
*
* @param V Input spatial vector.
* @return Transformed vector.
* @see phi(), phiMatrix()
*/
SpatialVector phiStar(const SpatialVector &V) const;
/**
* @brief Apply the phi^* dual rigid body transformation to a 6D vector.
*
* See \sref{rigid body transformation matrix,phiDef} for a
* definition of the transformation matrix.
*
* @param V Input 6-vector.
* @return Transformed 6-vector.
* @see phi(), phiMatrix()
*/
SpatialVector phiStarVec(const Vec6 &V) const;
/**
* @brief Apply the phi^* dual rigid body transformation to a 6xN matrix.
*
* See \sref{rigid body transformation matrix,phiDef} for a
* definition of the transformation matrix.
*
* @param m Input matrix.
* @return Transformed matrix.
*/
Mat6n phiStarMat(const Mat6n &m) const;
/**
* @brief Compute the phi^* * m * phi dual rigid body symmetric transformation for a 6x6
* matrix.
*
* See \sref{rigid body transformation matrix,phiDef} for a
* definition of the transformation matrix.
*
* @param m Input matrix.
* @return Transformed symmetric matrix.
* @see phi(), phiMatrix()
*/
Mat66 phiStarSymmetric(const Mat66 &m) const;
/**
* @brief Get the phi rigid body transformation matrix.
*
* See \sref{rigid body transformation matrix,phiDef} for a
* definition of the transformation matrix.
*
* @return Phi matrix.
* @see phi(), phiStar()
*/
const Mat66 &phiMatrix() const;
private:
/// @brief Update the cached rigid body transformation phi matrix
void _updatePhiMatrix() const;
/// @brief Check if the quaternion is non-identity.
void _checkUnitQuaternion(double epsilon = MATH_EPSILON);
/// @brief Check if the translation vector is non-zero.
void _checkTranslation(double epsilon = MATH_EPSILON);
UnitQuaternion _q = UnitQuaternion(0, 0, 0, 1, false, true); ///< Rotation component
Vec3 _vec = Vec3(0, 0, 0); ///< Translation component
bool _has_rotation = false,
_has_translation = false; ///< Flags indicating non-identity components
mutable bool _cached_phi_matrix_flag = false; ///< Cache flag
mutable Mat66 _cached_phi_matrix; ///< Cached phi matrix
};
} // namespace Karana::Math