#include <transform.hpp>

Public Member Functions
	Transform (const Position &translation, const Orientation &rotation)

	Transform (Position &&translation, Orientation &&rotation)

	Transform (const CMSISMat< 3, 1 > &translation, const CMSISMat< 3, 3 > &rotation)

	Transform (CMSISMat< 3, 1 > &&translation, CMSISMat< 3, 3 > &&rotation)

	Transform (float x, float y, float z, float rx, float ry, float rz)

	Transform (const Position &translation, const Orientation &rotation, const Vector &velocity, const Vector &acceleration, const Vector &angularVelocity)

	Transform (Position &&translation, Orientation &&rotation, Vector &&velocity, Vector &&acceleration, Vector &&angularVelocity)

	Transform (const DynamicPosition &dynamicPosition, const DynamicOrientation &dynamicOrientation)

	Transform (DynamicPosition &&dynamicPosition, DynamicOrientation &&dynamicOrientation)

	Transform (const CMSISMat< 3, 1 > &translation, const CMSISMat< 3, 3 > &rotation, const CMSISMat< 3, 1 > &velocity, const CMSISMat< 3, 1 > &acceleration, const CMSISMat< 3, 3 > &angularVelocity)

	Transform (CMSISMat< 3, 1 > &&translation, CMSISMat< 3, 3 > &&rotation, CMSISMat< 3, 1 > &&velocity, CMSISMat< 3, 1 > &&acceleration, CMSISMat< 3, 3 > &&angularVelocity)

	Transform (float x, float y, float z, float vx, float vy, float vz, float ax, float ay, float az, float rx, float ry, float rz, float wx, float wy, float wz)

Position	apply (const Position &position) const
	Apply this transform to a position.

Vector	apply (const Vector &vector) const
	Rotates a vector in the base frame to a vector in the follower frame.

DynamicPosition	apply (const DynamicPosition &dynamicPosition) const
	Brings a dynamic position in the base frame to one in the follower frame.

Orientation	apply (const Orientation &orientation) const
	Brings an orientation in the base frame to one in the follower frame.

DynamicOrientation	apply (const DynamicOrientation &dynamicOrientation) const
	Brings a dynamic orientation in the base frame to one in the follower frame.

void	updateTranslation (const Position &newTranslation)
	Updates the translation of the current transformation matrix.

void	updateTranslation (Position &&newTranslation)
	Updates the translation of the current transformation matrix.

void	updateTranslation (float x, float y, float z)
	Updates the translation of the current transformation matrix.

void	updateTranslation (const DynamicPosition &newTranslation)
	Updates the translation of the current transformation matrix.

void	updateTranslation (DynamicPosition &&newTranslation)
	Updates the translation of the current transformation matrix.

void	updateRotation (const Orientation &newRotation)
	Updates the rotation of the current transformation matrix.

void	updateRotation (Orientation &&newRotation)
	Updates the rotation of the current transformation matrix.

void	updateRotation (float roll, float pitch, float yaw)
	Updates the rotation of the current transformation matrix. Takes rotation angles in the order of roll->pitch->yaw.

void	updateRotation (const DynamicOrientation &newRotation)
	Updates the rotation of the current transformation matrix.

void	updateRotation (DynamicOrientation &&newRotation)
	Updates the rotation of the current transformation matrix.

void	updateVelocity (const Vector &newVelocity)
	Updates the velocity of the current transform.

void	updateVelocity (Vector &&newVelocity)
	Updates the velocity of the current transform.

void	updateVelocity (float vx, float vy, float vz)
	Updates the velocity of the current transform.

void	updateAcceleration (const Vector &newAcceleration)
	Updates the acceleration of the current transform.

void	updateAcceleration (Vector &&newAcceleration)
	Updates the acceleration of the current transform.

void	updateAcceleration (float ax, float ay, float az)
	Updates the acceleration of the current transform.

void	updateAngularVelocity (const Vector &newAngularVelocity)
	Updates the angular velocity of the current transform.

void	updateAngularVelocity (Position &&newAngularVelocity)
	Updates the angular velocity of the current transform.

void	updateAngularVelocity (float vr, float vp, float vy)
	Updates the angular velocity of the current transform.

Transform	getInverse () const

Transform	compose (const Transform &second) const
	Returns the composed transformation of the given transformations.

