ExternalVendorCode/trick
1
0
Fork 0
mirror of https://github.com/nasa/trick.git synced 2024-12-19 21:27:54 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Added the conversion capabilities between Euler angles and quaternions. Refs:#216

Browse Source
This commit is contained in:
Hung Nguyen 2016-04-04 17:07:04 -05:00
parent 0b7e3e2dc9
commit 5de28a360c
16 changed files with 1794 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
include/trick/quat_macros.h
View File

@ -135,4 +135,18 @@ Assign the product of quaternian Q and vector V to quaternian D.
VxV_ADD ((dest+1), (quat+1), vec); \ VxV_ADD ((dest+1), (quat+1), vec); \
} }
/**
@page QUATERNIAN_MACROS
\b Q_dot(Qdot, Q, V)
Assign the time derivative of quaternian Q given an angular velocity V to Qdot.
*/
#define Q_dot(qdot,quat,omega) { \
double v[3];\
V_SCALE(v, omega , -0.5);\
*qdot = - V_DOT((quat+1), v);\
V_SCALE ((qdot+1), v, *quat);\
VxV_ADD ((qdot+1), v, (quat+1)); \
}
#endif /* _QUAT_MACROS_H_ DO NOT PUT ANYTHING AFTER THIS LINE */ #endif /* _QUAT_MACROS_H_ DO NOT PUT ANYTHING AFTER THIS LINE */

59
include/trick/trick_math_proto.h
View File

