grad::VecRotInit< T, MatrixExpr > Class Template Reference

#include <matvec.h>

Collaboration diagram for grad::VecRotInit< T, MatrixExpr >:

Public Member Functions
	VecRotInit (const MatrixExpression< MatrixExpr, 3, 3 > &R)
index_type	iGetNumRows () const
void	Initialize (Vector< T, 3 > &unit) const

Static Public Attributes
static const index_type	iNumRows = 3

Static Private Member Functions
static void	RotCo (const T &phi, T &cf)

Private Attributes
const MatrixExpression < MatrixExpr, 3, 3 >	R

Detailed Description

template<typename T, typename MatrixExpr>
class grad::VecRotInit< T, MatrixExpr >

Definition at line 2814 of file matvec.h.

Constructor & Destructor Documentation

template<typename T , typename MatrixExpr >

grad::VecRotInit< T, MatrixExpr >::VecRotInit ( const MatrixExpression< MatrixExpr, 3, 3 > &  R )

inline

Definition at line 2818 of file matvec.h.

            :R(R) {
        }

Member Function Documentation

template<typename T , typename MatrixExpr >

index_type grad::VecRotInit< T, MatrixExpr >::iGetNumRows ( void  ) const

inline

Definition at line 2822 of file matvec.h.

References grad::Vector< T, N_rows >::iNumRows.

{ return iNumRows; }

template<typename T , typename MatrixExpr >

void grad::VecRotInit< T, MatrixExpr >::Initialize ( Vector< T, 3 > &  unit ) const

inline

Definition at line 2888 of file matvec.h.

References a, grad::atan2(), grad::Cross(), grad::Dot(), Mat3x3::GetCol(), grad::Matrix< T, N_rows, N_cols >::GetCol(), R, grad::sqrt(), and grad::Transpose().

                                                  {
            // Modified from Appendix 2.4 of
            //
            // author = {Marco Borri and Lorenzo Trainelli and Carlo L. Bottasso},
            // title = {On Representations and Parameterizations of Motion},
            // journal = {Multibody System Dynamics},
            // volume = {4},
            // pages = {129--193},
            // year = {2000}
            using std::atan2;
            using std::sqrt;
            
            const T cosphi = 0.5 * (R(1, 1) + R(2, 2) + R(3, 3) - 1.);
            
            if (cosphi > 0.) {
                unit(1) = 0.5*(R(3, 2) - R(2, 3));
                unit(2) = 0.5*(R(1, 3) - R(3, 1));
                unit(3) = 0.5*(R(2, 1) - R(1, 2));
                
                const T sinphi2 = Dot(unit, unit);
                T sinphi;
                
                if (sinphi2 != 0) {
                    sinphi = sqrt(sinphi2);
                } else {
                    sinphi = unit(1);
                }
                
                const T phi = atan2(sinphi, cosphi);
                T a;
                RotCo(phi, a);
                unit /= a;
            } else {
                // -1 <= cosphi <= 0
                Matrix<T, 3, 3> eet = (R + Transpose(R)) * 0.5;
                eet(1, 1) -= cosphi;
                eet(2, 2) -= cosphi;
                eet(3, 3) -= cosphi;
                // largest (abs) component of unit vector phi/|phi|
                index_type maxcol = 1;
                if (eet(2, 2) > eet(1, 1)) {
                    maxcol = 2;
                }
                if (eet(3, 3) > eet(maxcol, maxcol)) {
                    maxcol = 3;
                }
                unit = (eet.GetCol(maxcol)/sqrt(eet(maxcol, maxcol)*(1. - cosphi)));
                T sinphi(0.);
                for (index_type i = 1; i <= 3; ++i) {
                    sinphi -= Cross(unit, R.GetCol(i))(i) * 0.5;
                }

                unit *= atan2(sinphi, cosphi);
            }
        }

Here is the call graph for this function:

template<typename T , typename MatrixExpr >

static void grad::VecRotInit< T, MatrixExpr >::RotCo ( const T &  phi, T &  cf  )

inlinestaticprivate

Definition at line 2945 of file matvec.h.

References grad::cos(), grad::fabs(), grad::sin(), and grad::sqrt().

                                               {
            // This algorithm is a simplified version of RotCo in RotCoeff.hc
            // from Marco Morandini  <morandini@aero.polimi.it>
            // and Teodoro Merlini  <merlini@aero.polimi.it>
            using std::sin;
            using std::cos;
            using std::sqrt;
            using std::fabs;
        
            T phip[10];
            T phi2(phi * phi);

            if (fabs(phi) < RotCoeff::SerThrsh[0]) {
                phip[0] = 1.;
            
                for (index_type j = 1; j <= 9; j++) {
                    phip[j] = phip[j - 1] * phi2;
                }

                cf = 0.;
            
                for (index_type j = 0; j < RotCoeff::SerTrunc[0]; j++) {
                    cf += phip[j] / RotCoeff::SerCoeff[0][j];
                }

                return;
            } 
        
            const T pd(sqrt(phi2));
            cf = sin(pd) / pd;                 // a = sin(phi)/phi
        };

Here is the call graph for this function:

Member Data Documentation

template<typename T , typename MatrixExpr >

const index_type grad::VecRotInit< T, MatrixExpr >::iNumRows = 3

static

Definition at line 2816 of file matvec.h.

template<typename T , typename MatrixExpr >

const MatrixExpression<MatrixExpr, 3, 3> grad::VecRotInit< T, MatrixExpr >::R

private

Definition at line 2975 of file matvec.h.

---

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

• matvec.h