claraty::RMatrix

claraty::RMatrix< T > Class Template Reference

#include <rotation_matrix.h>

Inheritance diagram for claraty::RMatrix< T >:

[legend]Collaboration diagram for claraty::RMatrix< T >:

[legend]List of all members.


Public Member Functions
	RMatrix ()
	RMatrix (ROTATION_TYPE rot_type, T rot1, T rot2=T(), T rot3=T())
	RMatrix (T qi, T qj, T qk, T qs)
	RMatrix (const Quaternion< T > q)
	RMatrix (const Vector< T > &nx, const Vector< T > &ny, const Vector< T > &nz)
	RMatrix (const Vector< T > &a, const Vector< T > &b)
	RMatrix (const Base_RMatrix< T > &rhs)
	RMatrix (const Matrix< T > &rhs)
template<class TS>
	RMatrix (const RMatrix< TS > &rhs)
	RMatrix (const Point< T > &x_axis)
T &	operator() (int r, int c)
const T &	operator() (int r, int c) const
const RMatrix	operator- ()
void	set_identity ()
bool	is_identity ()
void	set_zero ()
void	set_rpy_angles (T roll, T pitch, T yaw)
void	get_rpy_angles (T &roll, T &pitch, T &yaw) const
Vector< T >	get_x_axis () const
Vector< T >	get_y_axis () const
Vector< T >	get_z_axis () const
bool	operator== (const RMatrix &rhs) const
	operator Matrix () const
	operator Matrix_NxM () const
RMatrix	transpose () const
bool	io (FDM_Map map)
RMatrix< T >	reverse ()
Public Attributes
T	_data [3][3]
Static Public Attributes
static const Base_RMatrix< T >	null
static const Base_RMatrix< T >	identity
Friends
template<class Y>
std::ostream &	operator<< (std::ostream &os, const RMatrix< Y > &m)
template<class Y>
const RMatrix< Y >	operator * (const RMatrix< Y > &lhs, const RMatrix< Y > &rhs)
template<class Y>
const Matrix< Y >	operator * (const RMatrix< Y > &lhs, const Matrix< Y > &rhs)
template<class Y>
const Matrix< Y >	operator * (const Matrix< Y > &lhs, const RMatrix< Y > &rhs)
template<class Y>
const Point< Y >	operator * (const RMatrix< Y > &lhs, const Point< Y > &rhs)

Detailed Description

template<class T>
class claraty::RMatrix< T >

Definition at line 73 of file rotation_matrix.h.

Constructor & Destructor Documentation

template<class T>

claraty::RMatrix< T >::RMatrix ( ) [inline]

Definition at line 94 of file rotation_matrix.h.

Referenced by claraty::RMatrix< T >::RMatrix().

00094 : Base_RMatrix<T>(identity) {}

template<class T>

claraty::RMatrix< T >::RMatrix	(	ROTATION_TYPE	rot_type,
		T	rot1,
		T	rot2 = `T()`,
		T	rot3 = `T()`
	)

Definition at line 232 of file rotation_matrix.h.

References claraty::Base_RMatrix< T >::_data, claraty::ANGLE_AXIS, claraty::EULER_ZYZ, claraty::RPY, claraty::RX, claraty::RY, and claraty::RZ.