Transform	composeStatic (const Transform &second) const
	Returns the composed transformation of the given transformations, ignoring any time derivatives.

Transform	projectForward (float dt) const
	Projects this transform forward in time according to its translational velocity/acceleration and angular velocity.

Position	getTranslation () const

Vector	getVelocity () const

Vector	getAcceleration () const

DynamicPosition	getDynamicTranslation () const

Orientation	getRotation () const

Vector	getAngularVel () const

DynamicOrientation	getDynamicOrientation () const

float	getRoll () const
	Get the roll of this transformation.

float	getPitch () const
	Get the pitch of this transformation.

float	getYaw () const
	Get the yaw of this transformation.

float	getRollVelocity () const
	Get the roll velocity of this transformation.

float	getPitchVelocity () const
	Get the pitch velocity of this transformation.

float	getYawVelocity () const
	Get the yaw velocity of this transformation.

float	getX () const
	Get the x-component of this transform's translation.

float	getY () const
	Get the y-component of this transform's translation.

float	getZ () const
	Get the z-component of this transform's translation.

float	getXVel () const
	Get the x-component of this transform's linear velocity.

float	getYVel () const
	Get the y-component of this transform's linear velocity.

float	getZVel () const
	Get the z-component of this transform's linear velocity.

float	getXAcc () const
	Get the x-component of this transform's linear acceleration.

float	getYAcc () const
	Get the y-component of this transform's linear acceleration.

float	getZAcc () const
	Get the z-component of this transform's linear acceleration.

bool	isDynamic () const
	Whether there are any non-zero derivatives.

Static Public Member Functions
static Transform	identity ()
	Constructs an identity transform.

Detailed Description

Represents a transformation from one coordinate frame to another. A Static Transform from frame A to frame B defines a relationship between the two frames, such that a spatial measurement in frame A can be represented equivalently in frame B by applying a translational and rotational offset. This process is known as applying a transform.

Static Transforms are specified as a translation and rotation of some "follower" frame relative to some "base" frame. The "translation" is the follower frame's origin in base frame, and the "rotation" is the follower frame's orientation relative to the base frame's orientation.

Conceptually, translations are applied "before" rotations. This means that the origin of the follower frame is entirely defined by the translation in the base frame, and the rotation serves only to change the orientation of the follower frame's axes relative to the base frame.

A Dynamic Transform is an extension of a Static Transform that can store linear velocity, linear acceleration, and angular velocity. This class handles both, automatically determining whether it is static or dynamic.

Utilizes ARM's CMSIS matrix operations.

Constructor & Destructor Documentation

◆ Transform() [1/12]

tap::algorithms::transforms::Transform::Transform	(	const Position &	translation,
		const Orientation &	rotation
	)

Parameters

rotation	Initial rotation of this transformation.
position	Initial translation of this transformation.

◆ Transform() [2/12]

tap::algorithms::transforms::Transform::Transform	(	Position &&	translation,
		Orientation &&	rotation
	)

◆ Transform() [3/12]

tap::algorithms::transforms::Transform::Transform	(	const CMSISMat< 3, 1 > &	translation,
		const CMSISMat< 3, 3 > &	rotation
	)

Parameters

rotation	Initial rotation of this transformation.
position	Initial translation of this transformation.

◆ Transform() [4/12]

tap::algorithms::transforms::Transform::Transform	(	CMSISMat< 3, 1 > &&	translation,
		CMSISMat< 3, 3 > &&	rotation
	)

◆ Transform() [5/12]

tap::algorithms::transforms::Transform::Transform	(	float	x,
		float	y,
		float	z,
		float	rx,
		float	ry,
		float	rz
	)

Constructs rotations using XYZ Euler angles, so rotations are applied in order of rx, ry, then rz. As an example, for an x-forward, z-up coordinate system, this is in the order of roll, pitch, then yaw.

Parameters

x	Initial x-component of the translation.
y	Initial y-component of the translation.
z	Initial z-component of the translation.
rx	Initial rotation angle about the x-axis.
ry	Initial rotation angle about the y-axis.
rz	Initial rotation angle about the z-axis.

◆ Transform() [6/12]

tap::algorithms::transforms::Transform::Transform	(	const Position &	translation,
		const Orientation &	rotation,
		const Vector &	velocity,
		const Vector &	acceleration,
		const Vector &	angularVelocity
	)

