Ravelin
Public Member Functions | Static Public Member Functions | List of all members
MATRIX3 Class Reference

A 3x3 matrix that may be used for orientation, inertia tensors, etc. More...

#include <Matrix3.h>

Public Member Functions

 MATRIX3 (const MATRIX3 &source)
 
 MATRIX3 (const REAL *array)
 Constructs a matrix from an array. More...
 
 MATRIX3 (const QUAT &q)
 
 MATRIX3 (const AANGLE &a)
 
 MATRIX3 (REAL, REAL, REAL, REAL, REAL, REAL, REAL, REAL, REAL)
 Constructs a matrix from a set of values.
 
MATRIX3operator= (const QUAT &q)
 
MATRIX3operator= (const AANGLE &a)
 
 MATRIX3 (const MATRIXN &m)
 
 MATRIX3 (const SHAREDMATRIXN &m)
 
 MATRIX3 (const CONST_SHAREDMATRIXN &m)
 
REAL norm_inf () const
 Computes the l-infinity norm of this matrix.
 
unsigned rows () const
 
unsigned columns () const
 
bool is_symmetric (REAL tolerance) const
 Checks whether this matrix is symmetric.
 
bool orthonormalize ()
 Makes the matrix orthonormal using Gram-Schmidt orthogonalization.
 
bool is_orthonormal () const
 Determines whether this is an orthonormal matrix.
 
REAL det () const
 Calculates the determinant for a 3x3 matrix.
 
MATRIX3invert ()
 Inverts this matrix.
 
const REAL & xx () const
 
REAL & xx ()
 
const REAL & xy () const
 
REAL & xy ()
 
const REAL & xz () const
 
REAL & xz ()
 
const REAL & yx () const
 
REAL & yx ()
 
const REAL & yy () const
 
REAL & yy ()
 
const REAL & yz () const
 
REAL & yz ()
 
const REAL & zx () const
 
REAL & zx ()
 
const REAL & zy () const
 
REAL & zy ()
 
const REAL & zz () const
 
REAL & zz ()
 
MATRIX3 inverse () const
 
void set_rot_X (REAL angle)
 Sets this matrix to the rotation matrix of the specified angle around the X axis.
 
void set_rot_Y (REAL angle)
 Sets this matrix to the rotation matrix of the specified angle around the Y axis.
 
void set_rot_Z (REAL angle)
 Sets this matrix to the rotation matrix of the specified angle around the Z axis.
 
void transpose ()
 Sets this matrix to its transpose.
 
MATRIX3set_zero (unsigned m, unsigned n)
 
MATRIX3set_identity ()
 Sets this matrix to identity.
 
MATRIX3set_zero ()
 
ORIGIN3 transpose_mult (const ORIGIN3 &v) const
 Multiplies the transpose of this matrix by a vector and returns the result in a new vector.
 
ORIGIN3 mult (const ORIGIN3 &v) const
 Multiplies this matrix by a vector and returns the result in a new vector.
 
MATRIX3 mult (const MATRIX3 &m) const
 Multiplies this matrix by a matrix and returns the result in a new matrix.
 
MATRIX3 transpose_mult (const MATRIX3 &m) const
 Multiplies the transpose of this matrix by a matrix and returns the result in a new matrix.
 
MATRIX3 mult_transpose (const MATRIX3 &m) const
 Multiplies this matrix by a transposed matrix and returns the result in a new matrix.
 
MATRIX3 transpose_mult_transpose (const MATRIX3 &m) const
 Multiplies the transpose of this matrix by a transposed matrix and returns the result in a new matrix.
 
MATRIX3operator= (const MATRIX3 &source)
 Copies a matrix to this one.
 
MATRIX3operator= (const MATRIXN &source)
 Copies a matrix to this one.
 
MATRIX3operator= (const SHAREDMATRIXN &source)
 Copies a matrix to this one.
 
MATRIX3operator= (const CONST_SHAREDMATRIXN &source)
 Copies a matrix to this one.
 
MATRIX3operator+= (const MATRIX3 &m)
 Adds m to this in place.
 
MATRIX3operator-= (const MATRIX3 &m)
 Subtracts m from this in place.
 
MATRIX3operator*= (REAL scalar)
 Multiplies this matrix by a scalar in place.
 
MATRIX3operator/= (REAL scalar)
 
MATRIX3 operator+ (const MATRIX3 &m) const
 
MATRIX3 operator- (const MATRIX3 &m) const
 
MATRIX3 operator* (REAL scalar) const
 
MATRIX3 operator/ (REAL scalar) const
 
MATRIX3 operator- () const
 Returns the negation of this matrix.
 
MATRIX3 operator* (const MATRIX3 &m) const
 
ORIGIN3 operator* (const ORIGIN3 &v) const
 
unsigned leading_dim () const
 
unsigned inc () const
 