00233 { 
00234   T s1 = (T)sin(rot1);
00235   T c1 = (T)cos(rot1);
00236   T s2 = (T)sin(rot2);
00237   T c2 = (T)cos(rot2);
00238   T s3 = (T)sin(rot3);
00239   T c3 = (T)cos(rot3);
00240   T zero = T();
00241 
00242   switch(rot_type) {
00243 
00244   case RX:
00245     this->_data[0][0] =   1;
00246     this->_data[0][1] =   zero;
00247     this->_data[0][2] =   zero;
00248     this->_data[1][0] =   zero;
00249     this->_data[1][1] =   c1;
00250     this->_data[1][2] = - s1;
00251     this->_data[2][0] =   zero;
00252     this->_data[2][1] =   s1;
00253     this->_data[2][2] =   c1;
00254     break;
00255     
00256   case RY:
00257     this->_data[0][0] =   c1;
00258     this->_data[0][1] =   zero;
00259     this->_data[0][2] =   s1;
00260     this->_data[1][0] =   zero;
00261     this->_data[1][1] =   1;
00262     this->_data[1][2] =   zero;
00263     this->_data[2][0] = - s1;
00264     this->_data[2][1] =   zero;
00265     this->_data[2][2] =   c1;
00266     break;
00267     
00268   case RZ:
00269     this->_data[0][0] =   c1;
00270     this->_data[0][1] = - s1;
00271     this->_data[0][2] =   zero;
00272     this->_data[1][0] =   s1;
00273     this->_data[1][1] =   c1;
00274     this->_data[1][2] =   zero;
00275     this->_data[2][0] =   zero;
00276     this->_data[2][1] =   zero;
00277     this->_data[2][2] =   1;
00278     break;
00279     
00280   case EULER_ZYZ: // Rotation about rotated axes Z, Y, Z
00281     this->_data[0][0] =   c1 * c2 * c3 - s1 * s3;
00282     this->_data[0][1] = - c1 * c2 * s3 - s1 * c3;
00283     this->_data[0][2] =   c1 * s2;
00284     this->_data[1][0] =   s1 * c2 * c3 + c1 * s3;
00285     this->_data[1][1] = - s1 * c2 * s3 + c1 * c3;
00286     this->_data[1][2] =   s1 * s2;
00287     this->_data[2][0] = - s2 * c3;
00288     this->_data[2][1] =   s2 * s3;
00289     this->_data[2][2] =   c2;
00290     break;
00291     
00292   case RPY:  // Rotation about fixed axes X, Y, Z
00293     this->_data[0][0] =   c3 * c2;
00294     this->_data[0][1] =   c3 * s2 * s1 - s3 * c1;
00295     this->_data[0][2] =   c3 * s2 * c1 + s3 * s1;
00296     this->_data[1][0] =   s3 * c2;
00297     this->_data[1][1] =   s3 * s2 * s1 + c3 * c1;
00298     this->_data[1][2] =   s3 * s2 * c1 - c3 * s1;
00299     this->_data[2][0] = - s2;
00300     this->_data[2][1] =   c2 * s1;
00301     this->_data[2][2] =   c2 * c1;
00302     break;
00303     
00304   case ANGLE_AXIS: // Angle-axis rotation about X, Y, Z axes    
00305     { // Solves the problem with crosses over initializations
00306       T theta = (T)sqrt(rot1*rot1+rot2*rot2+rot3*rot3);
00307       T kx = rot1/theta;
00308       T ky = rot2/theta;
00309       T kz = rot3/theta;
00310       T st = (T)sin(theta);
00311       T ct = (T)cos(theta);
00312       T vt = 1 - ct;
00313       this->_data[0][0] = kx * kx * vt + ct;
00314       this->_data[0][1] = kx * ky * vt - kz * st;
00315       this->_data[0][2] = kx * kz * vt + ky * st;
00316       this->_data[1][0] = kx * ky * vt + kz * st;
00317       this->_data[1][1] = ky * ky * vt + ct;
00318       this->_data[1][2] = ky * kz * vt - kx * st;
00319       this->_data[2][0] = kx * kz * vt - ky * st;
00320       this->_data[2][1] = ky * kz * vt + kx * st;
00321       this->_data[2][2] = kz * kz * vt + ct;
00322     }
00323     break;
00324 
00325   default:
00326     std::cout << "rotation matrix error: undefined matrix type" << std::endl;
00327   }
00328 }

template<class T>

claraty::RMatrix< T >::RMatrix	(	T	qi,
		T	qj,
		T	qk,
		T	qs
	)			`[inline]`

Definition at line 333 of file rotation_matrix.h.

References claraty::Quaternion< T >::quaternion_to_rotation_matrix().

