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); }
/** * 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)); } }
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); }