ORIGIN3 get_column (unsigned i) const
 Gets the specified column of the matrix. More...
 
ORIGIN3 get_row (unsigned i) const
 Gets the specified row of the matrix. More...
 
REAL & operator() (unsigned i, unsigned j)
 
const REAL & operator() (unsigned i, unsigned j) const
 
MATRIX3resize (unsigned rows, unsigned columns, bool preserve=false)
 
const REAL * data (unsigned i) const
 
REAL * data (unsigned i)
 
unsigned size () const
 Gets the total number of elements in this matrix.
 
const REAL * data () const
 Gets constant pointer to the beginning of the matrix array.
 
REAL * data ()
 Gets pointer to the beginning of the matrix array.
 
CONST_COLUMN_ITERATOR block_column_iterator_begin (unsigned row_start, unsigned row_end, unsigned col_start, unsigned col_end) const
 This file consists of general routines for constant matrices. More...
 
CONST_COLUMN_ITERATOR block_column_iterator_end (unsigned row_start, unsigned row_end, unsigned col_start, unsigned col_end) const
 Gets a column iterator to a block.
 
CONST_ROW_ITERATOR block_row_iterator_begin (unsigned row_start, unsigned row_end, unsigned col_start, unsigned col_end) const
 Gets a row iterator to a block.
 
CONST_ROW_ITERATOR block_row_iterator_end (unsigned row_start, unsigned row_end, unsigned col_start, unsigned col_end) const
 Gets a row iterator to a block.
 
template<class V >
V & get_row (unsigned row, V &v) const
 
template<class V >
V & get_column (unsigned column, V &v) const
 
template<class T , class U >
U & transpose_mult_transpose (const T &x, U &y, REAL alpha=(REAL) 1.0, REAL beta=(REAL) 0.0) const
 Does the operation y = beta*y + alpha*this'*x'.
 
template<class T , class U >
U & mult_transpose (const T &x, U &y, REAL alpha=(REAL) 1.0, REAL beta=(REAL) 0.0) const
 Does the operation y = beta*y + alpha*this*x'.
 
template<class T , class U >
U & transpose_mult (const T &x, U &y, REAL alpha=(REAL) 1.0, REAL beta=(REAL) 0.0) const
 Does the operation y = beta*y + alpha*this'*x.
 
template<class T , class U >
U & mult (const T &x, U &y, REAL alpha=(REAL) 1.0, REAL beta=(REAL) 0.0) const
 Does the operation y = beta*y + alpha*this*x.
 
template<class M >
M & get_sub_mat (unsigned row_start, unsigned row_end, unsigned col_start, unsigned col_end, M &m, Transposition trans=eNoTranspose) const
 Gets the specified sub matrix. More...
 
template<class ForwardIterator , class M >
M & select_columns (ForwardIterator col_start, ForwardIterator col_end, M &m) const
 Gets a submatrix of columns (not necessarily a block) More...
 
template<class M >
M & select_columns (std::vector< bool > &col_select, M &m) const
 Gets a submatrix of columns (not necessarily a block) More...
 
template<class ForwardIterator , class M >
M & select_rows (ForwardIterator row_start, ForwardIterator row_end, M &m) const
 Gets a submatrix of rows (not necessarily a block) More...
 
template<class M >
M & select_rows (std::vector< bool > &row_select, M &m) const
 Gets a submatrix of rows (not necessarily a block) More...
 
template<class ForwardIterator1 , class ForwardIterator2 , class X >
X & select (ForwardIterator1 row_start, ForwardIterator1 row_end, ForwardIterator2 col_start, ForwardIterator2 col_end, X &m) const
 Gets a submatrix (not necessarily a block) More...
 
template<class X >
X & select (const std::vector< bool > &rows, const std::vector< bool > &cols, X &m) const
 Gets a submatrix (not necessarily a block)
 
template<class M >
M & select_square (const std::vector< bool > &indices, M &result) const
 Gets a square submatrix (not necessarily a block)
 
template<class ForwardIterator , class M >
M & select_square (ForwardIterator start, ForwardIterator end, M &m) const
 
CONST_COLUMN_ITERATOR column_iterator_begin () const
 Get an iterator to the beginning.
 
CONST_COLUMN_ITERATOR column_iterator_end () const
 Get an iterator to the end.
 
CONST_ROW_ITERATOR row_iterator_begin () const
 Get an iterator to the beginning.
 
CONST_ROW_ITERATOR row_iterator_end () const
 Get an iterator to the end.
 
COLUMN_ITERATOR block_column_iterator_begin (unsigned row_start, unsigned row_end, unsigned col_start, unsigned col_end)
 Gets a column iterator to a block.
 
COLUMN_ITERATOR block_column_iterator_end (unsigned row_start, unsigned row_end, unsigned col_start, unsigned col_end)
 Gets a column iterator to a block.
 