00334 {   
00335   Quaternion<T> temp(qi, qj, qk, qs);
00336   *this = temp.quaternion_to_rotation_matrix();
00337 }

Here is the call graph for this function:

template<class T>

claraty::RMatrix< T >::RMatrix ( const Quaternion< T > q ) [inline]

Definition at line 341 of file rotation_matrix.h.

References claraty::Quaternion< T >::quaternion_to_rotation_matrix().

00342 { 
00343   *this = q.quaternion_to_rotation_matrix();
00344 }

Here is the call graph for this function:

template<class T>

claraty::RMatrix< T >::RMatrix	(	const Vector< T > &	nx,
		const Vector< T > &	ny,
		const Vector< T > &	nz
	)

Definition at line 348 of file rotation_matrix.h.

00351 {
00352   for (int r = 0; r < 3; r++) {
00353     this->_data[r][0] = nx[r];
00354     this->_data[r][1] = ny[r];
00355     this->_data[r][2] = nz[r];
00356   }
00357 }

template<class T>

claraty::RMatrix< T >::RMatrix	(	const Vector< T > &	a,
		const Vector< T > &	b
	)			`[inline]`

Definition at line 102 of file rotation_matrix.h.

References claraty::RMatrix< T >::RMatrix().

00102 {*this = RMatrix(Quaternion<T>(a, b));};

Here is the call graph for this function:

template<class T>

claraty::RMatrix< T >::RMatrix ( const Base_RMatrix< T > & rhs ) [inline]

Definition at line 103 of file rotation_matrix.h.

00103 : Base_RMatrix<T>(rhs) {}

template<class T>

claraty::RMatrix< T >::RMatrix ( const Matrix< T > & rhs )

Definition at line 516 of file rotation_matrix.h.

References EPSILON, claraty::N_2D_Array< T >::get_num_of_cols(), claraty::N_2D_Array< T >::get_num_of_rows(), claraty::IDENTITY, and claraty::Matrix< T >::transpose().

00516                               : Base_RMatrix<T>(null) {
00517   if((rhs.get_num_of_rows() != 3) || (rhs.get_num_of_cols() != 3)) {
00518     std::cout << "RMatrix: construction RMatrix from Matrix: num of row/cols != 3" << std::endl;
00519     return;
00520   }
00521   Matrix<T> iden = rhs*rhs.transpose()-Matrix<T>(3,IDENTITY);
00522   for(int r=0; r<3; r++) {
00523     for(int c=0; c<3; c++) {
00524       this->_data[r][c] = rhs(r,c);
00525       if(cl_abs(iden(r,c)) > EPSILON*1000) {  // EPSILON = 1e-8
00526         std::cout << "warning: RMatrix: construction RMatrix from Matrix: matrix is not orthonormal (" << cl_abs(iden(r,c)) << ")" << std::endl;
00527       }
00528     }
00529   }
00530 }

Here is the call graph for this function:

template<class T>

template<class TS>

claraty::RMatrix< T >::RMatrix ( const RMatrix< TS > & rhs ) [inline]

Definition at line 106 of file rotation_matrix.h.

00106                                   {
00107     for(int r=0; r<3; r++) {
00108       for(int c=0; c<3; c++) {
00109         this->_data[r][c] = (T) this->rhs(r,c);
00110       }
00111     }
00112   }

template<class T>

claraty::RMatrix< T >::RMatrix ( const Point< T > & x_axis ) [inline]

Definition at line 115 of file rotation_matrix.h.

References claraty::RPY, claraty::Point< T >::x(), claraty::Point< T >::y(), and claraty::Point< T >::z().

