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