Parameters

translation	Initial translation of this transformation.
rotation	Initial rotation of this transformation.
velocity	Translational velocity of this transformation.
acceleration	Translational acceleration of this transformation.
angularVelocity	Angular velocity pseudovector of this transformation.

◆ Transform() [7/12]

tap::algorithms::transforms::Transform::Transform	(	Position &&	translation,
		Orientation &&	rotation,
		Vector &&	velocity,
		Vector &&	acceleration,
		Vector &&	angularVelocity
	)

Parameters

translation	Initial translation of this transformation.
rotation	Initial rotation of this transformation.
velocity	Translational velocity of this transformation.
acceleration	Translational acceleration of this transformation.
angularVelocity	Angular velocity pseudovector of this transformation.

◆ Transform() [8/12]

tap::algorithms::transforms::Transform::Transform	(	const DynamicPosition &	dynamicPosition,
		const DynamicOrientation &	dynamicOrientation
	)

Parameters

rotation	Initial rotation of this transformation.
position	Initial translation of this transformation.

◆ Transform() [9/12]

tap::algorithms::transforms::Transform::Transform	(	DynamicPosition &&	dynamicPosition,
		DynamicOrientation &&	dynamicOrientation
	)

◆ Transform() [10/12]

tap::algorithms::transforms::Transform::Transform	(	const CMSISMat< 3, 1 > &	translation,
		const CMSISMat< 3, 3 > &	rotation,
		const CMSISMat< 3, 1 > &	velocity,
		const CMSISMat< 3, 1 > &	acceleration,
		const CMSISMat< 3, 3 > &	angularVelocity
	)

Parameters

translation	Initial translation of this transformation.
rotation	Initial rotation of this transformation.
velocity	Translational velocity of this transformation.
acceleration	Translational acceleration of this transformation.
angularVelocity	Angular velocity skew symmetric matrix of this transformation.

◆ Transform() [11/12]

tap::algorithms::transforms::Transform::Transform	(	CMSISMat< 3, 1 > &&	translation,
		CMSISMat< 3, 3 > &&	rotation,
		CMSISMat< 3, 1 > &&	velocity,
		CMSISMat< 3, 1 > &&	acceleration,
		CMSISMat< 3, 3 > &&	angularVelocity
	)

Parameters

translation	Initial translation of this transformation.
rotation	Initial rotation of this transformation.
velocity	Translational velocity of this transformation.
acceleration	Translational acceleration of this transformation.
angularVelocity	Angular velocity skew symmetric matrix of this transformation.

◆ Transform() [12/12]

tap::algorithms::transforms::Transform::Transform	(	float	x,
		float	y,
		float	z,
		float	vx,
		float	vy,
		float	vz,
		float	ax,
		float	ay,
		float	az,
		float	rx,
		float	ry,
		float	rz,
		float	wx,
		float	wy,
		float	wz
	)

Constructs rotations using XYZ Euler angles, so rotations are applied in order of rx, ry, then rx.

Parameters

x	Initial x-component of the translation.
y	Initial y-component of the translation.
z	Initial z-component of the translation.
vx	Initial x-component of the translational velocity.
vy	Initial y-component of the translational velocity.
vz	Initial z-component of the translational velocity.
ax	Initial x-component of the translational acceleration.
ay	Initial y-component of the translational acceleration.
az	Initial z-component of the translational acceleration.
rx	Initial rotation angle about the x-axis.
ry	Initial rotation angle about the y-axis.
rz	Initial rotation angle about the z-axis.
wx	Initial angular velocity about the x-axis.
wy	Initial angular velocity about the y-axis.
wz	Initial angular velocity about the z-axis.

Member Function Documentation

◆ apply() [1/5]

DynamicOrientation tap::algorithms::transforms::Transform::apply ( const DynamicOrientation & dynamicOrientation ) const

Brings a dynamic orientation in the base frame to one in the follower frame.

◆ apply() [2/5]

DynamicPosition tap::algorithms::transforms::Transform::apply ( const DynamicPosition & dynamicPosition ) const

Brings a dynamic position in the base frame to one in the follower frame.

◆ apply() [3/5]

Orientation tap::algorithms::transforms::Transform::apply ( const Orientation & orientation ) const

