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_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; }