00115                                   {
00116     // Choose a rotation matrix with the x_axis 
00117     // RPY are defined as follows
00118     // Initially, frame's x, y, and z axes same as parent's X, Y, and Z axes
00119     // 1. Apply yaw rotation about the aligned z and Z axes
00120     // 2. Apply pitch rotation about the frame's y axis
00121     // 3. Apply roll rotation about the frame's x axis
00122     double roll= 0;
00123     double pitch= -atan2(x_axis.z(),
00124                          sqrt(x_axis.x()*x_axis.x() + x_axis.y()*x_axis.y()));
00125     double yaw= atan2(x_axis.y(), x_axis.x());
00126     *this= RMatrix<double>(RPY, roll, pitch, yaw);
00127   }

Here is the call graph for this function:

Member Function Documentation

template<class T>

T& claraty::RMatrix< T >::operator()	(	int	r,
		int	c
	)			`[inline]`

Definition at line 131 of file rotation_matrix.h.

References claraty::Base_RMatrix< T >::_data.

00131 {return this->_data[r][c];}

template<class T>

const T& claraty::RMatrix< T >::operator()	(	int	r,
		int	c
	)			const `[inline]`

Definition at line 132 of file rotation_matrix.h.

References claraty::Base_RMatrix< T >::_data.

00132 {return this->_data[r][c];}

template<class T>

const RMatrix< T > claraty::RMatrix< T >::operator- ( )

Definition at line 397 of file rotation_matrix.h.

References claraty::Base_RMatrix< T >::_data.

00398 { 
00399   RMatrix<T> result;
00400   result._data[0][0] = -this->_data[0][0];
00401   result._data[0][1] = -this->_data[1][0];
00402   result._data[0][2] = -this->_data[2][0];
00403   result._data[1][0] = -this->_data[0][1];
00404   result._data[1][1] = -this->_data[1][1];
00405   result._data[1][2] = -this->_data[2][1];
00406   result._data[2][0] = -this->_data[0][2];
00407   result._data[2][1] = -this->_data[1][2];
00408   result._data[2][2] = -this->_data[2][2];
00409   return result;
00410 }

template<class T>

void claraty::RMatrix< T >::set_identity ( ) [inline]

Definition at line 135 of file rotation_matrix.h.

References claraty::RMatrix< T >::identity.

00135 {*this = identity;}

template<class T>

bool claraty::RMatrix< T >::is_identity ( )

Definition at line 363 of file rotation_matrix.h.

References EPSILON.

00364 { 
00365   for (int i=0; i<3; i++) 
00366     for (int j=0; j<3; j++) {
00367       if (i==j) { 
00368         if (cl_abs(this->_data[i][j] - 1) > EPSILON) return false;
00369       }
00370       else if (cl_abs(this->_data[i][j]) > EPSILON) return false;    
00371     }
00372   
00373   return true;
00374 }

template<class T>

void claraty::RMatrix< T >::set_zero ( ) [inline]

Definition at line 137 of file rotation_matrix.h.

References claraty::RMatrix< T >::null.

00137 {*this = null;}

template<class T>

void claraty::RMatrix< T >::set_rpy_angles	(	T	roll,
		T	pitch,
		T	yaw
	)			`[inline]`

Definition at line 416 of file rotation_matrix.h.

References claraty::Base_RMatrix< T >::_data.

Referenced by claraty::RMatrix< T >::io().

00417 { 
00418   T s1 = (T)sin(rot1);
00419   T c1 = (T)cos(rot1);
00420   T s2 = (T)sin(rot2);
00421   T c2 = (T)cos(rot2);
00422   T s3 = (T)sin(rot3);
00423   T c3 = (T)cos(rot3);
00424   T zero = T();
00425     
00426   // Rotation about fixed axes X, Y, Z
00427   this->_data[0][0] =   c3 * c2;
00428   this->_data[0][1] =   c3 * s2 * s1 - s3 * c1;
00429   this->_data[0][2] =   c3 * s2 * c1 + s3 * s1;
00430   this->_data[1][0] =   s3 * c2;
00431   this->_data[1][1] =   s3 * s2 * s1 + c3 * c1;
00432   this->_data[1][2] =   s3 * s2 * c1 - c3 * s1;
00433   this->_data[2][0] = - s2;
00434   this->_data[2][1] =   c2 * s1;
00435   this->_data[2][2] =   c2 * c1;
00436 }