Brings an orientation in the base frame to one in the follower frame.

◆ apply() [4/5]

Position tap::algorithms::transforms::Transform::apply ( const Position & position ) const

Apply this transform to a position.

Parameters

[in] position Position in base frame.

Returns: Position in follower frame.

◆ apply() [5/5]

Vector tap::algorithms::transforms::Transform::apply ( const Vector & vector ) const

Rotates a vector in the base frame to a vector in the follower frame.

Intended to be used for things like velocities and accelerations which represent the difference between two positions in space, since both positions get translated the same way, causing the translation to cancel out.

Note: Only accurate for static transforms!

Parameters

vector Vector as read by base frame.

Returns: Vector in follower frame's basis.

◆ compose()

Transform tap::algorithms::transforms::Transform::compose ( const Transform & second ) const

Returns the composed transformation of the given transformations.

Returns: Transformation from this transform's base frame to second's follower frame.

◆ composeStatic()

Transform tap::algorithms::transforms::Transform::composeStatic ( const Transform & second ) const

Returns the composed transformation of the given transformations, ignoring any time derivatives.

Returns: Static transformation from this transform's base frame to second's follower frame.

◆ getAcceleration()

Vector tap::algorithms::transforms::Transform::getAcceleration ( ) const

inline

◆ getAngularVel()

Vector tap::algorithms::transforms::Transform::getAngularVel ( ) const

inline

◆ getDynamicOrientation()

DynamicOrientation tap::algorithms::transforms::Transform::getDynamicOrientation ( ) const

inline

◆ getDynamicTranslation()

DynamicPosition tap::algorithms::transforms::Transform::getDynamicTranslation ( ) const

inline

◆ getInverse()

Transform tap::algorithms::transforms::Transform::getInverse ( ) const

Returns: Inverse of this Transform.

Note: This is only instantaneously correct for dynamic transforms; It can no longer be projected forward in time and behave the same way as the original. This is due to the now reversed translation-rotation that would be required to truly mimic the motion of the original. Ex: Consider a dynamic transform with only non-zero translation and angular velocity. Projecting this forward will cause the follower frame to rotate around its origin. Intuitively, one would expect the inverted transform to have its follower frame rotate around the base frame origin. However, this circular translation can only be approximated here with translational velocity/acceleration. The true inverse would need to be the composition of a rotation then a translation.

◆ getPitch()

float tap::algorithms::transforms::Transform::getPitch ( ) const

Get the pitch of this transformation.

◆ getPitchVelocity()

float tap::algorithms::transforms::Transform::getPitchVelocity ( ) const

Get the pitch velocity of this transformation.

◆ getRoll()

float tap::algorithms::transforms::Transform::getRoll ( ) const

Get the roll of this transformation.

◆ getRollVelocity()

float tap::algorithms::transforms::Transform::getRollVelocity ( ) const

Get the roll velocity of this transformation.

◆ getRotation()

Orientation tap::algorithms::transforms::Transform::getRotation ( ) const

inline

◆ getTranslation()

Position tap::algorithms::transforms::Transform::getTranslation ( ) const

inline

◆ getVelocity()

Vector tap::algorithms::transforms::Transform::getVelocity ( ) const

inline

◆ getX()

float tap::algorithms::transforms::Transform::getX ( ) const

inline

Get the x-component of this transform's translation.

◆ getXAcc()

float tap::algorithms::transforms::Transform::getXAcc ( ) const

inline

Get the x-component of this transform's linear acceleration.

◆ getXVel()

float tap::algorithms::transforms::Transform::getXVel ( ) const

inline

Get the x-component of this transform's linear velocity.

◆ getY()

float tap::algorithms::transforms::Transform::getY ( ) const

inline

Get the y-component of this transform's translation.

◆ getYAcc()

float tap::algorithms::transforms::Transform::getYAcc ( ) const

inline

Get the y-component of this transform's linear acceleration.

◆ getYaw()

float tap::algorithms::transforms::Transform::getYaw ( ) const

Get the yaw of this transformation.

◆ getYawVelocity()

float tap::algorithms::transforms::Transform::getYawVelocity ( ) const

Get the yaw velocity of this transformation.

◆ getYVel()

float tap::algorithms::transforms::Transform::getYVel ( ) const

inline

