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);