ROW_ITERATOR block_row_iterator_begin (unsigned row_start, unsigned row_end, unsigned col_start, unsigned col_end)
 Gets a row iterator to a block.
 
ROW_ITERATOR block_row_iterator_end (unsigned row_start, unsigned row_end, unsigned col_start, unsigned col_end)
 Gets a row iterator to a block.
 
template<class V >
MATRIXX & set_row (unsigned row, const V &v)
 
template<class V >
MATRIXX & set_column (unsigned column, const V &v)
 
template<class M >
MATRIXX & set_sub_mat (unsigned row_start, unsigned col_start, const M &m, Transposition trans=eNoTranspose)
 Sets the specified sub matrix. More...
 
ROW_ITERATOR row_iterator_begin ()
 Get an iterator to the beginning (iterates column by column)
 
ROW_ITERATOR row_iterator_end ()
 Get an iterator to the end.
 
COLUMN_ITERATOR column_iterator_begin ()
 Get an iterator to the beginning (iterates column by column)
 
COLUMN_ITERATOR column_iterator_end ()
 Get an iterator to the end.
 
template<>
MATRIX3set_column (unsigned i, const ORIGIN3 &o)
 Sets the specified column of the matrix. More...
 
template<>
ORIGIN3get_column (unsigned i, ORIGIN3 &result) const
 Gets the specified column of the matrix.
 
template<>
MATRIX3set_row (unsigned i, const ORIGIN3 &o)
 Sets the specified row of the matrix. More...
 
template<>
ORIGIN3get_row (unsigned i, ORIGIN3 &result) const
 Gets the specified row of the matrix. More...
 

Static Public Member Functions

static VECTOR3 calc_differential (const MATRIX3 &R1, const MATRIX3 &R2)
 Calculates the differential between two rotations.
 
static MATRIX3 invert (const MATRIX3 &m)
 Determines the inverse of the given matrix.
 
static MATRIX3 rot_X (REAL angle)
 Returns the rotation matrix of the specified angle around the X axis.
 
static MATRIX3 rot_Y (REAL angle)
 Returns the rotation matrix of the specified angle around the Y axis.
 
static MATRIX3 rot_Z (REAL angle)
 Returns the rotation matrix of the specified angle around the Z axis.
 
static MATRIX3 skew_symmetric (REAL a, REAL b, REAL c)
 Constructs a skew-symmetric matrix from the given values. More...
 
static MATRIX3 skew_symmetric (const ORIGIN3 &o)
 Constructs a skew-symmetric matrix from the given values. More...
 
static MATRIX3 skew_symmetric (const VECTOR3 &v)
 Constructs a skew-symmetric matrix from the given values. More...
 
static VECTOR3 inverse_skew_symmetric (const MATRIX3 &R)
 Constructs the vector from a skew-symmetric matrix.
 
static MATRIX3 transpose (const MATRIX3 &m)
 Determines the transpose of this matrix.
 
static bool valid_rotation (const MATRIX3 &R)
 Checks whether this matrix represents a valid rotation for a right-handed coordinate system.
 
static bool valid_rotation_scale (const MATRIX3 &R)
 Checks whether this matrix represents a valid rotation and scale for a right-handed coordinate system.
 
static MATRIX3 identity ()
 
static MATRIX3 zero ()
 
template<class V , class U , class W >
static W & diag_mult (const V &d, const U &v, W &result)
 Determines the transpose of a matrix and stores the result in a given matrix. More...
 

Detailed Description

A 3x3 matrix that may be used for orientation, inertia tensors, etc.

Constructor & Destructor Documentation

MATRIX3::MATRIX3 ( const REAL *  array)

Constructs a matrix from an array.

Parameters
arrayan array of 9 REAL values in row-major format

Member Function Documentation

CONST_COLUMN_ITERATOR MATRIX3::block_column_iterator_begin ( unsigned  row_start,
unsigned  row_end,
unsigned  col_start,
unsigned  col_end 
) const
inline

This file consists of general routines for constant matrices.

Gets a column iterator to a block

template<class V , class U , class W >
static W& MATRIX3::diag_mult ( const V &  d,
const U &  v,
W &  result 
)
inlinestatic

Determines the transpose of a matrix and stores the result in a given matrix.

Multiplies the diagonal matrix formed from d by the vector v

ORIGIN3 MATRIX3::get_column ( unsigned  i) const

Gets the specified column of the matrix.

Parameters
ithe 0-index of the column

Referenced by calc_differential(), get_column(), and valid_rotation_scale().

template<>
ORIGIN3& MATRIX3::get_row ( unsigned  i,
ORIGIN3 result 
) const

Gets the specified row of the matrix.

Parameters
ithe 0-index of the row

References get_row().

ORIGIN3 MATRIX3::get_row ( unsigned  i) const

