ExternalVendorCode/trick
1
0
Fork 0
mirror of https://github.com/nasa/trick.git synced 2025-02-20 17:22:52 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #536 from abrogley/determinate-determinant

Browse Source 
Fixing typos in trick_math
This commit is contained in:
Alex Lin 2018-01-08 10:40:48 -06:00 committed by GitHub
commit 6ca81bc50d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
9 changed files with 82 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
trick_source/trick_utils/math/src/deuler_123_quat.c
View File

@ -1,6 +1,6 @@
/*
PURPOSE:
(Generate a LELF_HANDED quaternion using an Euler ROLL_PITCH_YAW sequence
(Generate a LEFT_HANDED quaternion using an Euler ROLL_PITCH_YAW sequence
OR generate an Euler ROLL_PITCH_YAW sequence using a quaternion.)
PROGRAMMERS:
@ -19,7 +19,7 @@ int euler123_quat(
double *prev) /* In: r Previous values of euler angles. */
{
double haft_angle[3];
double half_angle[3];
double mat00, mat01, mat10, mat11, mat20, mat21, mat22;
double s1;
double c1;
@ -34,13 +34,13 @@ int euler123_quat(
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]);
V_SCALE(half_angle, angle, 0.5);
s1 = sin(half_angle[0]);
c1 = cos(half_angle[0]);
s2 = sin(half_angle[1]);
c2 = cos(half_angle[1]);
s3 = sin(half_angle[2]);
c3 = cos(half_angle[2]);
quat[0] = c1*c2*c3 - s1*s2*s3;
quat[1] = -c1*s2*s3 - s1*c2*c3;

18
trick_source/trick_utils/math/src/deuler_132_quat.c
View File

@ -1,7 +1,7 @@
/*
PURPOSE:
(Generate a LELF_HANDED quaternion using an Euler ROLL_YAW_PITCH sequence
(Generate a LEFT_HANDED quaternion using an Euler ROLL_YAW_PITCH sequence
OR generate an Euler ROLL_YAW_PITCH sequence using a quaternion.)
PROGRAMMERS:
@ -20,7 +20,7 @@ int euler132_quat(
double *prev) /* In: r Previous values of euler angles. */
{
double haft_angle[3];
double half_angle[3];
double mat00, mat01, mat02, mat10, mat11, mat12, mat20;
double s1;
double c1;
@ -34,13 +34,13 @@ int euler132_quat(
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]);
V_SCALE(half_angle, angle, 0.5);
s1 = sin(half_angle[0]);
c1 = cos(half_angle[0]);
s2 = sin(half_angle[1]);
c2 = cos(half_angle[1]);
s3 = sin(half_angle[2]);
c3 = cos(half_angle[2]);
quat[0] = c1*c2*c3 + s1*s2*s3;
quat[1] = -s1*c2*c3 + c1*s2*s3;

18
trick_source/trick_utils/math/src/deuler_213_quat.c
View File

@ -1,6 +1,6 @@
/*
PURPOSE:
(Generate a LELF_HANDED quaternion using an Euler PITCH_ROLL_YAW sequence
(Generate a LEFT_HANDED quaternion using an Euler PITCH_ROLL_YAW sequence
OR generate an Euler PITCH_ROLL_YAW sequence using a quaternion.)
PROGRAMMERS:
@ -19,7 +19,7 @@ int euler213_quat(
double *prev) /* In: r Previous values of euler angles. */
{
double haft_angle[3];
double half_angle[3];
double mat00, mat01, mat10, mat11, mat20, mat21, mat22;
double s1;
double c1;
@ -33,13 +33,13 @@ int euler213_quat(
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]);
V_SCALE(half_angle, angle, 0.5);
s1 = sin(half_angle[0]);
c1 = cos(half_angle[0]);
s2 = sin(half_angle[1]);
c2 = cos(half_angle[1]);
s3 = sin(half_angle[2]);
c3 = cos(half_angle[2]);
quat[0] = c1*c2*c3 + s1*s2*s3;
quat[1] = -c1*s2*c3 - s1*c2*s3;

18
trick_source/trick_utils/math/src/deuler_231_quat.c
View File

@ -1,6 +1,6 @@
/*
PURPOSE:
(Generate a LELF_HANDED quaternion using an Euler PITCH_YAW_ROLL sequence
(Generate a LEFT_HANDED quaternion using an Euler PITCH_YAW_ROLL sequence
OR generate an Euler PITCH_YAW_ROLL sequence using a quaternion.)
PROGRAMMERS:
@ -19,7 +19,7 @@ int euler231_quat(
double *prev) /* In: r Previous values of euler angles. */
{
double haft_angle[3];
double half_angle[3];
double mat00, mat01, mat02, mat11, mat20, mat21, mat22;
double s1;
double c1;
@ -33,13 +33,13 @@ int euler231_quat(
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]);
V_SCALE(half_angle, angle, 0.5);
s1 = sin(half_angle[0]);
c1 = cos(half_angle[0]);
s2 = sin(half_angle[1]);
c2 = cos(half_angle[1]);
s3 = sin(half_angle[2]);
c3 = cos(half_angle[2]);
quat[0] = c1*c2*c3 - s1*s2*s3;
quat[1] = -c1*c2*s3 - s1*s2*c3;