Get the y-component of this transform's linear velocity.

◆ getZ()

float tap::algorithms::transforms::Transform::getZ ( ) const

inline

Get the z-component of this transform's translation.

◆ getZAcc()

float tap::algorithms::transforms::Transform::getZAcc ( ) const

inline

Get the z-component of this transform's linear acceleration.

◆ getZVel()

float tap::algorithms::transforms::Transform::getZVel ( ) const

inline

Get the z-component of this transform's linear velocity.

◆ identity()

static Transform tap::algorithms::transforms::Transform::identity ( )

inlinestatic

Constructs an identity transform.

◆ isDynamic()

bool tap::algorithms::transforms::Transform::isDynamic ( ) const

inline

Whether there are any non-zero derivatives.

◆ projectForward()

Transform tap::algorithms::transforms::Transform::projectForward ( float dt ) const

Projects this transform forward in time according to its translational velocity/acceleration and angular velocity.

Parameters

dt	Seconds to project forward (can be negative)

Returns: Projected transform

◆ updateAcceleration() [1/3]

void tap::algorithms::transforms::Transform::updateAcceleration ( const Vector & newAcceleration )

inline

Updates the acceleration of the current transform.

Parameters

updateAcceleration updated acceleration of follower in base frame.

◆ updateAcceleration() [2/3]

void tap::algorithms::transforms::Transform::updateAcceleration	(	float	ax,
		float	ay,
		float	az
	)

inline

Updates the acceleration of the current transform.

Parameters

ax	new acceleration x-component.
ay	new acceleration y-component.
az	new acceleration z-component.

◆ updateAcceleration() [3/3]

void tap::algorithms::transforms::Transform::updateAcceleration ( Vector && newAcceleration )

inline

Updates the acceleration of the current transform.

Parameters

updateAcceleration updated acceleration of follower in base frame.

◆ updateAngularVelocity() [1/3]

void tap::algorithms::transforms::Transform::updateAngularVelocity ( const Vector & newAngularVelocity )

inline

Updates the angular velocity of the current transform.

Parameters

updateAngularVelocity updated angular velocity of follower in base frame.

◆ updateAngularVelocity() [2/3]

void tap::algorithms::transforms::Transform::updateAngularVelocity	(	float	vr,
		float	vp,
		float	vy
	)

inline

Updates the angular velocity of the current transform.

Parameters

ax	new angular velocity x-component.
ay	new angular velocity y-component.
az	new angular velocity z-component.

◆ updateAngularVelocity() [3/3]

void tap::algorithms::transforms::Transform::updateAngularVelocity ( Position && newAngularVelocity )

inline

Updates the angular velocity of the current transform.

Parameters

updateAngularVelocity updated angular velocity of follower in base frame.

◆ updateRotation() [1/5]

void tap::algorithms::transforms::Transform::updateRotation ( const DynamicOrientation & newRotation )

inline

Updates the rotation of the current transformation matrix.

Parameters

newRotation updated orientation of follower frame in base frame.

◆ updateRotation() [2/5]

void tap::algorithms::transforms::Transform::updateRotation ( const Orientation & newRotation )

inline

Updates the rotation of the current transformation matrix.

Parameters

newRotation updated orientation of follower frame in base frame.

◆ updateRotation() [3/5]

void tap::algorithms::transforms::Transform::updateRotation ( DynamicOrientation && newRotation )

inline

Updates the rotation of the current transformation matrix.

Parameters

newRotation updated orientation of follower frame in base frame.

◆ updateRotation() [4/5]

void tap::algorithms::transforms::Transform::updateRotation	(	float	roll,
		float	pitch,
		float	yaw
	)

inline

Updates the rotation of the current transformation matrix. Takes rotation angles in the order of roll->pitch->yaw.

Parameters

roll	updated rotation angle about the x-axis.
pitch	updated rotation angle about the y-axis.
yaw	updated rotation angle about the z-axis.

◆ updateRotation() [5/5]

void tap::algorithms::transforms::Transform::updateRotation ( Orientation && newRotation )

inline

Updates the rotation of the current transformation matrix.

Parameters

newRotation updated orientation of follower frame in base frame.

◆ updateTranslation() [1/5]

void tap::algorithms::transforms::Transform::updateTranslation ( const DynamicPosition & newTranslation )

inline