template<class T>

void claraty::RMatrix< T >::get_rpy_angles	(	T &	roll,
		T &	pitch,
		T &	yaw
	)			const `[inline]`

Definition at line 441 of file rotation_matrix.h.

References DOUBLE_EPSILON, and M_PI.

Referenced by claraty::Quaternion< T >::get_rpy_angles(), claraty::RMatrix< T >::io(), and claraty::RMatrix< T >::operator==().

00442 {
00443   pitch = atan2(-this->_data[2][0], sqrt(cl_sqr(this->_data[0][0])+cl_sqr(this->_data[1][0])));
00444 
00445   // The first two cases are the degenerate ones
00446   //
00447   if (cl_abs(pitch-0.5*M_PI) < DOUBLE_EPSILON) {
00448     roll = atan2(this->_data[0][1], this->_data[1][1]);
00449     yaw  = 0.0;
00450   }
00451   if (cl_abs(pitch+0.5*M_PI) < DOUBLE_EPSILON) {
00452     roll = -atan2(this->_data[0][1], this->_data[1][1]);
00453     yaw  = 0.0;
00454   }
00455   else {
00456     roll  = atan2(this->_data[2][1], this->_data[2][2]);
00457     yaw   = atan2(this->_data[1][0], this->_data[0][0]);
00458   }
00459 }

template<class T>

Vector<T> claraty::RMatrix< T >::get_x_axis ( ) const [inline]

Definition at line 156 of file rotation_matrix.h.

00156                                {
00157     return Vector<T>(Point_d(this->_data[0][0], this->_data[1][0], this->_data[2][0]));
00158   }

template<class T>

Vector<T> claraty::RMatrix< T >::get_y_axis ( ) const [inline]

Definition at line 159 of file rotation_matrix.h.

00159                                {
00160     return Vector<T>(Point_d(this->_data[0][1], this->_data[1][1], this->_data[2][1]));
00161   }

template<class T>

Vector<T> claraty::RMatrix< T >::get_z_axis ( ) const [inline]

Definition at line 162 of file rotation_matrix.h.

00162                                {
00163     return Vector<T>(Point_d(this->_data[0][2], this->_data[1][2], this->_data[2][2]));
00164   }

template<class T>

bool claraty::RMatrix< T >::operator== ( const RMatrix< T > & rhs ) const [inline]

Definition at line 166 of file rotation_matrix.h.

References claraty::RMatrix< T >::get_rpy_angles().

00166                                             {
00167     T r, p, y, rr, rp, ry;
00168     get_rpy_angles(r, p, y);
00169     rhs.get_rpy_angles(rr, rp, ry);
00170     return r == rr && p == rp && y == ry;
00171   }

Here is the call graph for this function:

template<class T>

claraty::RMatrix< T >::operator Matrix ( ) const [inline]

Definition at line 179 of file rotation_matrix.h.

References claraty::Base_RMatrix< T >::_data.

00179 {return Matrix<T>(3,3, &this->_data[0][0]);}

template<class T>

claraty::RMatrix< T >::operator Matrix_NxM ( ) const [inline]

Definition at line 181 of file rotation_matrix.h.

References claraty::Base_RMatrix< T >::_data.

00181                                        {
00182     return Matrix_NxM<T, 3, 3>(&this->_data[0][0]);
00183   }

template<class T>

RMatrix< T > claraty::RMatrix< T >::transpose ( ) const

Definition at line 379 of file rotation_matrix.h.

References claraty::Base_RMatrix< T >::_data.

Referenced by claraty::inverse(), claraty::RMatrix< T >::reverse(), and claraty::ME_Body::rotate_inertia_tensor().