18
trick_source/trick_utils/math/src/deuler_312_quat.c
View File

@ -1,6 +1,6 @@
/*
PURPOSE:
(Generate a LELF_HANDED quaternion using an Euler YAW_ROLL_PITCH sequence
(Generate a LEFT_HANDED quaternion using an Euler YAW_ROLL_PITCH sequence
OR generate an Euler YAW_ROLL_PITCH sequence using a quaternion.)
PROGRAMMERS:
@ -19,7 +19,7 @@ int euler312_quat(
double *prev) /* In: r Previous values of euler angles. */
{
double haft_angle[3];
double half_angle[3];
double mat00, mat01, mat02, mat10, mat11, mat12, mat22;
double s1;
double c1;
@ -34,13 +34,13 @@ int euler312_quat(
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]);
V_SCALE(half_angle, angle, 0.5);
s1 = sin(half_angle[0]);
c1 = cos(half_angle[0]);
s2 = sin(half_angle[1]);
c2 = cos(half_angle[1]);
s3 = sin(half_angle[2]);
c3 = cos(half_angle[2]);
quat[0] = c1*c2*c3 - s1*s2*s3;
quat[1] = -c1*s2*c3 + s1*c2*s3;

18
trick_source/trick_utils/math/src/deuler_321_quat.c
View File

