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; }
void orientationCalcRMat_i(struct OrientationReps* orientation) { if (bit_is_set(orientation->status, ORREP_RMAT_I)) return; if (bit_is_set(orientation->status, ORREP_RMAT_F)) { RMAT_BFP_OF_REAL(orientation->rmat_i, orientation->rmat_f); } else if (bit_is_set(orientation->status, ORREP_QUAT_I)) { INT32_RMAT_OF_QUAT(orientation->rmat_i, orientation->quat_i); } else if (bit_is_set(orientation->status, ORREP_EULER_I)) { INT32_RMAT_OF_EULERS(orientation->rmat_i, orientation->eulers_i); } else if (bit_is_set(orientation->status, ORREP_QUAT_F)) { QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f); SetBit(orientation->status, ORREP_QUAT_I); INT32_RMAT_OF_QUAT(orientation->rmat_i, orientation->quat_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_RMAT_OF_EULERS(orientation->rmat_i, orientation->eulers_i); } /* set bit to indicate this representation is computed */ SetBit(orientation->status, ORREP_RMAT_I); }
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 orientationCalcEulers_i(struct OrientationReps* orientation) { if (bit_is_set(orientation->status, ORREP_EULER_I)) { return; } if (bit_is_set(orientation->status, ORREP_EULER_F)) { EULERS_BFP_OF_REAL(orientation->eulers_i, orientation->eulers_f); } else if (bit_is_set(orientation->status, ORREP_RMAT_I)) { int32_eulers_of_rmat(&(orientation->eulers_i), &(orientation->rmat_i)); } else if (bit_is_set(orientation->status, ORREP_QUAT_I)) { int32_eulers_of_quat(&(orientation->eulers_i), &(orientation->quat_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_eulers_of_rmat(&(orientation->eulers_i), &(orientation->rmat_i)); } else if (bit_is_set(orientation->status, ORREP_QUAT_F)) { QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f); SetBit(orientation->status, ORREP_QUAT_I); int32_eulers_of_quat(&(orientation->eulers_i), &(orientation->quat_i)); } /* set bit to indicate this representation is computed */ SetBit(orientation->status, ORREP_EULER_I); }