00380 { 
00381   RMatrix result;
00382   result._data[0][0] = this->_data[0][0];
00383   result._data[0][1] = this->_data[1][0];
00384   result._data[0][2] = this->_data[2][0];
00385   result._data[1][0] = this->_data[0][1];
00386   result._data[1][1] = this->_data[1][1];
00387   result._data[1][2] = this->_data[2][1];
00388   result._data[2][0] = this->_data[0][2];
00389   result._data[2][1] = this->_data[1][2];
00390   result._data[2][2] = this->_data[2][2];
00391   return result;
00392 }

template<class T>

bool claraty::RMatrix< T >::io ( FDM_Map map )

Definition at line 535 of file rotation_matrix.h.

References claraty::FDM_Map::field(), claraty::RMatrix< T >::get_rpy_angles(), claraty::FDM_Map::is_read(), claraty::FDM_Map::is_write(), and claraty::RMatrix< T >::set_rpy_angles().

00536 {
00537   T roll, pitch, yaw;
00538   
00539   if (map.is_write()) {
00540     get_rpy_angles(roll, pitch, yaw);
00541   }
00542   
00543   bool ok = map.field("rx", roll);
00544   ok &= map.field("ry", pitch);
00545   ok &= map.field("rz", yaw);
00546     
00547   if (map.is_read())
00548   set_rpy_angles(roll, pitch, yaw);
00549 
00550   return ok;
00551 }

Here is the call graph for this function:

template<class T>

RMatrix<T> claraty::RMatrix< T >::reverse ( ) [inline]

Definition at line 190 of file rotation_matrix.h.

References claraty::RMatrix< T >::transpose().

00190 {return transpose();}

Here is the call graph for this function:

Friends And Related Function Documentation

template<class T>

template<class Y>

std::ostream& operator<<	(	std::ostream &	os,
		const RMatrix< Y > &	m
	)			`[friend]`

template<class T>

template<class Y>

const RMatrix<Y> operator *	(	const RMatrix< Y > &	lhs,
		const RMatrix< Y > &	rhs
	)			`[friend]`

template<class T>

template<class Y>

const Matrix<Y> operator *	(	const RMatrix< Y > &	lhs,
		const Matrix< Y > &	rhs
	)			`[friend]`

template<class T>

template<class Y>

const Matrix<Y> operator *	(	const Matrix< Y > &	lhs,
		const RMatrix< Y > &	rhs
	)			`[friend]`

template<class T>

template<class Y>

const Point<Y> operator *	(	const RMatrix< Y > &	lhs,
		const Point< Y > &	rhs
	)			`[friend]`

Member Data Documentation

template<class T>

const Base_RMatrix< T > claraty::RMatrix< T >::null [static]

Definition at line 187 of file rotation_matrix.h.

Referenced by claraty::RMatrix< T >::set_zero().

template<class T>

const Base_RMatrix< T > claraty::RMatrix< T >::identity [static]

Definition at line 188 of file rotation_matrix.h.

Referenced by claraty::RMatrix< T >::set_identity().

template<class T>

T claraty::Base_RMatrix< T >::_data[3][3] [inherited]

Definition at line 55 of file rotation_matrix.h.

Referenced by claraty::operator *(), claraty::RMatrix< T >::operator Matrix(), claraty::RMatrix< T >::operator Matrix_NxM(), claraty::RMatrix< T >::operator()(), claraty::RMatrix< T >::operator-(), claraty::RMatrix< T >::RMatrix(), claraty::RMatrix< T >::set_rpy_angles(), and claraty::RMatrix< T >::transpose().

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

rotation_matrix.h

Last modified: June 05 2007 10:19:53

JPL PRIVACY STATEMENT GLOSSARY SITEMAP FEEDBACK CONTACT US

Site Manager: Issa A.D. Nesnas
Site Maintainer: Kelly Breed
Document Clearance #: CL 07-0635

claraty::RMatrix< T > Class Template Reference

Public Member Functions

Public Attributes

Static Public Attributes

Friends

Detailed Description

template<class T> class claraty::RMatrix< T >

Constructor & Destructor Documentation

Member Function Documentation

Friends And Related Function Documentation

Member Data Documentation

template<class T>
class claraty::RMatrix< T >