That function enters into numerical instability when
sin_tilt_sin_psi==0, which produces psi ==0. I've tried to fixe it,
and i think is correct, but now the code throws the exception. The
input and output are "apart". Can anybody help me to fix it? Anything
that I missed?
double psi= std::atan2(sin_tilt_sin_psi, cos_psi_sin_tilt);
if(almost_equal(sin_tilt_sin_psi,0,1e-6)) {
double sin_tilt= 0;
double tilt= std::atan2(sin_tilt, cos_tilt);
} else {
double sin_tilt= sin_tilt_sin_psi/std::sin(psi);
double tilt= std::atan2(sin_tilt, cos_tilt);
}
// double cos_rot= cos_rot_sin_tilt/sin_tilt;
// double sin_rot= sin_rot_sin_tilt/sin_tilt;
// double rot= std::atan2(sin_rot, cos_rot);
double rot= std::atan2(sin_rot, cos_rot);