Class HomTran#

Class Documentation#

class HomTran#

Represents a 3D homogeneous transformation for defining frame poses. It uses a unit quaternion for the attitude and a 3-vector for the translation.

Homogenous transforms are a member of the SE(3) Lie group. They are used to define the relative pose of pairs for frames. See the definition of a homogenous transform for the the definition of a homogeneous transform. Also, see the

Pose representations section for more discussion on attitude representations.

Public Functions

HomTran()#

Default constructor.

HomTran(const Vec3 &vec, double epsilon = MATH_EPSILON)#

Construct a homogeneous transform with pure translation.

Parameters:
  • vec – Translation vector.

  • epsilon – Tolerance for checking whether translation is zero.

HomTran(const UnitQuaternion &q, double epsilon = MATH_EPSILON)#

Construct a homogeneous transform with pure rotation.

Parameters:
  • q – Unit quaternion representing rotation.

  • epsilon – Tolerance for checking whether the quaternion is identity.

HomTran(const UnitQuaternion &q, const Vec3 &vec, double epsilon = MATH_EPSILON)#

Construct a general homogeneous transform with rotation and translation.

Parameters:
  • q – Unit quaternion representing rotation.

  • vec – Translation vector.

  • epsilon – Tolerance for checking whether translation is zero and unit quaternion is identity.

HomTran(const HomTran &T)#

Copy constructor.

Parameters:

THomTran to copy

HomTran &operator=(const HomTran &T)#

Copy assignment operator.

Parameters:

THomTran to copy

Returns:

A copy of T

HomTran(HomTran &&T) noexcept#

Move constructor.

Parameters:

THomTran to move

HomTran &operator=(HomTran &&T) noexcept#

Move assignment operator.

Parameters:

THomTran to move

Returns:

The moved T.

bool operator==(const HomTran &other) const#

Equality operator.

Parameters:

other – Another HomTran instance.

Returns:

True if both transforms are equal.

bool isApprox(const HomTran &other, double prec = MATH_EPSILON) const#

Check if two transforms are approximately equal.

Parameters:
  • other – The other HomTran to compare with.

  • prec – Tolerance for comparison.

Returns:

True if approximately equal.

std::string_view typeString() const#

Get a string representing the type.

Returns:

The string “HomTran”.

void setTranslation(const Vec3 &vec, double epsilon = MATH_EPSILON)#

Set the translation vector.

Parameters:
  • vec – The translation vector.

  • epsilon – Tolerance for checking whether the translation is zero.

void uninitialize()#

Mark the transform as uninitialized.

bool isInitialized() const#

Check whether the transform has been initialized.

Returns:

True if initialized.

std::string dumpString(std::string_view prefix = "", unsigned int precision = 6, bool exponential = false) const#

Get a string representation of the transform.

Parameters:
  • prefix – Optional prefix for each line.

  • precision – Number of digits to use for floating point values.

  • exponential – Whether to use exponential notation.

Returns:

String dump of the transform.

void dump(std::string_view prefix = "", unsigned int precision = 10, bool exponential = false) const#

Print transform information to standard output.

Parameters:
  • prefix – Optional prefix for each line.

  • precision – Number of digits to use for floating point values.

  • exponential – Whether to use exponential notation.

void setUnitQuaternion(const UnitQuaternion &q, double epsilon = MATH_EPSILON)#

Set the unit quaternion (rotation part).

Parameters:
  • q – Unit quaternion.

  • epsilon – Tolerance for checking whether the unit quaternion is identity.

Mat44 getMatrix() const#

Get the 4x4 homogeneous transformation matrix representation.

Returns:

The transformation matrix.

const Vec3 &getTranslation() const#

Get the translation vector.

Returns:

Translation vector.

const UnitQuaternion &getUnitQuaternion() const#

Get the unit quaternion.

Returns:

Unit quaternion.

Vec6 toVector6() const#

Convert to a 6D vector [rotation_vector; translation].

Returns:

The 6D vector representation.

void setIdentity()#

Set the transform to identity.

bool isIdentity() const#

Check if transform is identity.

Returns:

True if identity.

bool hasTranslation() const#

Check if transform has translation.

Returns:

True if translation is non-zero.

bool hasRotation() const#

Check if transform has rotation.

Returns:

True if the unit quaternion is non-identity.

HomTran operator*(const HomTran &T) const#

Multiply two transforms.

Parameters:

T – The other transform to multiply with.

Returns:

Product transform.

Vec3 operator*(const Vec3 &v) const#

Apply transform to a 3D vector.

Parameters:

v – Input vector.

Returns:

Transformed vector.

HomTran inverse() const#

Get the inverse of this transform.

Returns:

Inverse transform.

SpatialVector phi(const SpatialVector &V) const#

Apply the “phi” rigid body transform to a spatial vector.

See rigid body transformation matrix for a definition of the transformation matrix.

Parameters:

V – Spatial vector.

Returns:

Transformed spatial vector.

SpatialVector phiVec(const Vec6 &V) const#

Apply “phi” rigid body transform to a 6D vector.

See rigid body transformation matrix for a definition of the transformation matrix.

Parameters:

V – Input 6-vector.

Returns:

Transformed 6-vector.

Mat6n phiMat(const Mat6n &m) const#

Apply phi transform to a 6xN matrix.

See rigid body transformation matrix for a definition of the transformation matrix.

Parameters:

m – Input matrix.

Returns:

Transformed matrix.

Mat66 phiSymmetric(const Mat66 &m) const#

Compute phi * m * phi^* symmetric rigid body transformation to a 6x6 matrix.

See rigid body transformation matrix for a definition of the transformation matrix.

Parameters:

m – Input matrix.

Returns:

Transformed symmetric matrix.

SpatialVector phiStar(const SpatialVector &V) const#

Apply the phi^* dual rigid body transformation to a spatial vector.

See rigid body transformation matrix for a definition of the transformation matrix.

See also

phi(), phiMatrix()

Parameters:

V – Input spatial vector.

Returns:

Transformed vector.

SpatialVector phiStarVec(const Vec6 &V) const#

Apply the phi^* dual rigid body transformation to a 6D vector.

See rigid body transformation matrix for a definition of the transformation matrix.

See also

phi(), phiMatrix()

Parameters:

V – Input 6-vector.

Returns:

Transformed 6-vector.

Mat6n phiStarMat(const Mat6n &m) const#

Apply the phi^* dual rigid body transformation to a 6xN matrix.

See rigid body transformation matrix for a definition of the transformation matrix.

Parameters:

m – Input matrix.

Returns:

Transformed matrix.

Mat66 phiStarSymmetric(const Mat66 &m) const#

Compute the phi^* * m * phi dual rigid body symmetric transformation for a 6x6 matrix.

See rigid body transformation matrix for a definition of the transformation matrix.

See also

phi(), phiMatrix()

Parameters:

m – Input matrix.

Returns:

Transformed symmetric matrix.

const Mat66 &phiMatrix() const#

Get the phi rigid body transformation matrix.

See rigid body transformation matrix for a definition of the transformation matrix.

See also

phi(), phiStar()

Returns:

Phi matrix.