示例#1
0
int euler312_quat(
    double angle[3],    /* Inout: r  Method=1, 0=YAW, 1=ROLL, 2=PITCH. */
    double quat[4],     /* Inout: r  Method=0, left handed quaternion matrix. */
    int method,         /*    In: -- 0 = Make quaternion from angles,
   			             1 = Make angles from quaternion
   			             2 = Make angles from quaternion but use previous
   				     values to prevent singularities. */
    double *prev)       /*   In: r  Previous values of euler angles. */
{

   double haft_angle[3];
   double mat00, mat01, mat02, mat10, mat11, mat12,  mat22; 
   double s1;		   
   double c1;		   
   double s2;		   
   double c2;		   
   double s3;		   
   double c3;		   
   double tmp;
   int ret = 0;
   static unsigned short error_flag[5] = {0, 0, 0, 0, 0}; /* Send errors only once */

   if (method == 0){
   
       /* Compute sines and cosines of 0.5*eulers */
       V_SCALE(haft_angle, angle, 0.5);
       s1 = sin(haft_angle[0]);
       c1 = cos(haft_angle[0]);
       s2 = sin(haft_angle[1]);
       c2 = cos(haft_angle[1]);
       s3 = sin(haft_angle[2]);
       c3 = cos(haft_angle[2]); 
       
       quat[0] =  c1*c2*c3 - s1*s2*s3; 
       quat[1] = -c1*s2*c3 + s1*c2*s3;
       quat[2] = -c1*c2*s3 - s1*s2*c3;
       quat[3] = -c1*s2*s3 - s1*c2*c3;
            
   } else if (method == 1) {
#define TOLERANCE 1.0e-15

       mat12 =  2.*(quat[2]*quat[3] - quat[0]*quat[1]);
       
       /* Within normal range for asin function */
       if (-1.0 <= mat12 && mat12 <= 1.0) {
               angle[1] = asin(mat12);
               if (M_ABS(angle[1] - M_PI_2) < 1.0e-6) {
                       mat00 = 1. - 2. * (quat[2] * quat[2] + quat[3] * quat[3]);
                       mat01 = 2. * (quat[1] * quat[2] - quat[0] * quat[3]);
        	       angle[0] = atan2(mat01, mat00);
        	       angle[1] = M_PI_2;
        	       angle[2] = 0.0;
        	       ret = TM_SING_312_P;
        	       if ( error_flag[0] == 0 ) {
        		   tm_print_error(ret);
        		   error_flag[0]=1;
        	       }
               } else if (M_ABS(angle[1] + M_PI_2) < 1.0e-6) {
                       mat00 = 1. - 2. * (quat[2] * quat[2] + quat[3] * quat[3]);
                       mat01 = 2. * (quat[1] * quat[2] - quat[0] * quat[3]);
        	       angle[0] = atan2(mat01, mat00);
        	       angle[1] = -M_PI_2;
        	       angle[2] = 0.0;
        	       ret = TM_SING_312_N;
        	       if ( error_flag[1] == 0 ) {
        		   tm_print_error(ret);
        		   error_flag[1]=1;
        	       }
               } else {
                       mat02 = 2. * (quat[1] * quat[3] + quat[0] * quat[2]);
                       mat10 = 2. * (quat[1] * quat[2] + quat[0] * quat[3]);
                       mat11 = 1. - 2. * (quat[3] * quat[3] + quat[1] * quat[1]);
                       mat22 = 1. - 2. * (quat[1] * quat[1] + quat[2] * quat[2]);
        	       angle[0] = atan2(-mat10, mat11);
        	       angle[2] = atan2(-mat02, mat22);
               }
       }
       /* Out of normal range for asin func, but within tolerance */
       else if (1.0 < mat12 && mat12 <= (1.0 + TOLERANCE)) {
               mat00 = 1. - 2. * (quat[2] * quat[2] + quat[3] * quat[3]);
               mat01 = 2. * (quat[1] * quat[2] - quat[0] * quat[3]);
               angle[0] = atan2(mat01, mat00);
               angle[1] = M_PI_2;
               angle[2] = 0.0;
               ret = TM_SING_312_P;
               if ( error_flag[2] == 0 ) {
        	   tm_print_error(ret);
        	   error_flag[2]=1;
               }
       } else if ((-1.0 - TOLERANCE) <= mat12 && mat12 < -1.0) {
               mat00 = 1. - 2. * (quat[2] * quat[2] + quat[3] * quat[3]);
               mat01 = 2. * (quat[1] * quat[2] - quat[0] * quat[3]);
               angle[0] = atan2(mat01, mat00);
               angle[1] = -M_PI_2;
               angle[2] = 0.0;
               ret = TM_SING_312_N;
               if ( error_flag[3] == 0 ) {
        	   tm_print_error(ret);
        	   error_flag[3]=1;
               }
       }
       /* Error: Out of normal range & beyond tolerance for asin func*/
       else {
               double zero = 0.0;
               ret = TM_ANG_NAN;
               if ( error_flag[4] == 0 ) {
        	   tm_print_error(ret);
        	   error_flag[4]=1;
               }
               angle[0] = angle[1] = angle[2] = 0.0 / zero;
       }
#undef TOLERANCE
        } else if (method == 2) {
#define TOLERANCE 0.0314159265358979    /* 1.8 degree tolerance */

           mat12 =	2.*(quat[2]*quat[3] + quat[1]*quat[0]);

           if (M_ABS(mat12 - 1.0) < 1.0e-6) {
           	   mat00 = 1. - 2. * (quat[2] * quat[2] + quat[3] * quat[3]);
           	   mat01 = 2. * (quat[1] * quat[2] - quat[0] * quat[3]);
           	   angle[0] = atan2(mat01, mat00) - prev[2];
           	   angle[1] = M_PI_2;
           	   angle[2] = prev[2];
           	   tmp = angle[0] - prev[0];
           	   if (M_ABS(tmp - 2.0 * M_PI) < TOLERANCE)
           		   angle[0] -= 2.0 * M_PI;
           	   else if (M_ABS(tmp + 2.0 * M_PI) < TOLERANCE)
           		   angle[0] += 2.0 * M_PI;

           } else if (M_ABS(mat12 + 1.0) < 1.0e-6) {
           	   mat00 = 1. - 2. * (quat[2] * quat[2] + quat[3] * quat[3]);
           	   mat01 = 2. * (quat[1] * quat[2] - quat[0] * quat[3]);
           	   angle[0] = atan2(mat01, mat00) + prev[2];
           	   angle[1] = -M_PI_2;
           	   angle[2] = prev[2];
           	   tmp = angle[0] - prev[0];
           	   if (M_ABS(tmp - 2.0 * M_PI) < TOLERANCE)
           		   angle[0] -= 2.0 * M_PI;
           	   else if (M_ABS(tmp + 2.0 * M_PI) < TOLERANCE)
           		   angle[0] += 2.0 * M_PI;
           } else {
           	   mat02 = 2. * (quat[1] * quat[3] + quat[0] * quat[2]);
           	   mat10 = 2. * (quat[1] * quat[2] + quat[0] * quat[3]);
           	   mat11 = 1. - 2. * (quat[3] * quat[3] + quat[1] * quat[1]);
           	   mat22 = 1. - 2. * (quat[1] * quat[1] + quat[2] * quat[2]);
           	   angle[0] = atan2(-mat10, mat11);
           	   angle[1] = asin(mat12);
           	   angle[2] = atan2(-mat02, mat22);
           }
#undef TOLERANCE
        }

   return(ret);
}
示例#2
0
/**
* collision/landing gear model using vertical displacement method.
*
* @param  {struct} gear parameters and state
* @return {struct} gear forces and moments
**/
void F_GEAR_COLLISION(struct gear_inputs_def *pIn, struct gear_outputs_def *pOut) {
	// locals
	double _omega[3][3],			// rotational velocity matrix
	       _displacement,			// the displacement of the contact force under ground
	       _vectorTemp1[3],			// temporary vector
	       _vectorTemp2[3],			// temporary vector
	  	   _wheelVelocity_body[3],	// wheel velocity vector (body)
	  	   _wheelVelocity_earth[3],	// wheel velocity vector (earth)
	  	   _wheelPosition_body[3],	// wheel position vector (body)
	  	   _wheelPosition_earth[3],	// wheel position vector (earth)
	  	   _body2earth[3][3],		// body to earth transformation matrix
	  	   _earth2body[3][3],		// earth to body transformation matrix
	  	   _whellForces_earth[3],		// force on wheel in earth frame
	  	   _sinRotation, _cosRotation;	// sine and cosine of wheel rotation angle

	// housekeeping
	memcpy(_wheelPosition_body, pIn->cg2point, 3 * sizeof(double));
	memset(_omega, 0, 9 * sizeof(double));

	// earth to body transformation matrix (yaw is not needed)
	F_EULER_TO_DCM(_earth2body, pIn->THETA[_PHI], pIn->THETA[_THETA], 0.0);

	// body to earth transformation matrix
	M_MAT_TRANSPOSE(_body2earth, _earth2body);

	// construct Euler "omega cross" matrix
	_omega[_X][_Y] = -pIn->pqr[_Z];
	_omega[_X][_Z] =  pIn->pqr[_Y];
	_omega[_Y][_X] =  pIn->pqr[_Z];
	_omega[_Y][_Z] = -pIn->pqr[_X];
	_omega[_Z][_X] = -pIn->pqr[_Y];
	_omega[_Z][_Y] =  pIn->pqr[_X];

	// calculate wheel position on earth
	_wheelPosition_earth[_X] = _body2earth[_X][_X] * _wheelPosition_body[_X] +
		                       _body2earth[_X][_Y] * _wheelPosition_body[_Y] +
							   _body2earth[_X][_Z] * _wheelPosition_body[_Z];
	_wheelPosition_earth[_Y] = _body2earth[_Y][_X] * _wheelPosition_body[_X] +
						       _body2earth[_Y][_Y] * _wheelPosition_body[_Y] +
							   _body2earth[_Y][_Z] * _wheelPosition_body[_Z];
	_wheelPosition_earth[_Z] = _body2earth[_Z][_X] * _wheelPosition_body[_X] +
							   _body2earth[_Z][_Y] * _wheelPosition_body[_Y] +
							   _body2earth[_Z][_Z] * _wheelPosition_body[_Z];
	_wheelPosition_earth[_Z] += pIn->altitude;

	// calculate displacement
	_displacement = _wheelPosition_earth[_Z];

	if(_displacement > 0.0 ) {		// wheel is underground
		// demand wheel position to be on ground
		_wheelPosition_earth[_Z] = 0.0;

		// transform position demand to body coordinate system
		_wheelPosition_body[_X] = _earth2body[_X][_X] * _wheelPosition_earth[_X] +
								  _earth2body[_X][_Y] * _wheelPosition_earth[_Y] +
								  _earth2body[_X][_Z] * _wheelPosition_earth[_Z];
		_wheelPosition_body[_Y] = _earth2body[_Y][_X] * _wheelPosition_earth[_X] +
								  _earth2body[_Y][_Y] * _wheelPosition_earth[_Y] +
								  _earth2body[_Y][_Z] * _wheelPosition_earth[_Z];
		_wheelPosition_body[_Z] = _earth2body[_Z][_X] * _wheelPosition_earth[_X] +
								  _earth2body[_Z][_Y] * _wheelPosition_earth[_Y] +
								  _earth2body[_Z][_Z] * _wheelPosition_earth[_Z];

		// calculate wheel velocity (body coordinate system)
		_vectorTemp1[_X] = _omega[_X][_X] * _wheelPosition_body[_X] +
						   _omega[_X][_Y] * _wheelPosition_body[_Y] +
						   _omega[_X][_Z] * _wheelPosition_body[_Z];
		_vectorTemp1[_Y] = _omega[_Y][_X] * _wheelPosition_body[_X] +
						   _omega[_Y][_Y] * _wheelPosition_body[_Y] +
						   _omega[_Y][_Z] * _wheelPosition_body[_Z];
		_vectorTemp1[_Z] = _omega[_Z][_X] * _wheelPosition_body[_X] +
						   _omega[_Z][_Y] * _wheelPosition_body[_Y] +
						   _omega[_Z][_Z] * _wheelPosition_body[_Z];

		_wheelVelocity_body[_X] = _vectorTemp1[_X] + pIn->uvw[_X];
		_wheelVelocity_body[_Y] = _vectorTemp1[_Y] + pIn->uvw[_Y];
		_wheelVelocity_body[_Z] = _vectorTemp1[_Z] + pIn->uvw[_Z];

		// transform wheel velocity to earth coordinate system
		_wheelVelocity_earth[_X] = _body2earth[_X][_X] * _wheelVelocity_body[_X] +
			                       _body2earth[_X][_Y] * _wheelVelocity_body[_Y] +
								   _body2earth[_X][_Z] * _wheelVelocity_body[_Z];
		_wheelVelocity_earth[_Y] = _body2earth[_Y][_X] * _wheelVelocity_body[_X] +
								   _body2earth[_Y][_Y] * _wheelVelocity_body[_Y] +
								   _body2earth[_Y][_Z] * _wheelVelocity_body[_Z];
		_wheelVelocity_earth[_Z] = _body2earth[_Z][_X] * _wheelVelocity_body[_X] +
								   _body2earth[_Z][_Y] * _wheelVelocity_body[_Y] +
								   _body2earth[_Z][_Z] * _wheelVelocity_body[_Z];

		// if the wheel has steering ability
		if(pIn->rotation != 0.0 ) {
			// rotate the body velocity by the rotation of the wheel (+ rotation about Z body axis)
			_sinRotation = sin(pIn->rotation);
			_cosRotation = cos(pIn->rotation);
			_vectorTemp1[_X] =  pIn->uvw[_X] * _cosRotation + pIn->uvw[_Y] * _sinRotation;
			_vectorTemp1[_Y] = -pIn->uvw[_X] * _sinRotation + pIn->uvw[_Y] * _cosRotation;
			_vectorTemp1[_Z] =  pIn->uvw[_Z];

			// transform velocity to body coordinate system
			_vectorTemp2[_X] = _omega[_X][_X] * _wheelPosition_body[_X] +
							   _omega[_X][_Y] * _wheelPosition_body[_Y] +
							   _omega[_X][_Z] * _wheelPosition_body[_Z];
			_vectorTemp2[_Y] = _omega[_Y][_X] * _wheelPosition_body[_X] +
							   _omega[_Y][_Y] * _wheelPosition_body[_Y] +
							   _omega[_Y][_Z] * _wheelPosition_body[_Z];
			_vectorTemp2[_Z] = _omega[_Z][_X] * _wheelPosition_body[_X] +
							   _omega[_Z][_Y] * _wheelPosition_body[_Y] +
							   _omega[_Z][_Z] * _wheelPosition_body[_Z];

			_wheelVelocity_body[_X] = _vectorTemp1[_X] + _vectorTemp2[_X];
			_wheelVelocity_body[_Y] = _vectorTemp1[_Y] + _vectorTemp2[_Y];
			_wheelVelocity_body[_Z] = _vectorTemp1[_Z] + _vectorTemp2[_Z];

			// transform velocity to earth coordinate system
			_wheelVelocity_earth[_X] = _body2earth[_X][_X] * _wheelVelocity_body[_X] +
									   _body2earth[_X][_Y] * _wheelVelocity_body[_Y] +
									   _body2earth[_X][_Z] * _wheelVelocity_body[_Z];
			_wheelVelocity_earth[_Y] = _body2earth[_Y][_X] * _wheelVelocity_body[_X] +
									   _body2earth[_Y][_Y] * _wheelVelocity_body[_Y] +
									   _body2earth[_Y][_Z] * _wheelVelocity_body[_Z];
			_wheelVelocity_earth[_Z] = _body2earth[_Z][_X] * _wheelVelocity_body[_X] +
									   _body2earth[_Z][_Y] * _wheelVelocity_body[_Y] +
									   _body2earth[_Z][_Z] * _wheelVelocity_body[_Z];
		}

		// vertical touch force (earth coordinate system)
		_whellForces_earth[_Z] = pIn->k * _displacement + pIn->b * _wheelVelocity_earth[_Z];

		// lateral touch forces on wheel (earth coordinate system) - X
		if(_wheelVelocity_earth[_X] > 0.0) {
			_whellForces_earth[_X] = -pIn->mu_x * _whellForces_earth[_Z] *_wheelVelocity_earth[_X] / M_ABS(_wheelVelocity_earth[_X]);
		} else {
			_whellForces_earth[_X] = 0.0;
		}

		// lateral touch forces on wheel (earth coordinate system) - Y
		if(_wheelVelocity_earth[_Y] > 0.0) {
			_whellForces_earth[_Y] = -pIn->mu_y * _whellForces_earth[_Z] * _wheelVelocity_earth[_Y] / M_ABS(_wheelVelocity_earth[_Y]);
		} else {
			_whellForces_earth[_Y] = 0.0;
		}

		// vertical force sign
		_whellForces_earth[_Z] *= -1.0;

		// transform forces to body coordinate system
		pOut->F[_X] = _earth2body[_X][_X] * _whellForces_earth[_X] +
					  _earth2body[_X][_Y] * _whellForces_earth[_Y] +
					  _earth2body[_X][_Z] * _whellForces_earth[_Z];
		pOut->F[_Y] = _earth2body[_Y][_X] * _whellForces_earth[_X] +
					  _earth2body[_Y][_Y] * _whellForces_earth[_Y] +
					  _earth2body[_Y][_Z] * _whellForces_earth[_Z];
		pOut->F[_Z] = _earth2body[_Z][_X] * _whellForces_earth[_X] +
					  _earth2body[_Z][_Y] * _whellForces_earth[_Y] +
					  _earth2body[_Z][_Z] * _whellForces_earth[_Z];

		// calculate wheel moments (cross of position and force) (earth coordinate system)
		_vectorTemp2[_X] = _whellForces_earth[_Z] * _wheelPosition_earth[_Y] - _whellForces_earth[_Y] * _wheelPosition_earth[_Z];
		_vectorTemp2[_Y] = _whellForces_earth[_X] * _wheelPosition_earth[_Z] - _whellForces_earth[_Z] * _wheelPosition_earth[_X];
		_vectorTemp2[_Z] = _whellForces_earth[_Y] * _wheelPosition_earth[_X] - _whellForces_earth[_X] * _wheelPosition_earth[_Y];

		// transform moments to earth coordinate system
		pOut->M[_X] = _earth2body[_X][_X] * _vectorTemp2[_X] +
			          _earth2body[_X][_Y] * _vectorTemp2[_Y] +
					  _earth2body[_X][_Z] * _vectorTemp2[_Z];
		pOut->M[_Y] = _earth2body[_Y][_X] * _vectorTemp2[_X] +
			          _earth2body[_Y][_Y] * _vectorTemp2[_Y] +
					  _earth2body[_Y][_Z] * _vectorTemp2[_Z];
		pOut->M[_Z] = _earth2body[_Z][_X] * _vectorTemp2[_X] +
			          _earth2body[_Z][_Y] * _vectorTemp2[_Y] +
					  _earth2body[_Z][_Z] * _vectorTemp2[_Z];
	} else {  // wheel in on air
		memset(pOut->F, 0, 3 * sizeof(double));
		memset(pOut->M, 0, 3 * sizeof(double));
	}
}
示例#3
0
int euler213(double angle[3],   /* In:  r  Method=0, 0=PITCH , 1=ROLL , 2=YAW */
             double mat[3][3],  /* Out: r  Method=0, 
                                           Coordinate tranformation matrix   */
             int method,        /* In: 0 = Make matrix from angles,
                                       1 = Make angles from matrix,
                                       2 = Make angles from matrix but use 
                                           previous values to prevent 
                                           singularities */
             double *prev,      /* In: r   prev[3], Previous values of 
                                           euler angles */
             char *file,        /* In: file_name of caller of this function */
             int lineno)
{                               /* In: line # of call to this function in fname */

        double s1;              /* SINE OF PITCH */
        double c1;              /* COSINE OF PITCH */
        double s2;              /* SINE OF ROLL */
        double c2;              /* COSINE OF ROLL */
        double s3;              /* SINE OF YAW */
        double c3;              /* COSINE OF YAW */
        double tmp;
        int ret = 0;
        static unsigned short error_flag[5] = {0, 0, 0, 0, 0}; /* Send errors only once */

        (void)file ; /* unused */
        (void)lineno ; /* unused */

        if (method == 0) {

                /* Compute sines and cosines of pitch, roll, yaw */
                s1 = sin(angle[0]);
                c1 = cos(angle[0]);
                s2 = sin(angle[1]);
                c2 = cos(angle[1]);
                s3 = sin(angle[2]);
                c3 = cos(angle[2]);

                /* Compute values for matrix "mat" */
                mat[0][0] = c1 * c3 + s1 * s2 * s3;
                mat[1][0] = -c1 * s3 + s1 * s2 * c3;
                mat[2][0] = s1 * c2;
                mat[0][1] = c2 * s3;
                mat[1][1] = c2 * c3;
                mat[2][1] = -s2;
                mat[0][2] = -s1 * c3 + c1 * s2 * s3;
                mat[1][2] = s1 * s3 + c1 * s2 * c3;
                mat[2][2] = c1 * c2;

        } else if (method == 1) {

                /* Within normal range for asin function */
                if (-1.0 <= -mat[2][1] && -mat[2][1] <= 1.0) {
                        angle[1] = asin(-mat[2][1]);
                        if (M_ABS(angle[1] - M_PI_2) < 1.0e-6) {
                                angle[0] = atan2(mat[1][0], mat[0][0]);
                                angle[1] = M_PI_2;
                                angle[2] = 0.0;
                                ret = TM_SING_213_P;
                                if ( error_flag[0] == 0 ) {
                                    tm_print_error(ret);
                                    error_flag[0]=1;
                                }
                        } else if (M_ABS(angle[1] + M_PI_2) < 1.0e-6) {
                                angle[0] = atan2(-mat[1][0], mat[0][0]);
                                angle[1] = -M_PI_2;
                                angle[2] = 0.0;
                                ret = TM_SING_213_N;
                                if ( error_flag[1] == 0 ) {
                                    tm_print_error(ret);
                                    error_flag[1]=1;
                                }
                        } else {
                                angle[0] = atan2(mat[2][0], mat[2][2]);
                                angle[2] = atan2(mat[0][1], mat[1][1]);
                        }
                }

                /* Out of normal range for asin func, but within tolerance */
#define TOLERANCE 1.0e-15
                else if (1.0 < -mat[2][1] && -mat[2][1] <= (1.0 + TOLERANCE)) {
                        angle[0] = atan2(mat[1][0], mat[0][0]);
                        angle[1] = M_PI_2;
                        angle[2] = 0.0;
                        ret = TM_SING_213_P;
                        if ( error_flag[2] == 0 ) {
                            tm_print_error(ret);
                            error_flag[2]=1;
                        }
                } else if ((-1.0 - TOLERANCE) <= -mat[2][1]
                           && -mat[2][1] < -1.0) {
                        angle[0] = atan2(-mat[1][0], mat[0][0]);
                        angle[1] = -M_PI_2;
                        angle[2] = 0.0;
                        ret = TM_SING_213_N;
                        if ( error_flag[3] == 0 ) {
                            tm_print_error(ret);
                            error_flag[3]=1;
                        }
                }
                /* Error: Out of normal range & beyond tolerance 
                   for asin func */
                else {
                        double zero = 0.0;
                        ret = TM_ANG_NAN;
                        if ( error_flag[4] == 0 ) {
                            tm_print_error(ret);
                            error_flag[4]=1;
                        }
                        angle[0] = angle[1] = angle[2] = 0.0 / zero;
                }
#undef TOLERANCE
        } else if (method == 2) {

#define TOLERANCE 0.0314159265358979    /* 1.8 degree tolerance */
                /* Compute euler angles from tranformation */
                if (M_ABS(mat[2][1] + 1.0) < 1.0e-6) {
                        angle[0] = atan2(mat[1][0], mat[0][0]) + prev[2];
                        angle[1] = M_PI_2;
                        angle[2] = prev[2];
                        tmp = angle[0] - prev[0];
                        if (M_ABS(tmp - 2.0 * M_PI) < TOLERANCE)
                                angle[0] -= 2.0 * M_PI;
                        else if (M_ABS(tmp + 2.0 * M_PI) < TOLERANCE)
                                angle[0] += 2.0 * M_PI;

                } else if (M_ABS(mat[2][1] - 1.0) < 1.0e-6) {
                        angle[0] = atan2(-mat[1][0], mat[0][0]) - prev[2];
                        angle[1] = -M_PI_2;
                        angle[2] = prev[2];
                        tmp = angle[0] - prev[0];
                        if (M_ABS(tmp - 2.0 * M_PI) < TOLERANCE)
                                angle[0] -= 2.0 * M_PI;
                        else if (M_ABS(tmp + 2.0 * M_PI) < TOLERANCE)
                                angle[0] += 2.0 * M_PI;
                } else {
                        angle[0] = atan2(mat[2][0], mat[2][2]);
                        angle[1] = asin(-mat[2][1]);
                        angle[2] = atan2(mat[0][1], mat[1][1]);
                }
#undef TOLERANCE
        }

        return (ret);
}