#ifndef _gena_h_ #define _gena_h_ const Matrix gena( const Vector & uvw, const Vector & pqr, const Vector & Q, double g ) { const double u = uvw[0]; const double v = uvw[1]; const double w = uvw[2]; //const double p = pqr[0]; const double q = pqr[1]; //const double r = pqr[2]; const double q0 = Q[0]; const double q1 = Q[1]; const double q2 = Q[2]; const double q3 = Q[3]; Matrix A; const Matrix C( quatDC( Q ) ); const Matrix Wxq( quatW( pqr ) ); const Matrix Wx( eulerWx( pqr ) ); const Matrix AA( 0.0 ); const Matrix AB( C.transpose() ); const Matrix AC( Vector( -2 * v * q3 + 2 * w * q2, 2 * v * q2 + 2 * w * q3, -4 * u * q2 + 2 * v * q1, -4 * u * q3 - 2 * v * q0 ), Vector( 2 * u * q3 - 2 * q * q1, 2 * u * q2 - 4 * v * q1 - 2 * w * q0, 2 * u * q1 + 2 * w * q3, 2 * u * q0 - 4 * v * q3 + 2 * w * q2 ), Vector( -2 * u * q2 + 2 * v * q1, 2 * u * q3 + 2 * v * q0 - 4 * w * q1, -2 * u * q0 + 2 * v * q3 - 4 * w * q2, 2 * u * q1 + 2 * v * q2 ) ); const Matrix AD( 0.0 ); const Matrix AE( 0.0 ); const Matrix AF( -Wx ); const Matrix AG( Vector( -2 * g * q2, 2 * g * q3, -2 * g * q0, 2 * g * q1 ), Vector( 2 * g * q1, 2 * g * q0, 2 * g * q3, 2 * g * q2 ), Vector( 0 -4 * g * q1, -4 * g * q2, 0 ) ); Matrix AH; AH[0][0] = C[0][2]; AH[1][0] = C[1][2]; AH[2][0] = C[2][2]; const Matrix AI( 0.0 ); const Matrix AJ( 0.0 ); const Matrix AK( Wxq ); const Matrix AL( 0.0 ); const Matrix AM( 0.0 ); A.insert( AA, 0, 0 ); A.insert( AB, 0, 3 ); A.insert( AC, 0, 6 ); A.insert( AD, 0, 10 ); A.insert( AE, 3, 0 ); A.insert( AF, 3, 3 ); A.insert( AG, 3, 6 ); A.insert( AH, 3, 10 ); A.insert( AI, 6, 0 ); A.insert( AJ, 6, 3 ); A.insert( AK, 6, 6 ); A.insert( AL, 6, 10 ); A.insert( AM, 10, 0 ); return A; } #endif
