tf2  master
tf2 maintains the relationship between coordinate frames in a tree structure buffered in time, and lets the user transform points, vectors, etc between any two coordinate frames at any desired point in time.
Public Member Functions | Static Public Member Functions | List of all members
tf2::Matrix3x3 Class Reference

The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Quaternion, Transform and Vector3. Make sure to only include a pure orthogonal matrix without scaling. More...

#include <Matrix3x3.h>

Public Member Functions

 Matrix3x3 ()
 No initializaion constructor. More...
 
 Matrix3x3 (const Quaternion &q)
 Constructor from Quaternion. More...
 
 Matrix3x3 (const tf2Scalar &xx, const tf2Scalar &xy, const tf2Scalar &xz, const tf2Scalar &yx, const tf2Scalar &yy, const tf2Scalar &yz, const tf2Scalar &zx, const tf2Scalar &zy, const tf2Scalar &zz)
 Constructor with row major formatting. More...
 
TF2SIMD_FORCE_INLINE Matrix3x3 (const Matrix3x3 &other)
 Copy constructor. More...
 
TF2SIMD_FORCE_INLINE Matrix3x3operator= (const Matrix3x3 &other)
 Assignment Operator. More...
 
TF2SIMD_FORCE_INLINE Vector3 getColumn (int i) const
 Get a column of the matrix as a vector. More...
 
const TF2SIMD_FORCE_INLINE Vector3 & getRow (int i) const
 Get a row of the matrix as a vector. More...
 
TF2SIMD_FORCE_INLINE Vector3 & operator[] (int i)
 Get a mutable reference to a row of the matrix as a vector. More...
 
const TF2SIMD_FORCE_INLINE Vector3 & operator[] (int i) const
 Get a const reference to a row of the matrix as a vector. More...
 
Matrix3x3operator*= (const Matrix3x3 &m)
 Multiply by the target matrix on the right. More...
 
void setFromOpenGLSubMatrix (const tf2Scalar *m)
 Set from a carray of tf2Scalars. More...
 
void setValue (const tf2Scalar &xx, const tf2Scalar &xy, const tf2Scalar &xz, const tf2Scalar &yx, const tf2Scalar &yy, const tf2Scalar &yz, const tf2Scalar &zx, const tf2Scalar &zy, const tf2Scalar &zz)
 Set the values of the matrix explicitly (row major) More...
 
void setRotation (const Quaternion &q)
 Set the matrix from a quaternion. More...
 
void setEulerYPR (tf2Scalar eulerZ, tf2Scalar eulerY, tf2Scalar eulerX)
 Set the matrix from euler angles YPR around ZYX axes. More...
 
void setRPY (tf2Scalar roll, tf2Scalar pitch, tf2Scalar yaw)
 Set the matrix using RPY about XYZ fixed axes. More...
 
void setIdentity ()
 Set the matrix to the identity. More...
 
void getOpenGLSubMatrix (tf2Scalar *m) const
 Fill the values of the matrix into a 9 element array. More...
 
void getRotation (Quaternion &q) const
 Get the matrix represented as a quaternion. More...
 
void getEulerYPR (tf2Scalar &yaw, tf2Scalar &pitch, tf2Scalar &roll, unsigned int solution_number=1) const
 Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR. More...
 
void getRPY (tf2Scalar &roll, tf2Scalar &pitch, tf2Scalar &yaw, unsigned int solution_number=1) const
 Get the matrix represented as roll pitch and yaw about fixed axes XYZ. More...
 
Matrix3x3 scaled (const Vector3 &s) const
 Create a scaled copy of the matrix. More...
 
tf2Scalar determinant () const
 Return the determinant of the matrix. More...
 
Matrix3x3 adjoint () const
 Return the adjoint of the matrix. More...
 
Matrix3x3 absolute () const
 Return the matrix with all values non negative. More...
 
Matrix3x3 transpose () const
 Return the transpose of the matrix. More...
 
Matrix3x3 inverse () const
 Return the inverse of the matrix. More...
 
Matrix3x3 transposeTimes (const Matrix3x3 &m) const
 
Matrix3x3 timesTranspose (const Matrix3x3 &m) const
 
TF2SIMD_FORCE_INLINE tf2Scalar tdotx (const Vector3 &v) const
 
TF2SIMD_FORCE_INLINE tf2Scalar tdoty (const Vector3 &v) const
 
TF2SIMD_FORCE_INLINE tf2Scalar tdotz (const Vector3 &v) const
 
void diagonalize (Matrix3x3 &rot, tf2Scalar threshold, int maxSteps)
 diagonalizes this matrix by the Jacobi method. More...
 
tf2Scalar cofac (int r1, int c1, int r2, int c2) const
 Calculate the matrix cofactor. More...
 
void serialize (struct Matrix3x3Data &dataOut) const
 
void serializeFloat (struct Matrix3x3FloatData &dataOut) const
 
void deSerialize (const struct Matrix3x3Data &dataIn)
 
