ExternalVendorCode/trick
1
0
Fork 0
mirror of https://github.com/nasa/trick.git synced 2025-02-27 03:07:13 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity
trick/trick_source/trick_utils/math/src/deuler_231.c

166 lines
7.1 KiB
C
Raw Normal View History

Initial commit of everything.
2015-02-26 09:02:31 -06:00
/*
PURPOSE:
(Generate a transformation matrix using an Euler PITCH-YAW-ROLL
sequence OR generate an Euler PITCH-YAW-ROLL sequence using an
coordinate transformation matrix.)
PROGRAMMERS:
(((Robert W. Bailey) (LinCom Corp) (2/1/91))
((An Huynh) (LinCom Corp) (June 97) (Singularity Handling))
((Robert Gay) (Titan-LinCom) (July 2001) (Added tolerance for
asin function)))
*/
/* FUNCTION: A transformation matrix is generated using the input
PITCH-YAW-ROLL euler angles (in radians). The angles
are input as:
angle[0] = PITCH - M_PI <= PITCH < M_PI
angle[1] = YAW - M_PI/2 <= YAW < M_PI/2
angle[2] = ROLL - M_PI <= ROLL < M_PI
*/
#include <stdio.h>
Standardize directory names Reorganized. Created a new top level include directory that will hold all of Trick's header files. Moved all of the Trick headers to this directory. Created a libexec directory that holds all of the executables that users don't need to execute directly. Changed all of the executables remaining in bin to start with "trick-". In the sim_services directories changed all source files to find the Trick headers in their new location. Since all of the include files are gone in sim_services, removed the src directories as well, moving all of the source files up a level. Moved the makefiles, docs, man, and other architecture independent files into a top level share directory. Renamed lib_${TRICK_HOST_CPU} to lib64 or lib depending on the platform we're currently on. refs #63
2015-06-01 10:28:29 -05:00
#include "trick/trick_math.h"
Initial commit of everything.
2015-02-26 09:02:31 -06:00
int euler231( /* Return: -- None. */
double angle[3], /* In: r Method=0, 0=PITCH, 1=YAW, 2=ROLL */
Remove trailing whitespaces Makes it easier to edit the files. So if we press 'end of line' we are really at the end of line.
2016-11-08 10:25:07 +01:00
double mat[3][3], /* Out: r Method=0,
Initial commit of everything.
2015-02-26 09:02:31 -06:00
Coordinate tranformation matrix */
int method, /* In: 0 = Make matrix from angles,
1 = Make angles from matrix,
Remove trailing whitespaces Makes it easier to edit the files. So if we press 'end of line' we are really at the end of line.
2016-11-08 10:25:07 +01:00
2 = Make angles from matrix but
use previous values to
Initial commit of everything.
2015-02-26 09:02:31 -06:00
prevent singularities */
Remove trailing whitespaces Makes it easier to edit the files. So if we press 'end of line' we are really at the end of line.
2016-11-08 10:25:07 +01:00
double *prev, /* In: r prev[3], Previous values of
Initial commit of everything.
2015-02-26 09:02:31 -06:00
euler angles */
Change file name from char* to const char* in euler tranformation matrix functions. Ref #423
2017-05-15 14:02:01 -05:00
const char *file, /* In: File_name of caller of this function */
Initial commit of everything.
2015-02-26 09:02:31 -06:00
int lineno) /* In: line # of call to this function in fname */
Cleaning up once include variables and copyright cleanup. Changed all header file once include variables to follow the same naming convention and not start with any underscores. Also deleted old incorrect copyright notices. Also removed $Id: tags from all files. Fixes #14. Fixes #22.
2015-03-23 16:03:14 -05:00
{
Initial commit of everything.
2015-02-26 09:02:31 -06:00
double s1; /* SINE OF PITCH */
double c1; /* COSINE OF PITCH */
double s2; /* SINE OF YAW */
double c2; /* COSINE OF YAW */
double s3; /* SINE OF ROLL */
double c3; /* COSINE OF ROLL */
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 yaw, pitch, and roll */
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 * c2;
mat[1][0] = -c1 * s2 * c3 + s1 * s3;
mat[2][0] = c1 * s2 * s3 + s1 * c3;
mat[0][1] = s2;
mat[1][1] = c2 * c3;
mat[2][1] = -c2 * s3;
mat[0][2] = -s1 * c2;
mat[1][2] = s1 * s2 * c3 + c1 * s3;
mat[2][2] = -s1 * s2 * s3 + c1 * c3;
} else if (method == 1) {
#define TOLERANCE 1.0e-15
/* Within normal range for asin function */
if (-1.0 <= mat[0][1] && mat[0][1] <= 1.0) {
angle[1] = asin(mat[0][1]);
if (M_ABS(angle[1] - M_PI_2) < 1.0e-6) {
angle[0] = atan2(mat[2][0], mat[2][2]);
angle[1] = M_PI_2;
angle[2] = 0.0;
ret = TM_SING_231_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[2][0], mat[2][2]);
angle[1] = -M_PI_2;
angle[2] = 0.0;
ret = TM_SING_231_N;
if ( error_flag[1] == 0 ) {
tm_print_error(ret);
error_flag[1]=1;
}
} else {
angle[0] = atan2(-mat[0][2], mat[0][0]);
angle[2] = atan2(-mat[2][1], mat[1][1]);
}
}
/* Out of normal range for asin func, but within tolerance */
else if (1.0 < mat[0][1] && mat[0][1] <= (1.0 + TOLERANCE)) {
angle[0] = atan2(mat[2][0], mat[2][2]);
angle[1] = M_PI_2;
angle[2] = 0.0;
ret = TM_SING_231_P;
if ( error_flag[2] == 0 ) {
tm_print_error(ret);
error_flag[2]=1;
}
} else if ((-1.0 - TOLERANCE) <= mat[0][1] && mat[0][1] < -1.0) {
angle[0] = atan2(mat[2][0], mat[2][2]);
angle[1] = -M_PI_2;
angle[2] = 0.0;
ret = TM_SING_231_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[0][1] - 1.0) < 1.0e-6) {
angle[0] = atan2(mat[2][0], mat[2][2]) - 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[0][1] + 1.0) < 1.0e-6) {
angle[0] = atan2(mat[2][0], mat[2][2]) + 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[0][2], mat[0][0]);
angle[1] = asin(mat[0][1]);
angle[2] = atan2(-mat[2][1], mat[1][1]);
}
#undef TOLERANCE
}
return (ret);
}
Reference in New Issue Copy Permalink