void orientationCalcQuat_i(struct OrientationReps* orientation) { if (bit_is_set(orientation->status, ORREP_QUAT_I)) return; if (bit_is_set(orientation->status, ORREP_QUAT_F)) { QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f); } else if (bit_is_set(orientation->status, ORREP_RMAT_I)) { INT32_QUAT_OF_RMAT(orientation->quat_i, orientation->rmat_i); } else if (bit_is_set(orientation->status, ORREP_EULER_I)) { INT32_QUAT_OF_EULERS(orientation->quat_i, orientation->eulers_i); } else if (bit_is_set(orientation->status, ORREP_RMAT_F)) { RMAT_BFP_OF_REAL(orientation->rmat_i, orientation->rmat_f); SetBit(orientation->status, ORREP_RMAT_I); INT32_QUAT_OF_RMAT(orientation->quat_i, orientation->rmat_i); } else if (bit_is_set(orientation->status, ORREP_EULER_F)) { EULERS_BFP_OF_REAL(orientation->eulers_i, orientation->eulers_f); SetBit(orientation->status, ORREP_EULER_I); INT32_QUAT_OF_EULERS(orientation->quat_i, orientation->eulers_i); } /* set bit to indicate this representation is computed */ SetBit(orientation->status, ORREP_QUAT_I); }
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; }