void deSerializeFloat (const struct Matrix3x3FloatData &dataIn)
 
void deSerializeDouble (const struct Matrix3x3DoubleData &dataIn)
 

Static Public Member Functions

static const Matrix3x3getIdentity ()
 

Detailed Description

The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Quaternion, Transform and Vector3. Make sure to only include a pure orthogonal matrix without scaling.

Constructor & Destructor Documentation

◆ Matrix3x3() [1/4]

tf2::Matrix3x3::Matrix3x3 ( )
inline

No initializaion constructor.

◆ Matrix3x3() [2/4]

tf2::Matrix3x3::Matrix3x3 ( const Quaternion q)
inlineexplicit

Constructor from Quaternion.

◆ Matrix3x3() [3/4]

tf2::Matrix3x3::Matrix3x3 ( const tf2Scalar xx,
const tf2Scalar xy,
const tf2Scalar xz,
const tf2Scalar yx,
const tf2Scalar yy,
const tf2Scalar yz,
const tf2Scalar zx,
const tf2Scalar zy,
const tf2Scalar zz 
)
inline

Constructor with row major formatting.

◆ Matrix3x3() [4/4]

TF2SIMD_FORCE_INLINE tf2::Matrix3x3::Matrix3x3 ( const Matrix3x3 other)
inline

Copy constructor.

Member Function Documentation

◆ operator=()

TF2SIMD_FORCE_INLINE Matrix3x3& tf2::Matrix3x3::operator= ( const Matrix3x3 other)
inline

Assignment Operator.

◆ getColumn()

TF2SIMD_FORCE_INLINE Vector3 tf2::Matrix3x3::getColumn ( int  i) const
inline

Get a column of the matrix as a vector.

Parameters
iColumn number 0 indexed

◆ getRow()

const TF2SIMD_FORCE_INLINE Vector3& tf2::Matrix3x3::getRow ( int  i) const
inline

Get a row of the matrix as a vector.

Parameters
iRow number 0 indexed

◆ operator[]() [1/2]

TF2SIMD_FORCE_INLINE Vector3& tf2::Matrix3x3::operator[] ( int  i)
inline

Get a mutable reference to a row of the matrix as a vector.

Parameters
iRow number 0 indexed

◆ operator[]() [2/2]

const TF2SIMD_FORCE_INLINE Vector3& tf2::Matrix3x3::operator[] ( int  i) const
inline

Get a const reference to a row of the matrix as a vector.

Parameters
iRow number 0 indexed

◆ operator*=()

TF2SIMD_FORCE_INLINE Matrix3x3 & tf2::Matrix3x3::operator*= ( const Matrix3x3 m)

Multiply by the target matrix on the right.

Parameters
mRotation matrix to be applied Equivilant to this = this * m

◆ setFromOpenGLSubMatrix()

void tf2::Matrix3x3::setFromOpenGLSubMatrix ( const tf2Scalar m)
inline

Set from a carray of tf2Scalars.

Parameters
mA pointer to the beginning of an array of 9 tf2Scalars

◆ setValue()

void tf2::Matrix3x3::setValue ( const tf2Scalar xx,
const tf2Scalar xy,
const tf2Scalar xz,
const tf2Scalar yx,
const tf2Scalar yy,
const tf2Scalar yz,
const tf2Scalar zx,
const tf2Scalar zy,
const tf2Scalar zz 
)
inline

Set the values of the matrix explicitly (row major)

Parameters
xxTop left
xyTop Middle
xzTop Right
yxMiddle Left
yyMiddle Middle
yzMiddle Right
zxBottom Left
zyBottom Middle
zzBottom Right

◆ setRotation()

void tf2::Matrix3x3::setRotation ( const Quaternion q)
inline

Set the matrix from a quaternion.

Parameters
qThe Quaternion to match

◆ setEulerYPR()

void tf2::Matrix3x3::setEulerYPR ( tf2Scalar  eulerZ,
tf2Scalar  eulerY,
tf2Scalar  eulerX 
)
inline

Set the matrix from euler angles YPR around ZYX axes.

Parameters
eulerZYaw aboud Z axis
eulerYPitch around Y axis
eulerXRoll about X axis

These angles are used to produce a rotation matrix. The euler angles are applied in ZYX order. I.e a vector is first rotated about X then Y and then Z

◆ setRPY()

void tf2::Matrix3x3::setRPY ( tf2Scalar  roll,
tf2Scalar  pitch,
tf2Scalar  yaw 
)
inline

Set the matrix using RPY about XYZ fixed axes.

Parameters
rollRoll about X axis
pitchPitch around Y axis
yawYaw aboud Z axis

◆ setIdentity()

void tf2::Matrix3x3::setIdentity ( )
inline

Set the matrix to the identity.

◆ getIdentity()

static const Matrix3x3& tf2::Matrix3x3::getIdentity ( )
inlinestatic

◆ getOpenGLSubMatrix()