@ -1,6 +1,6 @@
/*
PURPOSE:
(Generate a LELF_HANDED quaternion using an Euler YAW_PITCH_ROLL sequence
(Generate a LEFT_HANDED quaternion using an Euler YAW_PITCH_ROLL sequence
OR generate an Euler YAW_PITCH_ROLL sequence using a quaternion.)
PROGRAMMERS:
@ -19,7 +19,7 @@ int euler321_quat(
double *prev) /* In: r Previous values of euler angles. */
{
double haft_angle[3];
double half_angle[3];
double mat00, mat01, mat02, mat12, mat20, mat21, mat22;
double s1;
double c1;
@ -34,13 +34,13 @@ int euler321_quat(
if (method == 0){
/* Compute sines and cosines of 0.5*yaw, .5*pitch, and .5*roll */
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]);
V_SCALE(half_angle, angle, 0.5);
s1 = sin(half_angle[0]);
c1 = cos(half_angle[0]);
s2 = sin(half_angle[1]);
c2 = cos(half_angle[1]);
s3 = sin(half_angle[2]);
c3 = cos(half_angle[2]);
quat[0] = c3*c2*c1 + s3*s2*s1;
quat[1] = -s3*c2*c1 + c3*s2*s1;

30
trick_source/trick_utils/math/src/dm_invert.c
View File

@ -1,7 +1,7 @@
/*
PURPOSE: (Invert a 3X3 matrix.)
ASSUMPTIONS AND LIMITATIONS: ((Determinate is non-zero))
ASSUMPTIONS AND LIMITATIONS: ((Determinant is non-zero))
PROGRAMMERS: (((Robert W. Bailey) (LinCom Corporation) (4/1/91) (Trick-CR-00004) (Gravity / Inertial Frame Models))) */
@ -10,30 +10,30 @@
int dm_invert(double inv[3][3], /* Out: The 3X3 inverse of matrix 'm' */
double m[3][3])
{ /* In: A 3X3 matrix */
double determinate; /* The determinate of the input 'm' matrix */
double determinant; /* The determinant of the input 'm' matrix */
/* Calculate the determinate and test it */
/* Calculate the determinant and test it */
/* (Save the intermediate calculations in 'inv' for later use) */
inv[0][0] = m[1][1] * m[2][2] - m[2][1] * m[1][2];
inv[0][1] = m[2][1] * m[0][2] - m[0][1] * m[2][2];
inv[0][2] = m[0][1] * m[1][2] - m[1][1] * m[0][2];
determinate = m[0][0] * inv[0][0] + m[1][0] * inv[0][1] + m[2][0] * inv[0][2];
if (determinate == 0.0) {
fprintf(stderr, "trick_utils/math/dm_invert.c: 3x3 matrix has zero determinate\n");
determinant = m[0][0] * inv[0][0] + m[1][0] * inv[0][1] + m[2][0] * inv[0][2];
if (determinant == 0.0) {
fprintf(stderr, "trick_utils/math/dm_invert.c: 3x3 matrix has zero determinant\n");
return (TM_ZERO_DET);
}
/* Calculate the inverse matrix */
inv[0][0] /= determinate;
inv[0][1] /= determinate;
inv[0][2] /= determinate;
inv[1][0] = (m[2][0] * m[1][2] - m[1][0] * m[2][2]) / determinate;
inv[1][1] = (m[0][0] * m[2][2] - m[2][0] * m[0][2]) / determinate;
inv[1][2] = (m[1][0] * m[0][2] - m[0][0] * m[1][2]) / determinate;
inv[2][0] = (m[1][0] * m[2][1] - m[2][0] * m[1][1]) / determinate;
inv[2][1] = (m[2][0] * m[0][1] - m[0][0] * m[2][1]) / determinate;
inv[2][2] = (m[0][0] * m[1][1] - m[1][0] * m[0][1]) / determinate;
inv[0][0] /= determinant;
inv[0][1] /= determinant;
inv[0][2] /= determinant;
inv[1][0] = (m[2][0] * m[1][2] - m[1][0] * m[2][2]) / determinant;
inv[1][1] = (m[0][0] * m[2][2] - m[2][0] * m[0][2]) / determinant;
inv[1][2] = (m[1][0] * m[0][2] - m[0][0] * m[1][2]) / determinant;
inv[2][0] = (m[1][0] * m[2][1] - m[2][0] * m[1][1]) / determinant;
inv[2][1] = (m[2][0] * m[0][1] - m[0][0] * m[2][1]) / determinant;
inv[2][2] = (m[0][0] * m[1][1] - m[1][0] * m[0][1]) / determinant;
return (TM_SUCCESS);
}

24
trick_source/trick_utils/math/src/dm_invert_symm.c
View File

@ -1,7 +1,7 @@
/*
PURPOSE: (Invert a symmetric 3X3 matrix.)
ASSUMPTIONS AND LIMITATIONS: ((Matrix is symmetric) (Determinate is non-zero))
ASSUMPTIONS AND LIMITATIONS: ((Matrix is symmetric) (Determinant is non-zero))
PROGRAMMERS: (((Robert W. Bailey) (LinCom Corporation) (4/1/91) (Trick-CR-00004) (Gravity / Inertial Frame Models))) */
@ -11,30 +11,30 @@ int dm_invert_symm(double inv[3][3], /* Out: The 3X3 inverse of matrix 'm' */
double m[3][3])
{ /* In: A 3X3 matrix */
double determinate; /* The determinate of 'm' */
double determinant; /* The determinant of 'm' */
/* Calculate the determinate and test it */
/* Calculate the determinant and test it */
inv[0][0] = m[1][1] * m[2][2] - m[2][1] * m[1][2];
inv[0][1] = m[2][1] * m[0][2] - m[0][1] * m[2][2];
inv[0][2] = m[0][1] * m[1][2] - m[1][1] * m[0][2];
determinate = m[0][0] * inv[0][0] + m[1][0] * inv[0][1] + m[2][0] * inv[0][2];
determinant = m[0][0] * inv[0][0] + m[1][0] * inv[0][1] + m[2][0] * inv[0][2];
if (determinate == 0.0) {
fprintf(stderr, "trick_utils/math/dm_invert_symm.c: Tried to invert " "a 3x3 matrix with a zero determinate.");
if (determinant == 0.0) {
fprintf(stderr, "trick_utils/math/dm_invert_symm.c: Tried to invert " "a 3x3 matrix with a zero determinant.");
return (TM_ZERO_DET);
}
/* Calculate the inverse matrix, and return '1' */
inv[0][0] /= determinate;
inv[0][1] /= determinate;
inv[0][2] /= determinate;
inv[0][0] /= determinant;
inv[0][1] /= determinant;
inv[0][2] /= determinant;
inv[1][0] = inv[0][1];
inv[1][1] = (m[0][0] * m[2][2] - m[2][0] * m[0][2]) / determinate;
inv[1][2] = (m[1][0] * m[0][2] - m[0][0] * m[1][2]) / determinate;
inv[1][1] = (m[0][0] * m[2][2] - m[2][0] * m[0][2]) / determinant;
inv[1][2] = (m[1][0] * m[0][2] - m[0][0] * m[1][2]) / determinant;
inv[2][0] = inv[0][2];
inv[2][1] = inv[1][2];
inv[2][2] = (m[0][0] * m[1][1] - m[1][0] * m[0][1]) / determinate;
inv[2][2] = (m[0][0] * m[1][1] - m[1][0] * m[0][1]) / determinant;
return (TM_SUCCESS);
}

2
trick_source/trick_utils/math/src/tm_print_error.c
View File

@ -23,7 +23,7 @@ static char *trick_math_errors[] = {
/* 13 */ "WARNING: Passed value is beyond the range of the asin function, Euler angles set to NANs",
/* 14 */ "ERROR: Invalid Euler Rotation Sequence Option",
/* 15 */ "ERROR: Diagonal Element is too small",
/* 16 */ "ERROR: 3x3 matrix has zero determinate"
/* 16 */ "ERROR: 3x3 matrix has zero determinant"
};
/* ENTRY POINT: */