static void test_6(void) {
printf("\n");
struct FloatEulers ea2b;
EULERS_ASSIGN(ea2b, RadOfDeg(45.), RadOfDeg(22.), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("ea2b", ea2b);
struct FloatEulers eb2c;
EULERS_ASSIGN(eb2c, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(90.));
DISPLAY_FLOAT_EULERS_DEG("eb2c", eb2c);
struct FloatRMat fa2b;
FLOAT_RMAT_OF_EULERS(fa2b, ea2b);
struct FloatRMat fb2c;
FLOAT_RMAT_OF_EULERS(fb2c, eb2c);
printf("\n");
test_rmat_comp(fa2b, fb2c, 1);
struct FloatQuat qa2b;
FLOAT_QUAT_OF_EULERS(qa2b, ea2b);
struct FloatQuat qb2c;
FLOAT_QUAT_OF_EULERS(qb2c, eb2c);
printf("\n");
test_quat_comp(qa2b, qb2c, 1);
printf("\n");
}
static void test_7(void) {
printf("\n");
struct FloatEulers ea2c;
EULERS_ASSIGN(ea2c, RadOfDeg(29.742755), RadOfDeg(-40.966522), RadOfDeg(69.467265));
DISPLAY_FLOAT_EULERS_DEG("ea2c", ea2c);
struct FloatEulers eb2c;
EULERS_ASSIGN(eb2c, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(90.));
DISPLAY_FLOAT_EULERS_DEG("eb2c", eb2c);
struct FloatRMat fa2c;
FLOAT_RMAT_OF_EULERS(fa2c, ea2c);
struct FloatRMat fb2c;
FLOAT_RMAT_OF_EULERS(fb2c, eb2c);
printf("\n");
test_rmat_comp_inv(fa2c, fb2c, 1);
struct FloatQuat qa2c;
FLOAT_QUAT_OF_EULERS(qa2c, ea2c);
struct FloatQuat qb2c;
FLOAT_QUAT_OF_EULERS(qb2c, eb2c);
printf("\n");
test_quat_comp_inv(qa2c, qb2c, 1);
printf("\n");
}
static void test_10(void) {
struct FloatEulers euler;
EULERS_ASSIGN(euler , RadOfDeg(0.), RadOfDeg(10.), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("euler", euler);
struct FloatQuat quat;
FLOAT_QUAT_OF_EULERS(quat, euler);
DISPLAY_FLOAT_QUAT("####quat", quat);
struct Int32Eulers euleri;
EULERS_BFP_OF_REAL(euleri, euler);
DISPLAY_INT32_EULERS("euleri", euleri);
struct Int32Quat quati;
INT32_QUAT_OF_EULERS(quati, euleri);
DISPLAY_INT32_QUAT("####quat", quati);
struct Int32RMat rmati;
INT32_RMAT_OF_EULERS(rmati, euleri);
DISPLAY_INT32_RMAT("####rmat", rmati);
struct Int32Quat quat_ltp_to_body;
struct Int32Quat body_to_imu_quat;
INT32_QUAT_ZERO( body_to_imu_quat);
INT32_QUAT_COMP_INV(quat_ltp_to_body, body_to_imu_quat, quati);
DISPLAY_INT32_QUAT("####quat_ltp_to_body", quat_ltp_to_body);
}
static void test_10(void)
{
struct FloatEulers euler;
EULERS_ASSIGN(euler , RadOfDeg(0.), RadOfDeg(10.), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("euler", euler);
struct FloatQuat quat;
float_quat_of_eulers(&quat, &euler);
DISPLAY_FLOAT_QUAT("####quat", quat);
struct Int32Eulers euleri;
EULERS_BFP_OF_REAL(euleri, euler);
DISPLAY_INT32_EULERS("euleri", euleri);
struct Int32Quat quati;
int32_quat_of_eulers(&quati, &euleri);
DISPLAY_INT32_QUAT("####quat", quati);
struct Int32RMat rmati;
int32_rmat_of_eulers(&rmati, &euleri);
DISPLAY_INT32_RMAT("####rmat", rmati);
struct Int32Quat quat_ltp_to_body;
struct Int32Quat body_to_imu_quat;
int32_quat_identity(&body_to_imu_quat);
int32_quat_comp_inv(&quat_ltp_to_body, &body_to_imu_quat, &quati);
DISPLAY_INT32_QUAT("####quat_ltp_to_body", quat_ltp_to_body);
}
static void test_8(void)
{
printf("Running %d iterations of test_INT32_QUAT_OF_RMAT\n", NB_ITER);
#if SEED_RANDOM_FROM_TIME
#pragma message "Seeding random from current time"
time_t now;
now = time(NULL);
struct tm *ts;
ts = localtime(&now);
srandom(ts->tm_sec);
#endif
float max_err = 0.;
float sum_err = 0.;
int nb_err = 0;
int i;
for (i = 0; i < NB_ITER; i++) {
struct FloatEulers eul;
eul.phi = ((double)random() / (double)RAND_MAX - 0.5) * M_PI * 2.;
eul.theta = ((double)random() / (double)RAND_MAX - 0.5) * M_PI * 2.;
eul.psi = ((double)random() / (double)RAND_MAX - 0.5) * M_PI * 2.;
float err = test_INT32_QUAT_OF_RMAT(&eul, FALSE);
sum_err += err;
if (err > max_err) { max_err = err; }
if (err > .01) {
nb_err++;
printf("\nIteration %d with large error: %f\n", i, err);
DISPLAY_FLOAT_EULERS_DEG("Eulers in deg", eul);
test_INT32_QUAT_OF_RMAT(&eul, TRUE);
printf("\n");
}
}
printf("Number of errors %d, average error %f, max error %f\n", nb_err, sum_err / NB_ITER, max_err);
}
float test_eulers_of_rmat(struct FloatRMat frm, int display)
{
struct FloatEulers fe;
float_eulers_of_rmat(&fe, &frm);
struct Int32RMat irm;
RMAT_BFP_OF_REAL(irm, frm);
struct Int32Eulers ie;
int32_eulers_of_rmat(&ie, &irm);
struct FloatEulers fe2;
EULERS_FLOAT_OF_BFP(fe2, ie);
EULERS_SUB(fe2, ie);
float norm_err = FLOAT_EULERS_NORM(fe2);
if (display) {
printf("euler of rmat\n");
// DISPLAY_FLOAT_RMAT("fr", fr);
DISPLAY_FLOAT_EULERS_DEG("fe", fe);
DISPLAY_INT32_EULERS("ie", ie);
DISPLAY_INT32_EULERS_AS_FLOAT_DEG("ieaf", ie);
}
return norm_err;
}
void test_of_axis_angle(void)
{
struct FloatVect3 axis = { 0., 1., 0.};
FLOAT_VECT3_NORMALIZE(axis);
DISPLAY_FLOAT_VECT3("axis", axis);
const float angle = RadOfDeg(30.);
printf("angle %f\n", DegOfRad(angle));
struct FloatQuat my_q;
FLOAT_QUAT_OF_AXIS_ANGLE(my_q, axis, angle);
DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("quat", my_q);
struct FloatRMat my_r1;
float_rmat_of_quat(&my_r1, &my_q);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", my_r1);
DISPLAY_FLOAT_RMAT("rmat1", my_r1);
struct FloatRMat my_r;
FLOAT_RMAT_OF_AXIS_ANGLE(my_r, axis, angle);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", my_r);
DISPLAY_FLOAT_RMAT("rmat", my_r);
printf("\n");
struct FloatEulers eul = {RadOfDeg(30.), RadOfDeg(30.), 0.};
struct FloatVect3 uz = { 0., 0., 1.};
struct FloatRMat r_yaw;
FLOAT_RMAT_OF_AXIS_ANGLE(r_yaw, uz, eul.psi);
struct FloatVect3 uy = { 0., 1., 0.};
struct FloatRMat r_pitch;
FLOAT_RMAT_OF_AXIS_ANGLE(r_pitch, uy, eul.theta);
struct FloatVect3 ux = { 1., 0., 0.};
struct FloatRMat r_roll;
FLOAT_RMAT_OF_AXIS_ANGLE(r_roll, ux, eul.phi);
struct FloatRMat r_yaw_pitch;
float_rmat_comp(&r_yaw_pitch, &r_yaw, &r_pitch);
struct FloatRMat r_yaw_pitch_roll;
float_rmat_comp(&r_yaw_pitch_roll, &r_yaw_pitch, &r_roll);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", r_yaw_pitch_roll);
DISPLAY_FLOAT_RMAT("rmat", r_yaw_pitch_roll);
DISPLAY_FLOAT_EULERS_DEG("eul", eul);
struct FloatRMat rmat1;
float_rmat_of_eulers(&rmat1, &eul);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", rmat1);
DISPLAY_FLOAT_RMAT("rmat1", rmat1);
}
float test_quat_of_rmat(void)
{
// struct FloatEulers eul = {-0.280849, 0.613423, -1.850440};
struct FloatEulers eul = {RadOfDeg(0.131579), RadOfDeg(-62.397659), RadOfDeg(-110.470299)};
// struct FloatEulers eul = {RadOfDeg(0.13), RadOfDeg(180.), RadOfDeg(-61.)};
struct FloatRMat rm;
float_rmat_of_eulers(&rm, &eul);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", rm);
struct FloatQuat q;
float_quat_of_rmat(&q, &rm);
DISPLAY_FLOAT_QUAT("q_of_rm ", q);
DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_rm ", q);
struct FloatQuat qref;
float_quat_of_eulers(&qref, &eul);
DISPLAY_FLOAT_QUAT("q_of_euler", qref);
DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_euler", qref);
printf("\n\n\n");
struct FloatRMat r_att;
struct FloatEulers e312 = { eul.phi, eul.theta, eul.psi };
float_rmat_of_eulers_312(&r_att, &e312);
DISPLAY_FLOAT_RMAT("r_att ", r_att);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("r_att ", r_att);
struct FloatQuat q_att;
float_quat_of_rmat(&q_att, &r_att);
struct FloatEulers e_att;
float_eulers_of_rmat(&e_att, &r_att);
DISPLAY_FLOAT_EULERS_DEG("of rmat", e_att);
float_eulers_of_quat(&e_att, &q_att);
DISPLAY_FLOAT_EULERS_DEG("of quat", e_att);
return 0.;
}
float test_quat_of_rmat(void) {
// struct FloatEulers eul = {-0.280849, 0.613423, -1.850440};
struct FloatEulers eul = {RadOfDeg(0.131579), RadOfDeg(-62.397659), RadOfDeg(-110.470299)};
// struct FloatEulers eul = {RadOfDeg(0.13), RadOfDeg(180.), RadOfDeg(-61.)};
struct FloatMat33 rm;
FLOAT_RMAT_OF_EULERS(rm, eul);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", rm);
struct FloatQuat q;
FLOAT_QUAT_OF_RMAT(q, rm);
DISPLAY_FLOAT_QUAT("q_of_rm ", q);
DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_rm ", q);
struct FloatQuat qref;
FLOAT_QUAT_OF_EULERS(qref, eul);
DISPLAY_FLOAT_QUAT("q_of_euler", qref);
DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_euler", qref);
printf("\n\n\n");
struct FloatMat33 r_att;
struct FloatEulers e312 = { eul.phi, eul.theta, eul.psi };
FLOAT_RMAT_OF_EULERS_312(r_att, e312);
DISPLAY_FLOAT_RMAT("r_att ", r_att);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("r_att ", r_att);
struct FloatQuat q_att;
FLOAT_QUAT_OF_RMAT(q_att, r_att);
struct FloatEulers e_att;
FLOAT_EULERS_OF_RMAT(e_att, r_att);
DISPLAY_FLOAT_EULERS_DEG("of rmat", e_att);
FLOAT_EULERS_OF_QUAT(e_att, q_att);
DISPLAY_FLOAT_EULERS_DEG("of quat", e_att);
return 0.;
}
static void test_4_float(void) {
printf("euler to quat to euler - float\n");
/* initial euler angles */
struct FloatEulers e;
EULERS_ASSIGN(e, RadOfDeg(-10.66), RadOfDeg(-0.7), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("e", e);
/* transform to quaternion */
struct FloatQuat q;
FLOAT_QUAT_OF_EULERS(q, e);
// DISPLAY_FLOAT_QUAT("q", q);
FLOAT_QUAT_NORMALIZE(q);
DISPLAY_FLOAT_QUAT("q_n", q);
DISPLAY_FLOAT_QUAT_AS_INT("q_n as int", q);
/* back to eulers */
struct FloatEulers e2;
FLOAT_EULERS_OF_QUAT(e2, q);
DISPLAY_FLOAT_EULERS_DEG("e2", e2);
}
static void test_4_float(void)
{
printf("euler to quat to euler - float\n");
/* initial euler angles */
struct FloatEulers e;
EULERS_ASSIGN(e, RadOfDeg(-10.66), RadOfDeg(-0.7), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("e", e);
/* transform to quaternion */
struct FloatQuat q;
float_quat_of_eulers(&q, &e);
// DISPLAY_FLOAT_QUAT("q", q);
float_quat_normalize(&q);
DISPLAY_FLOAT_QUAT("q_n", q);
DISPLAY_FLOAT_QUAT_AS_INT("q_n as int", q);
/* back to eulers */
struct FloatEulers e2;
float_eulers_of_quat(&e2, &q);
DISPLAY_FLOAT_EULERS_DEG("e2", e2);
}
static void test_5(void) {
struct FloatEulers fe;
EULERS_ASSIGN(fe, RadOfDeg(-10.66), RadOfDeg(-0.7), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("fe", fe);
printf("\n");
struct FloatQuat fq;
FLOAT_QUAT_OF_EULERS(fq, fe);
test_eulers_of_quat(fq, 1);
printf("\n");
struct FloatRMat frm;
FLOAT_RMAT_OF_EULERS(frm, fe);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("frm", frm);
test_eulers_of_rmat(frm, 1);
printf("\n");
}
float test_rmat_of_eulers_312(void) {
struct FloatEulers eul312_f;
EULERS_ASSIGN(eul312_f, RadOfDeg(45.), RadOfDeg(22.), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("eul312_f", eul312_f);
struct Int32Eulers eul312_i;
EULERS_BFP_OF_REAL(eul312_i, eul312_f);
DISPLAY_INT32_EULERS("eul312_i", eul312_i);
struct FloatRMat rmat_f;
FLOAT_RMAT_OF_EULERS_312(rmat_f, eul312_f);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat float", rmat_f);
struct Int32RMat rmat_i;
INT32_RMAT_OF_EULERS_312(rmat_i, eul312_i);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("rmat int", rmat_i);
return 0;
}
static void test_8(void) {
float max_err = 0.;
float sum_err = 0.;
int i;
for (i=0; i<NB_ITER; i++) {
struct FloatEulers eul;
eul.phi = ((double)random()/(double)RAND_MAX - 0.5)*M_PI*2.;
eul.theta = ((double)random()/(double)RAND_MAX - 0.5)*M_PI*2.;
eul.psi = ((double)random()/(double)RAND_MAX - 0.5)*M_PI*2.;
float err = test_INT32_QUAT_OF_RMAT(&eul, FALSE);
sum_err += err;
if (err > max_err) max_err = err;
if (err > .01) {
printf("%f\n", err);
DISPLAY_FLOAT_EULERS_DEG("eul", eul);
test_INT32_QUAT_OF_RMAT(&eul, TRUE);
}
}
printf("err %f (%f)\n", sum_err/NB_ITER, max_err);
}
float test_eulers_of_quat(struct FloatQuat fq, int display) {
struct FloatEulers fe;
FLOAT_EULERS_OF_QUAT(fe, fq);
struct Int32Quat iq;
QUAT_BFP_OF_REAL(iq, fq);
struct Int32Eulers ie;
INT32_EULERS_OF_QUAT(ie, iq);
struct FloatEulers fe2;
EULERS_FLOAT_OF_BFP(fe2, ie);
EULERS_SUB(fe2, ie);
float norm_err = FLOAT_EULERS_NORM(fe2);
if (display) {
printf("euler of quat\n");
DISPLAY_FLOAT_QUAT("fq", fq);
DISPLAY_FLOAT_EULERS_DEG("fe", fe);
DISPLAY_INT32_EULERS("ie", ie);
DISPLAY_INT32_EULERS_AS_FLOAT_DEG("ieaf", ie);
}
return norm_err;
}