@ -113,6 +113,42 @@ int euler312(double angle[3], double mat[3][3], int method,
int euler321(double angle[3], double mat[3][3], int method, int euler321(double angle[3], double mat[3][3], int method,
double *prev, char *file, int lineno); double *prev, char *file, int lineno);
/**
* @ingroup TRICK_MATH
*/
int euler123_quat(double angle[3], double quat[4], int method,
double *prev);
/**
* @ingroup TRICK_MATH
*/
int euler132_quat(double angle[3], double quat[4], int method,
double *prev);
/**
* @ingroup TRICK_MATH
*/
int euler213_quat(double angle[3], double quat[4], int method,
double *prev);
/**
* @ingroup TRICK_MATH
*/
int euler231_quat(double angle[3], double quat[4], int method,
double *prev);
/**
* @ingroup TRICK_MATH
*/
int euler312_quat(double angle[3], double quat[4], int method,
double *prev);
/**
* @ingroup TRICK_MATH
*/
int euler321_quat(double angle[3], double quat[4], int method,
double *prev);
/** /**
* @ingroup TRICK_MATH * @ingroup TRICK_MATH
* Add matrix mat1 to matrix mat2, assigning the result to sum. * Add matrix mat1 to matrix mat2, assigning the result to sum.
@ -358,6 +394,15 @@ void eigen_ql(double *d, double *e, int n, double **z);
int euler_matrix(double angle[3], double mat[3][3], int method, int euler_matrix(double angle[3], double mat[3][3], int method,
Euler_Seq sequence); Euler_Seq sequence);
/**
* @ingroup TRICK_MATH
* Generate a quaternion matix using a mutually exculsive Euler
* angle sequence OR generate a mutually exclusive Euler angle sequence
* using a quaternion matrix.
*/
int euler_quat(double angle[3], double quat[4], int method,
Euler_Seq sequence);
/** /**
* @ingroup TRICK_MATH * @ingroup TRICK_MATH
* Matrix copy * Matrix copy
@ -526,4 +571,18 @@ double gauss_rnd_bell(RAND_GENERATOR * G);
euler312( ang, mat, method, prev, __FILE__, __LINE__ ) euler312( ang, mat, method, prev, __FILE__, __LINE__ )
#define euler_321( ang, mat, method, prev ) \ #define euler_321( ang, mat, method, prev ) \
euler321( ang, mat, method, prev, __FILE__, __LINE__ ) euler321( ang, mat, method, prev, __FILE__, __LINE__ )
#define deuler_123_quat( ang, quat, method ) \
euler123_quat( ang, quat, method, (double*)0 )
#define deuler_132_quat( ang, quat, method ) \
euler132_quat( ang, quat, method, (double*)0 )
#define deuler_213_quat( ang, quat, method ) \
euler213_quat( ang, quat, method, (double*)0 )
#define deuler_231_quat( ang, quat, method ) \
euler231_quat( ang, quat, method, (double*)0 )
#define deuler_312_quat( ang, quat, method ) \
euler312_quat( ang, quat, method, (double*)0 )
#define deuler_321_quat( ang, quat, method ) \
euler321_quat( ang, quat, method, (double*)0 )
#endif #endif

82
trick_source/trick_utils/math/Unittest/euler_quat_test.cpp Normal file
View File

@ -0,0 +1,82 @@
#include <iostream>
#include <stdlib.h>
#include <time.h>
#include "euler_quat_test.hh"
int main()
{
EulerQuatTest test;
int ii;
// ========= Testing deuler_*_quat.c ========================
// There are 2 capabilities for every deuler_*_quat.c function:
// 1. Convert form euler to left handed quaternion
// 2. Convert from left handed quaternion to euler: (2 methods)
// - Make angles from quaternion
// - Make angles from quaternion but use previous values to
// prevent singularities
// ============================================================
srand(time(NULL));
for (ii=0; ii<test.num_tests; ii++){
// Randomly input euler angles:
test.init_data.euler_angles[0] = (rand()%test.num_tests) * (2*M_PI/test.num_tests);
test.init_data.euler_angles[1] = (rand()%test.num_tests) * (2*M_PI/test.num_tests);
test.init_data.euler_angles[2] = (rand()%test.num_tests) * (2*M_PI/test.num_tests);
// Testing deuler_123_quat.c:
test.init_data.euler_sequence = Roll_Pitch_Yaw;
euler_matrix(test.init_data.euler_angles, test.mat, 0 , test.init_data.euler_sequence);
mat_to_quat(test.quat, test.mat);
test.test_euler_to_quat();
test.test_quat_to_euler(1);
test.test_quat_to_euler(2); // Tesing singularities
// Testing deuler_132_quat.c:
test.init_data.euler_sequence = Roll_Yaw_Pitch;
euler_matrix(test.init_data.euler_angles, test.mat, 0 , test.init_data.euler_sequence);
mat_to_quat(test.quat, test.mat);
test.test_euler_to_quat();
test.test_quat_to_euler(1);
test.test_quat_to_euler(2);
// Testing deuler_231_quat.c:
test.init_data.euler_sequence = Pitch_Yaw_Roll;
euler_matrix(test.init_data.euler_angles, test.mat, 0 , test.init_data.euler_sequence);
mat_to_quat(test.quat, test.mat);
test.test_euler_to_quat();
test.test_quat_to_euler(1);
test.test_quat_to_euler(2);
// Testing deuler_213_quat.c:
test.init_data.euler_sequence = Pitch_Roll_Yaw;
euler_matrix(test.init_data.euler_angles, test.mat, 0 , test.init_data.euler_sequence);
mat_to_quat(test.quat, test.mat);
test.test_euler_to_quat();
test.test_quat_to_euler(1);
test.test_quat_to_euler(2);
// Testing deuler_312_quat.c:
test.init_data.euler_sequence = Yaw_Roll_Pitch;
euler_matrix(test.init_data.euler_angles, test.mat, 0 , test.init_data.euler_sequence);
mat_to_quat(test.quat, test.mat);
test.test_euler_to_quat();
test.test_quat_to_euler(1);
test.test_quat_to_euler(2);
// Testing deuler_321_quat.c:
test.init_data.euler_sequence = Yaw_Pitch_Roll;
euler_matrix(test.init_data.euler_angles, test.mat, 0 , test.init_data.euler_sequence);
mat_to_quat(test.quat, test.mat);
test.test_euler_to_quat();
test.test_quat_to_euler(1);
test.test_quat_to_euler(2);
test.num_pass ++;
}
if (test.num_pass == test.num_tests){
printf(" \e[1;34m Tested all %d sets of Euler angles and they were all PASSED \n\e", test.num_tests);
printf("%c[%dm", 0x1B, 0);
}
return(0);
}

101
trick_source/trick_utils/math/Unittest/euler_quat_test.hh Normal file
View File

@ -0,0 +1,101 @@
#include <iostream>
#include "trick/trick_math.h"
#include "trick/reference_frame.h"
using namespace std;
class EulerQuatTest {
public:
TRANSFORM init_data;
bool pass;
int num_tests;
int num_pass;
int method;
double quat[4];
double mat[3][3];
EulerQuatTest() {
this->num_tests = 1080;
this->num_pass = 0;
this->pass = false;
}
~EulerQuatTest() {}
void compare_euler(double eul[3]){
double eul_error[3];
double mat_test[3][3];
double mat_error[3][3];
M_IDENT(mat_error);
M_IDENT(mat_test);
V_INIT(eul_error);
euler_matrix(eul, mat_test, 0 , this->init_data.euler_sequence);
MxMt(mat_error, mat_test, this->mat);
euler_matrix(eul_error, mat_error, 0 , this->init_data.euler_sequence);
if(V_MAG(eul_error) < 0.00001){
pass = true;
} else{
pass = false;
printf("EulerAngle = %lf %lf %lf\n",this->init_data.euler_angles[0],
this->init_data.euler_angles[1],
this->init_data.euler_angles[2]);
this->num_tests = 0;
}
return;
}
void compare_quat(double Q[4]){
double Q_error[4];
double mat_error[3][3];
double eul_error[3];
M_IDENT(mat_error);
V_INIT(eul_error);
QxQt(Q_error, Q, quat);
quat_to_mat(mat_error, Q_error);
euler_matrix(eul_error, mat_error, 0 , this->init_data.euler_sequence);
if(V_MAG(eul_error) < 0.00001){
this->pass = true;
} else{
this->pass = false;
}
return;
}
void test_euler_to_quat(){
double Q[4];
euler_quat(this->init_data.euler_angles, Q, 0, this->init_data.euler_sequence);
compare_quat( Q );
return;
}
void test_quat_to_euler(int test){
double eul[3];
this->method = test;
if(this->method == 1){
euler_quat(eul, this->quat, this->method, this->init_data.euler_sequence);
}
else {
switch (this->init_data.euler_sequence) {
case Roll_Pitch_Yaw:
euler123_quat( eul, this->quat, 2, this->init_data.euler_angles );
break;
case Roll_Yaw_Pitch:
euler132_quat( eul, this->quat, 2, this->init_data.euler_angles );
break;
case Pitch_Yaw_Roll:
euler231_quat( eul, this->quat, 2, this->init_data.euler_angles );
break;
case Pitch_Roll_Yaw:
euler213_quat( eul, this->quat, 2, this->init_data.euler_angles );
break;
case Yaw_Roll_Pitch:
euler312_quat( eul, this->quat, 2, this->init_data.euler_angles );
break;
case Yaw_Pitch_Roll:
euler321_quat( eul, this->quat, 2, this->init_data.euler_angles );
break;
}
}
compare_euler(eul);
return;
}
};

48
trick_source/trick_utils/math/Unittest/makefile Normal file
View File

@ -0,0 +1,48 @@
SHELL = /bin/sh
ifndef TRICK_HOST_CPU
TRICK_HOST_CPU := $(shell ${TRICK_HOME}/bin/trick-gte TRICK_HOST_CPU)
endif
ifndef TRICK_HOST_TYPE
TRICK_HOST_TYPE := $(shell ${TRICK_HOME}/bin/trick-gte TRICK_HOST_TYPE)
endif
CPP = g++
CFLAGS = -g -Wall -I${TRICK_HOME}/include
RM = /bin/rm -rf
LIBDIR = ${TRICK_HOME}/lib64
LIBS = -ltrick_math -lm
INCLUDES = -I${TRICK_HOME}/include
.cpp.o:
${CPP} ${CFLAGS} ${INCLUDES} -c $<
top:
@ if [ "${TRICK_HOME}" != "" ] ; then \
$(MAKE) -e all \
"CPP = `${TRICK_HOME}/bin/trick-gte TRICK_CPPC` "; \
else \
$(MAKE) all \
"TRICK_HOST_TYPE = `uname -s` " \
"TRICK_HOST_CPU = `uname -s` "; \
fi
all: euler_quat_test
euler_quat_test: euler_quat_test.o
${CPP} -o $@ euler_quat_test.o -L${LIBDIR} ${LIBS}
clean:
${RM} *.o
${RM} *~
${RM} *.exe
${RM} euler_quat_test
real_clean: clean
${RM} euler_quat_test

173
trick_source/trick_utils/math/src/deuler_123_quat.c Normal file
View File

@ -0,0 +1,173 @@
/*
PURPOSE:
(Generate a LELF_HANDED quaternion using an Euler ROLL_PITCH_YAW sequence
OR generate an Euler ROLL_PITCH_YAW sequence using a quaternion.)
PROGRAMMERS:
(((Hung Nguyen) (CACI) (03/23/2016)))
*/
#include "trick/trick_math.h"
int euler123_quat(
double angle[3], /* Inout: r Method=1, 0=ROLL, 1=PITCH, 2=YAW. */
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, mat10, mat11, mat20, mat21, 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*s3 - s1*c2*c3;
quat[2] = -c1*s2*c3 + s1*c2*s3;
quat[3] = -c1*c2*s3 - s1*s2*c3;
}
else if (method == 1){
#define TOLERANCE 1.0e-15
mat20 = 2. * (quat[1] * quat[3] - quat[0] * quat[2]);
/* Within normal range for asin function */
if (-1.0 <= mat20 && mat20 <= 1.0) {
angle[1] = asin(mat20);
if (M_ABS(angle[1] - M_PI_2) < 1.0e-6) {
mat01 = 2. * (quat[1] * quat[2] - quat[0] * quat[3]);
mat11 = 1. - 2. * (quat[3] * quat[3] + quat[1] * quat[1]);
angle[0] = atan2(mat01, mat11);
angle[1] = M_PI_2;
angle[2] = 0.0;
ret = TM_SING_123_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) {
mat01 = 2. * (quat[1] * quat[2] - quat[0] * quat[3]);
mat11 = 1. - 2. * (quat[3] * quat[3] + quat[1] * quat[1]);
angle[0] = atan2(-mat01, mat11);
angle[1] = -M_PI_2;
angle[2] = 0.0;
ret = TM_SING_123_N;
if ( error_flag[1] == 0 ) {
tm_print_error(ret);
error_flag[1]=1;
}
} else {
mat00 = 1. - 2. * (quat[2] * quat[2] + quat[3] * quat[3]);
mat10 = 2. * (quat[1] * quat[2] + quat[0] * quat[3]);
mat21 = 2. * (quat[2] * quat[3] + quat[0] * quat[1]);
mat22 = 1. - 2. * (quat[1] * quat[1] + quat[2] * quat[2]);
angle[0] = atan2(-mat21, mat22);
angle[2] = atan2(-mat10, mat00);
}
}
/* Out of normal range for asin function,
but within tolerance */
else if (1.0 < mat20 && mat20 <= (1.0 + TOLERANCE)) {
mat01 = 2. * (quat[1] * quat[2] - quat[0] * quat[3]);
mat11 = 1. - 2. * (quat[3] * quat[3] + quat[1] * quat[1]);
angle[0] = atan2(mat01, mat11);
angle[1] = M_PI_2;
angle[2] = 0.0;
ret = TM_SING_123_P;
if ( error_flag[2] == 0 ) {
tm_print_error(ret);
error_flag[2]=1;
}
} else if ((-1.0 - TOLERANCE) <= mat20 && mat20 < -1.0) {
mat01 = 2. * (quat[1] * quat[2] - quat[0] * quat[3]);
mat11 = 1. - 2. * (quat[3] * quat[3] + quat[1] * quat[1]);
angle[0] = atan2(-mat01, mat11);
angle[1] = -M_PI_2;
angle[2] = 0.0;
ret = TM_SING_123_N;
if ( error_flag[3] == 0 ) {
tm_print_error(ret);
error_flag[3]=1;
}
}
/* Error: Out of normal range and beyond tolerance
for asin function */
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 */
mat20 = 2. * (quat[1] * quat[3] - quat[0] * quat[2]);
/* Compute euler angles from tranformation */
if (M_ABS(mat20 - 1.0) < 1.0e-6) {
mat01 = 2. * (quat[1] * quat[2] - quat[0] * quat[3]);
mat11 = 1. - 2. * (quat[3] * quat[3] + quat[1] * quat[1]);
angle[0] = atan2(mat01, mat11) - 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(mat20 + 1.0) < 1.0e-6) {
mat01 = 2. * (quat[1] * quat[2] - quat[0] * quat[3]);
mat11 = 1. - 2. * (quat[3] * quat[3] + quat[1] * quat[1]);
angle[0] = atan2(-mat01, mat11) + 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 {
mat00 = 1. - 2. * (quat[2] * quat[2] + quat[3] * quat[3]);
mat10 = 2. * (quat[1] * quat[2] + quat[0] * quat[3]);
mat20 = 2. * (quat[1] * quat[3] - quat[0] * quat[2]);
mat21 = 2. * (quat[2] * quat[3] + quat[0] * quat[1]);
mat22 = 1. - 2. * (quat[1] * quat[1] + quat[2] * quat[2]);
angle[0] = atan2(-mat21, mat22);
angle[1] = asin(mat20);
angle[2] = atan2(-mat10, mat00);
}
#undef TOLERANCE
}
return(ret);
}

173
trick_source/trick_utils/math/src/deuler_132_quat.c Normal file
View File

@ -0,0 +1,173 @@
/*
PURPOSE:
(Generate a LELF_HANDED quaternion using an Euler ROLL_YAW_PITCH sequence
OR generate an Euler ROLL_YAW_PITCH sequence using a quaternion.)
PROGRAMMERS:
(((Hung Nguyen) (CACI) (03/23/2016)))
*/
#include "trick/trick_math.h"
int euler132_quat(
double angle[3], /* Inout: r Method=1, 0=ROLL, 1=YAW, 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, mat20;
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] = -s1*c2*c3 + c1*s2*s3;
quat[2] = -c1*c2*s3 + s1*s2*c3;
quat[3] = -c1*s2*c3 - s1*c2*s3;
}
else if (method == 1){
#define TOLERANCE 1.0e-15
mat10 = 2. * (quat[1] * quat[2] + quat[0] * quat[3]);
/* Within normal range for asin function */
if (-1.0 <= -mat10 && -mat10 <= 1.0) {
angle[1] = asin(-mat10);
if (M_ABS(angle[1] - M_PI_2) < 1.0e-6) {
mat01 = 2. * (quat[1] * quat[2] - quat[0] * quat[3]);
mat02 = 2. * (quat[1] * quat[3] + quat[0] * quat[2]);
angle[0] = atan2(mat02, mat01);
angle[1] = M_PI_2;
angle[2] = 0.0;
ret = TM_SING_132_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) {
mat01 = 2. * (quat[1] * quat[2] - quat[0] * quat[3]);
mat02 = 2. * (quat[1] * quat[3] + quat[0] * quat[2]);
angle[0] = atan2(-mat02, -mat01);
angle[1] = -M_PI_2;
angle[2] = 0.0;
ret = TM_SING_132_N;
if ( error_flag[1] == 0 ) {
tm_print_error(ret);
error_flag[1]=1;
}
} else {
mat00 = 1. - 2. * (quat[2] * quat[2] + quat[3] * quat[3]);
mat11 = 1. - 2. * (quat[3] * quat[3] + quat[1] * quat[1]);
mat12 = 2. * (quat[2] * quat[3] - quat[0] * quat[1]);
mat20 = 2. * (quat[1] * quat[3] - quat[0] * quat[2]);
angle[0] = atan2(mat12, mat11);
angle[2] = atan2(mat20, mat00);
}
}
/* Out of normal range for asin function,
* but within tolerance
*/
else if (1.0 < -mat10 && -mat10 <= (1.0 + TOLERANCE)) {
mat01 = 2. * (quat[1] * quat[2] - quat[0] * quat[3]);
mat02 = 2. * (quat[1] * quat[3] + quat[0] * quat[2]);
angle[1] = M_PI_2;
angle[0] = atan2(mat02, mat01);
angle[2] = 0.0;
ret = TM_SING_132_P;
if ( error_flag[2] == 0 ) {
tm_print_error(ret);
error_flag[2]=1;
}
} else if ((-1.0 - TOLERANCE) <= -mat10 && -mat10 < -1.0) {
mat01 = 2. * (quat[1] * quat[2] - quat[0] * quat[3]);
mat02 = 2. * (quat[1] * quat[3] + quat[0] * quat[2]);
angle[0] = atan2(-mat02, -mat01);
angle[1] = -M_PI_2;
angle[2] = 0.0;
ret = TM_SING_132_N;
if ( error_flag[3] == 0 ) {
tm_print_error(ret);
error_flag[3]=1;
}
}
/* Error: Out of normal range and beyond tolerance
* for asin function
*/
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 */
mat10 = 2. * (quat[1] * quat[2] + quat[0] * quat[3]);
/* Compute euler angles from tranformation */
if (M_ABS(mat10 + 1.0) < 1.0e-6) {
mat01 = 2. * (quat[1] * quat[2] - quat[0] * quat[3]);
mat02 = 2. * (quat[1] * quat[3] + quat[0] * quat[2]);
angle[0] = atan2(mat02, mat01) + 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(mat10 - 1.0) < 1.0e-6) {
mat01 = 2. * (quat[1] * quat[2] - quat[0] * quat[3]);
mat02 = 2. * (quat[1] * quat[3] + quat[0] * quat[2]);
angle[0] = atan2(-mat02, -mat01) - 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 {
mat00 = 1. - 2. * (quat[2] * quat[2] + quat[3] * quat[3]);
mat10 = 2. * (quat[1] * quat[2] + quat[0] * quat[3]);
mat11 = 1. - 2. * (quat[3] * quat[3] + quat[1] * quat[1]);
mat12 = 2. * (quat[2] * quat[3] - quat[0] * quat[1]);
mat20 = 2. * (quat[1] * quat[3] - quat[0] * quat[2]);
angle[0] = atan2(mat12, mat11);
angle[1] = asin(-mat10);
angle[2] = atan2(mat20, mat00);
}
#undef TOLERANCE
}
return(ret);
}

169
trick_source/trick_utils/math/src/deuler_213_quat.c Normal file
View File

@ -0,0 +1,169 @@
/*
PURPOSE:
(Generate a LELF_HANDED quaternion using an Euler PITCH_ROLL_YAW sequence
OR generate an Euler PITCH_ROLL_YAW sequence using a quaternion.)
PROGRAMMERS:
(((Hung Nguyen) (CACI) (03/23/2016)))
*/
#include "trick/trick_math.h"
int euler213_quat(
double angle[3], /* Inout: r Method=1, 0=PITCH, 1=ROLL, 2=YAW. */
double quat[4], /* Inout: r Method=0, quaternion matrix. */
int method, /* In: -- 0 = Make left handed 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, mat10, mat11, mat20, mat21, 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] = -s1*c2*c3 + c1*s2*s3;
quat[3] = -c1*c2*s3 + s1*s2*c3;
}
else if (method == 1){
#define TOLERANCE 1.0e-15
/* Within normal range for asin function */
mat21 = 2. * (quat[2] * quat[3] + quat[0] * quat[1]);
if (-1.0 <= -mat21 && -mat21 <= 1.0) {
angle[1] = asin(-mat21);
if (M_ABS(angle[1] - M_PI_2) < 1.0e-6) {
mat10 = 2. * (quat[1] * quat[2] + quat[0] * quat[3]);
mat00 = 1. - 2. * (quat[2] * quat[2] + quat[3] * quat[3]);
angle[0] = atan2(mat10, mat00);
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) {
mat10 = 2. * (quat[1] * quat[2] + quat[0] * quat[3]);
mat00 = 1. - 2. * (quat[2] * quat[2] + quat[3] * quat[3]);
angle[0] = atan2(-mat10, mat00);
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 {
mat01 = 2. * (quat[1] * quat[2] - quat[0] * quat[3]);
mat11 = 1. - 2. * (quat[3] * quat[3] + quat[1] * quat[1]);
mat20 = 2. * (quat[1] * quat[3] - quat[0] * quat[2]);
mat22 = 1. - 2. * (quat[1] * quat[1] + quat[2] * quat[2]);
angle[0] = atan2(mat20, mat22);
angle[2] = atan2(mat01, mat11);
}
}
/* Out of normal range for asin func, but within tolerance */
else if (1.0 < -mat21 && -mat21 <= (1.0 + TOLERANCE)) {
mat10 = 2. * (quat[1] * quat[2] + quat[0] * quat[3]);
mat00 = 1. - 2. * (quat[2] * quat[2] + quat[3] * quat[3]);
angle[0] = atan2(mat10, mat00);
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) <= -mat21 && -mat21 < -1.0) {
mat10 = 2. * (quat[1] * quat[2] + quat[0] * quat[3]);
mat00 = 1. - 2. * (quat[2] * quat[2] + quat[3] * quat[3]);
angle[0] = atan2(-mat10, mat00);
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 */
mat21 = 2. * (quat[2] * quat[3] + quat[0] * quat[1]);
/* Compute euler angles from tranformation */
if (M_ABS(mat21 + 1.0) < 1.0e-6) {
mat10 = 2. * (quat[1] * quat[2] + quat[0] * quat[3]);
mat00 = 1. - 2. * (quat[2] * quat[2] + quat[3] * quat[3]);
angle[0] = atan2(mat10, 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(mat21 - 1.0) < 1.0e-6) {
mat10 = 2. * (quat[1] * quat[2] + quat[0] * quat[3]);
mat00 = 1. - 2. * (quat[2] * quat[2] + quat[3] * quat[3]);
angle[0] = atan2(-mat10, 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 {
mat01 = 2. * (quat[1] * quat[2] - quat[0] * quat[3]);
mat11 = 1. - 2. * (quat[3] * quat[3] + quat[1] * quat[1]);
mat20 = 2. * (quat[1] * quat[3] - quat[0] * quat[2]);
mat21 = 2. * (quat[2] * quat[3] + quat[0] * quat[1]);
mat22 = 1. - 2. * (quat[1] * quat[1] + quat[2] * quat[2]);
angle[0] = atan2(mat20, mat22);
angle[1] = asin(-mat21);
angle[2] = atan2(mat01, mat11);
}
#undef TOLERANCE
}
return(ret);
}

167
trick_source/trick_utils/math/src/deuler_231_quat.c Normal file
View File

@ -0,0 +1,167 @@
/*
PURPOSE:
(Generate a LELF_HANDED quaternion using an Euler PITCH_YAW_ROLL sequence
OR generate an Euler PITCH_YAW_ROLL sequence using a quaternion.)
PROGRAMMERS:
(((Hung Nguyen) (CACI) (03/23/2016)))
*/
#include "trick/trick_math.h"
int euler231_quat(
double angle[3], /* Inout: r Method=1, 0=PITCH, 1=YAW, 2=ROLL. */
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, mat11, mat20, mat21, 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*c2*s3 - s1*s2*c3;
quat[2] = -c1*s2*s3 - s1*c2*c3;
quat[3] = -c1*s2*c3 + s1*c2*s3;
}
else if (method == 1){
#define TOLERANCE 1.0e-15
mat01 = 2. * (quat[1] * quat[2] - quat[0] * quat[3]);
/* Within normal range for asin function */
if (-1.0 <= mat01 && mat01 <= 1.0) {
angle[1] = asin(mat01);
if (M_ABS(angle[1] - M_PI_2) < 1.0e-6) {
mat20 = 2. * (quat[1] * quat[3] - quat[0] * quat[2]);
mat22 = 1. - 2. * (quat[1] * quat[1] + quat[2] * quat[2]);
angle[0] = atan2(mat20, mat22);
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) {
mat20 = 2. * (quat[1] * quat[3] - quat[0] * quat[2]);
mat22 = 1. - 2. * (quat[1] * quat[1] + quat[2] * quat[2]);
angle[0] = atan2(mat20, mat22);
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 {
mat00 = 1. - 2. * (quat[2] * quat[2] + quat[3] * quat[3]);
mat02 = 2. * (quat[1] * quat[3] + quat[0] * quat[2]);
mat11 = 1. - 2. * (quat[3] * quat[3] + quat[1] * quat[1]);
mat21 = 2. * (quat[2] * quat[3] + quat[0] * quat[1]);
angle[0] = atan2(-mat02, mat00);
angle[2] = atan2(-mat21, mat11);
}
}
/* Out of normal range for asin func, but within tolerance */
else if (1.0 < mat01 && mat01 <= (1.0 + TOLERANCE)) {
mat20 = 2. * (quat[1] * quat[3] - quat[0] * quat[2]);
mat22 = 1. - 2. * (quat[1] * quat[1] + quat[2] * quat[2]);
angle[0] = atan2(mat20, mat22);
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) <= mat01 && mat01 < -1.0) {
mat20 = 2. * (quat[1] * quat[3] - quat[0] * quat[2]);
mat22 = 1. - 2. * (quat[1] * quat[1] + quat[2] * quat[2]);
angle[0] = atan2(mat20, mat22);
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 */
mat01 = 2. * (quat[1] * quat[2] - quat[0] * quat[3]);
/* Compute euler angles from tranformation */
if (M_ABS(mat01 - 1.0) < 1.0e-6) {
mat20 = 2. * (quat[1] * quat[3] - quat[0] * quat[2]);
mat22 = 1. - 2. * (quat[1] * quat[1] + quat[2] * quat[2]);
angle[0] = atan2(mat20, mat22) - 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(mat01 + 1.0) < 1.0e-6) {
mat20 = 2. * (quat[1] * quat[3] - quat[0] * quat[2]);
mat22 = 1. - 2. * (quat[1] * quat[1] + quat[2] * quat[2]);
angle[0] = atan2(mat20, mat22) + 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 {
mat00 = 1. - 2. * (quat[2] * quat[2] + quat[3] * quat[3]);
mat02 = 2. * (quat[1] * quat[3] + quat[0] * quat[2]);
mat11 = 1. - 2. * (quat[3] * quat[3] + quat[1] * quat[1]);
mat21 = 2. * (quat[2] * quat[3] + quat[0] * quat[1]);
angle[0] = atan2(-mat02, mat00);
angle[1] = asin(mat01);
angle[2] = atan2(-mat21, mat11);
}
#undef TOLERANCE
}
return(ret);
}

165
trick_source/trick_utils/math/src/deuler_312_quat.c Normal file
View File

@ -0,0 +1,165 @@
/*
PURPOSE:
(Generate a LELF_HANDED quaternion using an Euler YAW_ROLL_PITCH sequence
OR generate an Euler YAW_ROLL_PITCH sequence using a quaternion.)
PROGRAMMERS:
((Hung Nguyen) (CACI) (03/23/2016))
*/
#include "trick/trick_math.h"
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);
}

167
trick_source/trick_utils/math/src/deuler_321_quat.c Normal file
View File

@ -0,0 +1,167 @@
/*
PURPOSE:
(Generate a LELF_HANDED quaternion using an Euler YAW_PITCH_ROLL sequence
OR generate an Euler YAW_PITCH_ROLL sequence using a quaternion.)
PROGRAMMERS:
(((Hung Nguyen) (CACI) (03/23/2016)))
*/
#include "trick/trick_math.h"
int euler321_quat(
double angle[3], /* Inout: r Method=1, 0=YAW, 1=PITCH, 2=ROLL. */
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, mat12, mat20, mat21, 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*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]);
quat[0] = c3*c2*c1 + s3*s2*s1;
quat[1] = -s3*c2*c1 + c3*s2*s1;
quat[2] = -c3*s2*c1 - s3*c2*s1;
quat[3] = -c3*c2*s1 + s3*s2*c1;
}else if (method == 1){
#define TOLERANCE 1.0e-15
mat02 = 2.*(quat[1]*quat[3] + quat[0] * quat[2]);
/* Within normal range for asin function */
if (-1.0 <= -mat02 && -mat02 <= 1.0) {
angle[1] = asin(-mat02);
if (M_ABS(angle[1] - M_PI_2) < 1.0e-6) {
mat20 = 2.*(quat[1]*quat[3] + quat[2]*quat[0]);
mat21 = 2.*(quat[2]*quat[3] - quat[1]*quat[0]);
angle[0] = atan2(mat21, mat20);
angle[1] = M_PI_2;
angle[2] = 0.0;
ret = TM_SING_321_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) {
mat20 = 2.*(quat[1]*quat[3] + quat[2]*quat[0]);
mat21 = 2.*(quat[2]*quat[3] - quat[1]*quat[0]);
angle[0] = atan2(-mat21, -mat20);
angle[1] = -M_PI_2;
angle[2] = 0.0;
ret = TM_SING_321_N;
if ( error_flag[1] == 0 ) {
tm_print_error(ret);
error_flag[1]=1;
}
} else {
mat00 = 1. - 2.*(quat[2]*quat[2] + quat[3]*quat[3]);
mat01 = 2.*(quat[1]*quat[2] - quat[0]*quat[3]);
mat12 = 2.*(quat[2]*quat[3] - quat[0]*quat[1]);
mat22 = 1. - 2.*(quat[1]*quat[1] + quat[2]*quat[2]);
angle[0] = atan2(mat01, mat00);
angle[2] = atan2(mat12, mat22);
}
}
/* Out of normal range for asin func, but within tolerance */
else if (1.0 < -mat02 && -mat02 <= (1.0 + TOLERANCE)) {
mat20 = 2.*(quat[1]*quat[3] + quat[2]*quat[0]);
mat21 = 2.*(quat[2]*quat[3] - quat[1]*quat[0]);
angle[0] = atan2(mat21, mat20);
angle[1] = M_PI_2;
angle[2] = 0.0;
ret = TM_SING_321_P;
if ( error_flag[2] == 0 ) {
tm_print_error(ret);
error_flag[2]=1;
}
} else if ((-1.0 - TOLERANCE) <= -mat02 && -mat02 < -1.0) {
mat20 = 2.*(quat[1]*quat[3] + quat[2]*quat[0]);
mat21 = 2.*(quat[2]*quat[3] - quat[1]*quat[0]);
angle[0] = atan2(-mat21, -mat20);
angle[1] = -M_PI_2;
angle[2] = 0.0;
ret = TM_SING_321_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 */
mat02 = 2.*(quat[1]*quat[3] - quat[2]*quat[0]);
/* Compute euler angles from tranformation */
if (M_ABS(mat02 + 1.0) < 1.0e-6) {
mat20 = 2.*(quat[1]*quat[3] + quat[2]*quat[0]);
mat21 = 2.*(quat[2]*quat[3] - quat[1]*quat[0]);
angle[0] = atan2(mat21, mat20) + 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(mat02 - 1.0) < 1.0e-6) {
mat20 = 2.*(quat[1]*quat[3] + quat[2]*quat[0]);
mat21 = 2.*(quat[2]*quat[3] - quat[1]*quat[0]);
angle[0] = atan2(-mat21, -mat20) + 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 {
mat00 = 1. - 2.*(quat[2]*quat[2] + quat[3]*quat[3]);
mat01 = 2.*(quat[1]*quat[2] - quat[0]*quat[3]);
mat12 = 2.*(quat[2]*quat[3] + quat[1]*quat[0]);
mat22 = 1. - 2.*(quat[1]*quat[1] + quat[2]*quat[2]);
angle[0] = atan2(mat01, mat00);
angle[1] = asin(-mat02);
angle[2] = atan2(mat12, mat22);
}
#undef TOLERANCE
}
return(ret);
}

37
trick_source/trick_utils/math/src/euler_quat.c Normal file
View File

@ -0,0 +1,37 @@
#include "trick/reference_frame.h"
#include "trick/trick_math.h"
int euler_quat (double angle[3], /* In: r Method=0, Euler angles */
double quat[4], /* Out: r Method=0, quaternion matrix */
int method, /* In: 0 = Make quaternion from angles, 1 = Make angles from matrix */
Euler_Seq sequence) /* In: Euler angle sequence for 'angle' */
{
int ret = 0;
switch (sequence) {
case Roll_Pitch_Yaw:
ret = deuler_123_quat(angle, quat, method);
break;
case Roll_Yaw_Pitch:
ret = deuler_132_quat(angle, quat, method);
break;
case Pitch_Yaw_Roll:
ret = deuler_231_quat(angle, quat, method);
break;
case Pitch_Roll_Yaw:
ret = deuler_213_quat(angle, quat, method);
break;
case Yaw_Roll_Pitch:
ret = deuler_312_quat(angle, quat, method);
break;
case Yaw_Pitch_Roll:
ret = deuler_321_quat(angle, quat, method);
break;
default:
ret = TM_INV_ROT_SEQ;
}
return (ret);
}

4
trick_source/trick_utils/math/src/mat_to_quat.c
View File

@ -1,5 +1,5 @@
/* /*
PURPOSE: (Compute the normalized quaternion from the corresponding orthonormal transformation matrix) PURPOSE: (Compute the normalized left handed quaternion from the corresponding orthonormal transformation matrix)
REFERENCE: ((Flight Control - Orbit DAP) (FSSR STS 83-0009C OI-20, Section 4.3.8 MAT_TO_QUAT) (November 29, 1990)) REFERENCE: ((Flight Control - Orbit DAP) (FSSR STS 83-0009C OI-20, Section 4.3.8 MAT_TO_QUAT) (November 29, 1990))
@ -9,7 +9,7 @@
#include "trick/trick_math.h" #include "trick/trick_math.h"
void mat_to_quat(double quat[4], /* Out: Quaternion */ void mat_to_quat(double quat[4], /* Out: Left handed quaternion */
double a[3][3]) double a[3][3])
{ /* In: Transformation matrix */ { /* In: Transformation matrix */

4
trick_source/trick_utils/math/src/quat_to_mat.c
View File

@ -7,9 +7,9 @@
#include "trick/trick_math.h" #include "trick/trick_math.h"
void quat_to_mat(double a[3][3], /* Out: transformation matrix */ void quat_to_mat(double a[3][3], /* Out: transformation matrix */
double quat[4]) double quat[4])
{ /* In: quaternion */ { /* In: left handed quaternion */
a[0][0] = 1. - 2. * (quat[2] * quat[2] + quat[3] * quat[3]); a[0][0] = 1. - 2. * (quat[2] * quat[2] + quat[3] * quat[3]);
a[1][1] = 1. - 2. * (quat[3] * quat[3] + quat[1] * quat[1]); a[1][1] = 1. - 2. * (quat[3] * quat[3] + quat[1] * quat[1]);

37
trick_source/trick_utils/math/test/Makefile Normal file
View File

@ -0,0 +1,37 @@
#SYNOPSIS:
#
# make [all] - makes everything.
# make TARGET - makes the given target.
# make clean - removes all files generated by make.
include ${TRICK_HOME}/share/trick/makefiles/Makefile.common
# Flags passed to the preprocessor.
TRICK_CPPFLAGS += -I${GTEST_HOME}/include -I$(TRICK_HOME)/include -g -Wall -Wextra -DGTEST_HAS_TR1_TUPLE=0
TRICK_LIBS = ${TRICK_LIB_DIR}/libtrick_math.a
TRICK_EXEC_LINK_LIBS += ${GTEST_HOME}/lib/libgtest.a ${GTEST_HOME}/lib/libgtest_main.a
TRICK_EXEC_LINK_LIBS += -lm
GTEST_HEADERS = $(GTEST_DIR)/include/gtest/*.h \
$(GTEST_DIR)/include/gtest/internal/*.h
TESTS = UnitTestEulerQuat
all : test
test: $(TESTS)
./UnitTestEulerQuat --gtest_output=xml:${TRICK_HOME}/trick_test/UnitTestEulerQuat.xml
clean :
rm -f $(TESTS)
rm -f *.o
rm -rf XMLtestReports
UnitTestEulerQuat.o : UnitTestEulerQuat.cpp
$(TRICK_CPPC) $(TRICK_CPPFLAGS) -c $<
UnitTestEulerQuat : UnitTestEulerQuat.o
@echo 'Building UnitTestEulerQuat'
$(TRICK_LD) $(TRICK_LDFLAGS) -o $@ $^ $(OTHER_OBJECTS) -L${TRICK_HOME}/lib_${TRICK_HOST_CPU} $(TRICK_LIBS) $(TRICK_EXEC_LINK_LIBS)

398
trick_source/trick_utils/math/test/UnitTestEulerQuat.cpp Normal file
View File

@ -0,0 +1,398 @@
#include <gtest/gtest.h>
#include "trick/trick_math.h"
#include "trick/reference_frame.h"
using namespace std;
class EulerQuatTest : public ::testing::Test {
public:
TRANSFORM init_data;
bool pass;
int num_tests;
int num_pass;
int method;
double quat[4];
double mat[3][3];
EulerQuatTest() {
this->num_tests = 100;
this->num_pass = 0;
this->pass = false;
}
~EulerQuatTest() {}
void SetUp() {}
void TearDown() {}
void compare_euler(double eul[3]){
double eul_error[3];
double mat_test[3][3];
double mat_error[3][3];
M_IDENT(mat_error);
M_IDENT(mat_test);
V_INIT(eul_error);
euler_matrix(eul, mat_test, 0 , this->init_data.euler_sequence);
MxMt(mat_error, mat_test, this->mat);
euler_matrix(eul_error, mat_error, 0 , this->init_data.euler_sequence);
if(V_MAG(eul_error) < 0.00001){
this->num_pass ++;
pass = true;
} else{
pass = false;
printf("EulerAngle = %lf %lf %lf\n",this->init_data.euler_angles[0],
this->init_data.euler_angles[1],
this->init_data.euler_angles[2]);
}
return;
}
void compare_quat(double Q[4]){
double Q_error[4];
double mat_error[3][3];
double eul_error[3];
M_IDENT(mat_error);
V_INIT(eul_error);
QxQt(Q_error, Q, quat);
quat_to_mat(mat_error, Q_error);
euler_matrix(eul_error, mat_error, 0 , this->init_data.euler_sequence);
if(V_MAG(eul_error) < 0.00001){
this->num_pass ++;
this->pass = true;
} else{
this->pass = false;
printf("EulerAngle = %lf %lf %lf\n",this->init_data.euler_angles[0],
this->init_data.euler_angles[1],
this->init_data.euler_angles[2]);
}
return;
}
void test_euler_to_quat(){
double Q[4];
euler_quat(this->init_data.euler_angles, Q, 0, this->init_data.euler_sequence);
compare_quat( Q );
return;
}
void test_quat_to_euler(int test){
double eul[3];
this->method = test;
if(this->method == 1){
euler_quat(eul, this->quat, this->method, this->init_data.euler_sequence);
}
else {
switch (this->init_data.euler_sequence) {
case Roll_Pitch_Yaw:
euler123_quat( eul, this->quat, 2, this->init_data.euler_angles );
break;
case Roll_Yaw_Pitch:
euler132_quat( eul, this->quat, 2, this->init_data.euler_angles );
break;
case Pitch_Yaw_Roll:
euler231_quat( eul, this->quat, 2, this->init_data.euler_angles );
break;
case Pitch_Roll_Yaw:
euler213_quat( eul, this->quat, 2, this->init_data.euler_angles );
break;
case Yaw_Roll_Pitch:
euler312_quat( eul, this->quat, 2, this->init_data.euler_angles );
break;
case Yaw_Pitch_Roll:
euler321_quat( eul, this->quat, 2, this->init_data.euler_angles );
break;
}
}
compare_euler(eul);
return;
}
};
TEST_F(EulerQuatTest, euler123_to_quat)
{
init_data.euler_sequence = Roll_Pitch_Yaw;
num_pass = 0;
srand(time(NULL));
for (int ii=0; ii<num_tests; ii++){
// Randomly input euler angles:
init_data.euler_angles[0] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[1] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[2] = (rand()%num_tests) * (2*M_PI/num_tests);
euler_matrix(init_data.euler_angles, mat, 0 , init_data.euler_sequence);
mat_to_quat(quat, mat);
test_euler_to_quat();
}
EXPECT_EQ(num_pass, num_tests);
}
TEST_F(EulerQuatTest, euler132_to_quat)
{
init_data.euler_sequence = Roll_Yaw_Pitch;
num_pass = 0;
for (int ii=0; ii<num_tests; ii++){
// Randomly input euler angles:
init_data.euler_angles[0] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[1] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[2] = (rand()%num_tests) * (2*M_PI/num_tests);
euler_matrix(init_data.euler_angles, mat, 0 , init_data.euler_sequence);
mat_to_quat(quat, mat);
test_euler_to_quat();
}
EXPECT_EQ(num_pass, num_tests);
}
TEST_F(EulerQuatTest, euler231_to_quat)
{
init_data.euler_sequence = Pitch_Yaw_Roll;
num_pass = 0;
for (int ii=0; ii<num_tests; ii++){
// Randomly input euler angles:
init_data.euler_angles[0] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[1] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[2] = (rand()%num_tests) * (2*M_PI/num_tests);
euler_matrix(init_data.euler_angles, mat, 0 , init_data.euler_sequence);
mat_to_quat(quat, mat);
test_euler_to_quat();
}
EXPECT_EQ(num_pass, num_tests);
}
TEST_F(EulerQuatTest, euler213_to_quat)
{
init_data.euler_sequence = Pitch_Roll_Yaw;
num_pass = 0;
for (int ii=0; ii<num_tests; ii++){
// Randomly input euler angles:
init_data.euler_angles[0] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[1] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[2] = (rand()%num_tests) * (2*M_PI/num_tests);
euler_matrix(init_data.euler_angles, mat, 0 , init_data.euler_sequence);
mat_to_quat(quat, mat);
test_euler_to_quat();
}
EXPECT_EQ(num_pass, num_tests);
}
TEST_F(EulerQuatTest, euler312_to_quat)
{
init_data.euler_sequence = Yaw_Roll_Pitch;
num_pass = 0;
for (int ii=0; ii<num_tests; ii++){
// Randomly input euler angles:
init_data.euler_angles[0] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[1] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[2] = (rand()%num_tests) * (2*M_PI/num_tests);
euler_matrix(init_data.euler_angles, mat, 0 , init_data.euler_sequence);
mat_to_quat(quat, mat);
test_euler_to_quat();
}
EXPECT_EQ(num_pass, num_tests);
}
TEST_F(EulerQuatTest, euler321_to_quat)
{
init_data.euler_sequence = Yaw_Pitch_Roll;
num_pass = 0;
for (int ii=0; ii<num_tests; ii++){
// Randomly input euler angles:
init_data.euler_angles[0] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[1] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[2] = (rand()%num_tests) * (2*M_PI/num_tests);
euler_matrix(init_data.euler_angles, mat, 0 , init_data.euler_sequence);
mat_to_quat(quat, mat);
test_euler_to_quat();
}
EXPECT_EQ(num_pass, num_tests);
}
TEST_F(EulerQuatTest, quat_to_euler123_method1)
{
init_data.euler_sequence = Roll_Pitch_Yaw;
num_pass = 0;
for (int ii=0; ii<num_tests; ii++){
// Randomly input euler angles:
init_data.euler_angles[0] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[1] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[2] = (rand()%num_tests) * (2*M_PI/num_tests);
euler_matrix(init_data.euler_angles, mat, 0 , init_data.euler_sequence);
mat_to_quat(quat, mat);
test_quat_to_euler(1);
}
EXPECT_EQ(num_pass, num_tests);
}
TEST_F(EulerQuatTest, quat_to_euler123_method2)
{
init_data.euler_sequence = Roll_Pitch_Yaw;
num_pass = 0;
for (int ii=0; ii<num_tests; ii++){
// Randomly input euler angles:
init_data.euler_angles[0] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[1] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[2] = (rand()%num_tests) * (2*M_PI/num_tests);
euler_matrix(init_data.euler_angles, mat, 0 , init_data.euler_sequence);
mat_to_quat(quat, mat);
test_quat_to_euler(2);
}
EXPECT_EQ(num_pass, num_tests);
}
TEST_F(EulerQuatTest, quat_to_euler132_method1)
{
init_data.euler_sequence = Roll_Yaw_Pitch;
num_pass = 0;
for (int ii=0; ii<num_tests; ii++){
// Randomly input euler angles:
init_data.euler_angles[0] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[1] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[2] = (rand()%num_tests) * (2*M_PI/num_tests);
euler_matrix(init_data.euler_angles, mat, 0 , init_data.euler_sequence);
mat_to_quat(quat, mat);
test_quat_to_euler(1);
}
EXPECT_EQ(num_pass, num_tests);
}
TEST_F(EulerQuatTest, quat_to_euler132_method2)
{
init_data.euler_sequence = Roll_Yaw_Pitch;
num_pass = 0;
for (int ii=0; ii<num_tests; ii++){
// Randomly input euler angles:
init_data.euler_angles[0] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[1] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[2] = (rand()%num_tests) * (2*M_PI/num_tests);
euler_matrix(init_data.euler_angles, mat, 0 , init_data.euler_sequence);
mat_to_quat(quat, mat);
test_quat_to_euler(2);
}
EXPECT_EQ(num_pass, num_tests);
}
TEST_F(EulerQuatTest, quat_to_euler231_method1)
{
init_data.euler_sequence = Pitch_Yaw_Roll;
num_pass = 0;
for (int ii=0; ii<num_tests; ii++){
// Randomly input euler angles:
init_data.euler_angles[0] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[1] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[2] = (rand()%num_tests) * (2*M_PI/num_tests);
euler_matrix(init_data.euler_angles, mat, 0 , init_data.euler_sequence);
mat_to_quat(quat, mat);
test_quat_to_euler(1);
}
EXPECT_EQ(num_pass, num_tests);
}
TEST_F(EulerQuatTest, quat_to_euler231_method2)
{
init_data.euler_sequence = Pitch_Yaw_Roll;
num_pass = 0;
for (int ii=0; ii<num_tests; ii++){
// Randomly input euler angles:
init_data.euler_angles[0] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[1] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[2] = (rand()%num_tests) * (2*M_PI/num_tests);
euler_matrix(init_data.euler_angles, mat, 0 , init_data.euler_sequence);
mat_to_quat(quat, mat);
test_quat_to_euler(2);
}
EXPECT_EQ(num_pass, num_tests);
}
TEST_F(EulerQuatTest, quat_to_euler213_method1)
{
init_data.euler_sequence = Pitch_Roll_Yaw;
num_pass = 0;
for (int ii=0; ii<num_tests; ii++){
// Randomly input euler angles:
init_data.euler_angles[0] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[1] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[2] = (rand()%num_tests) * (2*M_PI/num_tests);
euler_matrix(init_data.euler_angles, mat, 0 , init_data.euler_sequence);
mat_to_quat(quat, mat);
test_quat_to_euler(1);
}
EXPECT_EQ(num_pass, num_tests);
}
TEST_F(EulerQuatTest, quat_to_euler213_method2)
{
init_data.euler_sequence = Pitch_Roll_Yaw;
num_pass = 0;
for (int ii=0; ii<num_tests; ii++){
// Randomly input euler angles:
init_data.euler_angles[0] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[1] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[2] = (rand()%num_tests) * (2*M_PI/num_tests);
euler_matrix(init_data.euler_angles, mat, 0 , init_data.euler_sequence);
mat_to_quat(quat, mat);
test_quat_to_euler(2);
}
EXPECT_EQ(num_pass, num_tests);
}
TEST_F(EulerQuatTest, quat_to_euler312_method1)
{
init_data.euler_sequence = Yaw_Roll_Pitch;
num_pass = 0;
for (int ii=0; ii<num_tests; ii++){
// Randomly input euler angles:
init_data.euler_angles[0] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[1] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[2] = (rand()%num_tests) * (2*M_PI/num_tests);
euler_matrix(init_data.euler_angles, mat, 0 , init_data.euler_sequence);
mat_to_quat(quat, mat);
test_quat_to_euler(1);
}
EXPECT_EQ(num_pass, num_tests);
}
TEST_F(EulerQuatTest, quat_to_euler312_method2)
{
init_data.euler_sequence = Yaw_Roll_Pitch;
num_pass = 0;
for (int ii=0; ii<num_tests; ii++){
// Randomly input euler angles:
init_data.euler_angles[0] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[1] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[2] = (rand()%num_tests) * (2*M_PI/num_tests);
euler_matrix(init_data.euler_angles, mat, 0 , init_data.euler_sequence);
mat_to_quat(quat, mat);
test_quat_to_euler(2);
}
EXPECT_EQ(num_pass, num_tests);
}
TEST_F(EulerQuatTest, quat_to_euler321_method1)
{
init_data.euler_sequence = Yaw_Pitch_Roll;
num_pass = 0;
for (int ii=0; ii<num_tests; ii++){
// Randomly input euler angles:
init_data.euler_angles[0] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[1] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[2] = (rand()%num_tests) * (2*M_PI/num_tests);
euler_matrix(init_data.euler_angles, mat, 0 , init_data.euler_sequence);
mat_to_quat(quat, mat);
test_quat_to_euler(1);
}
EXPECT_EQ(num_pass, num_tests);
}
TEST_F(EulerQuatTest, quat_to_euler321_method2)
{
init_data.euler_sequence = Yaw_Pitch_Roll;
num_pass = 0;
for (int ii=0; ii<num_tests; ii++){
// Randomly input euler angles:
init_data.euler_angles[0] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[1] = (rand()%num_tests) * (2*M_PI/num_tests);
init_data.euler_angles[2] = (rand()%num_tests) * (2*M_PI/num_tests);
euler_matrix(init_data.euler_angles, mat, 0 , init_data.euler_sequence);
mat_to_quat(quat, mat);
test_quat_to_euler(2);
}
EXPECT_EQ(num_pass, num_tests);
}