Gets the specified row of the matrix.

Parameters
ithe 0-index of the row

Referenced by FIXEDJOINT::evaluate_constraints(), and get_row().

template<class M >
M& MATRIX3::get_sub_mat ( unsigned  row_start,
unsigned  row_end,
unsigned  col_start,
unsigned  col_end,
M &  m,
Transposition  trans = eNoTranspose 
) const
inline

Gets the specified sub matrix.

Parameters
row_startthe row to start (inclusive)
row_endthe row to end (exclusive)
col_startthe column to start (inclusive)
col_endthe column to end (exclusive)
transposedetermines whether to store the transpose of the submatrix into m
Returns
a (row_end - row_start) x (col_end - col_start) sized matrix
template<class ForwardIterator1 , class ForwardIterator2 , class X >
X& MATRIX3::select ( ForwardIterator1  row_start,
ForwardIterator1  row_end,
ForwardIterator2  col_start,
ForwardIterator2  col_end,
X &  m 
) const
inline

Gets a submatrix (not necessarily a block)

Parameters
row_startan iterator pointing to the beginning of a container of row indices (container need not be sorted)
row_endan iterator pointing to the end of a container of row indices (container need not be sorted)
col_startan iterator pointing to the beginning of a container of column indices (container need not be sorted)
col_endan iterator pointing to the end of a container of column indices (container need not be sorted)
Mcontains the submatrix on return
template<class ForwardIterator , class M >
M& MATRIX3::select_columns ( ForwardIterator  col_start,
ForwardIterator  col_end,
M &  m 
) const
inline

Gets a submatrix of columns (not necessarily a block)

Parameters
col_startan iterator pointing to the beginning of a container of column indices (container need not be sorted)
col_endan iterator pointing to the end of a container of column indices (container need not be sorted)
Mcontains the submatrix on return
template<class M >
M& MATRIX3::select_columns ( std::vector< bool > &  col_select,
M &  m 
) const
inline

Gets a submatrix of columns (not necessarily a block)

Parameters
col_selecta vector of bools; if the entry is true in the vector, the column is selected
Mcontains the submatrix on return
template<class ForwardIterator , class M >
M& MATRIX3::select_rows ( ForwardIterator  row_start,
ForwardIterator  row_end,
M &  m 
) const
inline

Gets a submatrix of rows (not necessarily a block)

Parameters
row_startan iterator pointing to the beginning of a container of row indices (container need not be sorted)
row_endan iterator pointing to the end of a container of row indices (container need not be sorted)
Mcontains the submatrix on return
template<class M >
M& MATRIX3::select_rows ( std::vector< bool > &  row_select,
M &  m 
) const
inline

Gets a submatrix of rows (not necessarily a block)

Parameters
row_selecta vector of bools; if the entry is true in the vector, the row is selected
Mcontains the submatrix on return
template<class ForwardIterator , class M >
M& MATRIX3::select_square ( ForwardIterator  start,
ForwardIterator  end,
M &  m 
) const
inline
Parameters
startan iterator pointing to the beginning of a container of row/column indices (container need not be sorted)
endan iterator pointing to the end of a container of row/column indices (container need not be sorted)
Mcontains the submatrix on return
template<>
MATRIX3& MATRIX3::set_column ( unsigned  i,
const ORIGIN3 o 
)

Sets the specified column of the matrix.

Parameters
ithe 0-index of the column
oa 3D vector
template<>
MATRIX3& MATRIX3::set_row ( unsigned  i,
const ORIGIN3 o 
)

Sets the specified row of the matrix.

Parameters
ithe 0-index of the row
oa 3D vector
Note
the number of columns of this must be three!
template<class M >
MATRIXX& MATRIX3::set_sub_mat ( unsigned  row_start,
unsigned  col_start,
const M &  m,
Transposition  trans = eNoTranspose 
)
inline

Sets the specified sub matrix.

Parameters
row_startthe row to start (inclusive)
col_startthe column to start (inclusive)
mthe source matrix
transposedetermines whether the m is to be transposed
Note
fails assertion if m is too large to insert into this
MATRIX3 MATRIX3::skew_symmetric ( REAL  x,
REAL  y,
REAL  z 
)
static
MATRIX3 MATRIX3::skew_symmetric ( const ORIGIN3 v)
static

Constructs a skew-symmetric matrix from the given values.

The skew symmetric matrix generated will be: | 0 -v.z() v.y() | | v.z() 0 -v.x() | | -v.y() v.x() 0 |

References skew_symmetric().

MATRIX3 MATRIX3::skew_symmetric ( const VECTOR3 v)
static

Constructs a skew-symmetric matrix from the given values.

The skew symmetric matrix generated will be: | 0 -v.z() v.y() | | v.z() 0 -v.x() | | -v.y() v.x() 0 |

References skew_symmetric().


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