void tf2::Matrix3x3::getOpenGLSubMatrix ( tf2Scalar m) const
inline

Fill the values of the matrix into a 9 element array.

Parameters
mThe array to be filled

◆ getRotation()

void tf2::Matrix3x3::getRotation ( Quaternion q) const
inline

Get the matrix represented as a quaternion.

Parameters
qThe quaternion which will be set

◆ getEulerYPR()

void tf2::Matrix3x3::getEulerYPR ( tf2Scalar yaw,
tf2Scalar pitch,
tf2Scalar roll,
unsigned int  solution_number = 1 
) const
inline

Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR.

Parameters
yawYaw around Z axis
pitchPitch around Y axis
rollaround X axis

◆ getRPY()

void tf2::Matrix3x3::getRPY ( tf2Scalar roll,
tf2Scalar pitch,
tf2Scalar yaw,
unsigned int  solution_number = 1 
) const
inline

Get the matrix represented as roll pitch and yaw about fixed axes XYZ.

Parameters
rollaround X axis
pitchPitch around Y axis
yawYaw around Z axis
solution_numberWhich solution of two possible solutions ( 1 or 2) are possible values

◆ scaled()

Matrix3x3 tf2::Matrix3x3::scaled ( const Vector3 &  s) const
inline

Create a scaled copy of the matrix.

Parameters
sScaling vector The elements of the vector will scale each column

◆ determinant()

TF2SIMD_FORCE_INLINE tf2Scalar tf2::Matrix3x3::determinant ( ) const

Return the determinant of the matrix.

◆ adjoint()

TF2SIMD_FORCE_INLINE Matrix3x3 tf2::Matrix3x3::adjoint ( ) const

Return the adjoint of the matrix.

◆ absolute()

TF2SIMD_FORCE_INLINE Matrix3x3 tf2::Matrix3x3::absolute ( ) const

Return the matrix with all values non negative.

◆ transpose()

TF2SIMD_FORCE_INLINE Matrix3x3 tf2::Matrix3x3::transpose ( ) const

Return the transpose of the matrix.

◆ inverse()

TF2SIMD_FORCE_INLINE Matrix3x3 tf2::Matrix3x3::inverse ( ) const

Return the inverse of the matrix.

◆ transposeTimes()

TF2SIMD_FORCE_INLINE Matrix3x3 tf2::Matrix3x3::transposeTimes ( const Matrix3x3 m) const

◆ timesTranspose()

TF2SIMD_FORCE_INLINE Matrix3x3 tf2::Matrix3x3::timesTranspose ( const Matrix3x3 m) const

◆ tdotx()

TF2SIMD_FORCE_INLINE tf2Scalar tf2::Matrix3x3::tdotx ( const Vector3 &  v) const
inline

◆ tdoty()

TF2SIMD_FORCE_INLINE tf2Scalar tf2::Matrix3x3::tdoty ( const Vector3 &  v) const
inline

◆ tdotz()

TF2SIMD_FORCE_INLINE tf2Scalar tf2::Matrix3x3::tdotz ( const Vector3 &  v) const
inline

◆ diagonalize()

void tf2::Matrix3x3::diagonalize ( Matrix3x3 rot,
tf2Scalar  threshold,
int  maxSteps 
)
inline

diagonalizes this matrix by the Jacobi method.

Parameters
rotstores the rotation from the coordinate system in which the matrix is diagonal to the original coordinate system, i.e., old_this = rot * new_this * rot^T.
thresholdSee iteration
iterationThe iteration stops when all off-diagonal elements are less than the threshold multiplied by the sum of the absolute values of the diagonal, or when maxSteps have been executed.

Note that this matrix is assumed to be symmetric.

◆ cofac()

tf2Scalar tf2::Matrix3x3::cofac ( int  r1,
int  c1,
int  r2,
int  c2 
) const
inline

Calculate the matrix cofactor.

Parameters
r1The first row to use for calculating the cofactor
c1The first column to use for calculating the cofactor
r1The second row to use for calculating the cofactor
c1The second column to use for calculating the cofactor See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details

◆ serialize()

TF2SIMD_FORCE_INLINE void tf2::Matrix3x3::serialize ( struct Matrix3x3Data dataOut) const

◆ serializeFloat()

TF2SIMD_FORCE_INLINE void tf2::Matrix3x3::serializeFloat ( struct Matrix3x3FloatData dataOut) const

◆ deSerialize()

TF2SIMD_FORCE_INLINE void tf2::Matrix3x3::deSerialize ( const struct Matrix3x3Data dataIn)

◆ deSerializeFloat()

TF2SIMD_FORCE_INLINE void tf2::Matrix3x3::deSerializeFloat ( const struct Matrix3x3FloatData dataIn)

◆ deSerializeDouble()

TF2SIMD_FORCE_INLINE void tf2::Matrix3x3::deSerializeDouble ( const struct Matrix3x3DoubleData dataIn)

The documentation for this class was generated from the following file: