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_rmat_comp_inv(struct FloatRMat ma2c_f, struct FloatRMat mb2c_f, int display)
{
struct FloatRMat ma2b_f;
float_rmat_comp_inv(&ma2b_f, &ma2c_f, &mb2c_f);
struct Int32RMat ma2c_i;
RMAT_BFP_OF_REAL(ma2c_i, ma2c_f);
struct Int32RMat mb2c_i;
RMAT_BFP_OF_REAL(mb2c_i, mb2c_f);
struct Int32RMat ma2b_i;
int32_rmat_comp_inv(&ma2b_i, &ma2c_i, &mb2c_i);
struct FloatRMat err;
RMAT_DIFF(err, ma2b_f, ma2b_i);
float norm_err = FLOAT_RMAT_NORM(err);
if (display) {
printf("rmap comp_inv\n");
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("a2cf", ma2b_f);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("a2ci", ma2b_i);
}
return norm_err;
}
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_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_INT32_QUAT_OF_RMAT(struct FloatEulers *eul_f, bool display)
{
struct Int32Eulers eul321_i;
EULERS_BFP_OF_REAL(eul321_i, (*eul_f));
struct Int32Eulers eul312_i;
EULERS_BFP_OF_REAL(eul312_i, (*eul_f));
if (display) { DISPLAY_INT32_EULERS("eul312_i", eul312_i); }
struct FloatRMat rmat_f;
float_rmat_of_eulers_321(&rmat_f, eul_f);
if (display) { DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat float", rmat_f); }
if (display) { DISPLAY_FLOAT_RMAT("rmat float", rmat_f); }
struct Int32RMat rmat_i;
int32_rmat_of_eulers_321(&rmat_i, &eul321_i);
if (display) { DISPLAY_INT32_RMAT_AS_EULERS_DEG("rmat int", rmat_i); }
if (display) { DISPLAY_INT32_RMAT("rmat int", rmat_i); }
if (display) { DISPLAY_INT32_RMAT_AS_FLOAT("rmat int", rmat_i); }
struct FloatQuat qf;
float_quat_of_rmat(&qf, &rmat_f);
//FLOAT_QUAT_WRAP_SHORTEST(qf);
if (display) { DISPLAY_FLOAT_QUAT("qf", qf); }
struct Int32Quat qi;
int32_quat_of_rmat(&qi, &rmat_i);
//int32_quat_wrap_shortest(&qi);
if (display) { DISPLAY_INT32_QUAT("qi", qi); }
if (display) { DISPLAY_INT32_QUAT_2("qi", qi); }
struct FloatQuat qif;
QUAT_FLOAT_OF_BFP(qif, qi);
// dot product of two quaternions is 1 if they represent same rotation
float qi_dot_qf = qif.qi * qf.qi + qif.qx * qf.qx + qif.qy * qf.qy + qif.qz * qf.qz;
float err_norm = fabs(fabs(qi_dot_qf) - 1.);
if (display) { printf("err %f\n", err_norm); }
if (display) { printf("\n"); }
return err_norm;
}
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;
}
float test_INT32_QUAT_OF_RMAT(struct FloatEulers* eul_f, bool_t display) {
struct Int32Eulers eul312_i;
EULERS_BFP_OF_REAL(eul312_i, (*eul_f));
if (display) DISPLAY_INT32_EULERS("eul312_i", eul312_i);
struct FloatRMat rmat_f;
FLOAT_RMAT_OF_EULERS_312(rmat_f, (*eul_f));
if (display) DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat float", rmat_f);
if (display) DISPLAY_FLOAT_RMAT("rmat float", rmat_f);
struct Int32RMat rmat_i;
INT32_RMAT_OF_EULERS_312(rmat_i, eul312_i);
if (display) DISPLAY_INT32_RMAT_AS_EULERS_DEG("rmat int", rmat_i);
if (display) DISPLAY_INT32_RMAT("rmat int", rmat_i);
if (display) DISPLAY_INT32_RMAT_AS_FLOAT("rmat int", rmat_i);
struct FloatQuat qf;
FLOAT_QUAT_OF_RMAT(qf, rmat_f);
FLOAT_QUAT_WRAP_SHORTEST(qf);
if (display) DISPLAY_FLOAT_QUAT("qf", qf);
struct Int32Quat qi;
INT32_QUAT_OF_RMAT(qi, rmat_i);
INT32_QUAT_WRAP_SHORTEST(qi);
if (display) DISPLAY_INT32_QUAT("qi", qi);
if (display) DISPLAY_INT32_QUAT_2("qi", qi);
struct FloatQuat qif;
QUAT_FLOAT_OF_BFP(qif, qi);
struct FloatQuat qerr;
QUAT_DIFF(qerr, qif, qf);
float err_norm = FLOAT_QUAT_NORM(qerr);
if (display) printf("err %f\n", err_norm);
if (display) printf("\n");
return err_norm;
}
