// function returns an approximation to angle(deg)=acos(x) for x in the range -1 <= x <= 1
// and returns 0 <= angle <= 180 deg
// maximum error is 14.67E-6 deg
float facos_deg(float x)
{
// for robustness, check for invalid arguments
if (x >= 1.0F) return 0.0F;
if (x <= -1.0F) return 180.0F;
// call the atan which will return an angle in the incorrect range -90 to 90 deg
// these lines cannot fail from division by zero or negative square root
if (x == 0.0F) return 90.0F;
if (x > 0.0F) return fatan_deg((sqrtf(1.0F - x * x) / x));
return 180.0F + fatan_deg((sqrtf(1.0F - x * x) / x));
}
// function returns an approximation to angle(deg)=asin(x) for x in the range -1 <= x <= 1
// and returns -90 <= angle <= 90 deg
// maximum error is 10.29E-6 deg
float fasin_deg(float x)
{
// for robustness, check for invalid argument
if (x >= 1.0F) return 90.0F;
if (x <= -1.0F) return -90.0F;
// call the atan which will return an angle in the correct range -90 to 90 deg
// this line cannot fail from division by zero or negative square root since |x| < 1
return (fatan_deg(x / sqrtf(1.0F - x * x)));
}
// function returns approximate atan2 angle in range -180 to 180 deg
// maximum error is 14.58E-6 deg
float fatan2_deg(float y, float x)
{
// check for zero x to avoid division by zero
if (x == 0.0F)
{
// return 90 deg for positive y
if (y > 0.0F) return 90.0F;
// return -90 deg for negative y
if (y < 0.0F) return -90.0F;
// otherwise y= 0.0 and return 0 deg (invalid arguments)
return 0.0F;
}
// from here onwards, x is guaranteed to be non-zero
// compute atan2 for quadrant 1 (0 to 90 deg) and quadrant 4 (-90 to 0 deg)
if (x > 0.0F) return (fatan_deg(y / x));
// compute atan2 for quadrant 2 (90 to 180 deg)
if ((x < 0.0F) && (y > 0.0F)) return (180.0F + fatan_deg(y / x));
// compute atan2 for quadrant 3 (-180 to -90 deg)
return (-180.0F + fatan_deg(y / x));
}
// extract the Windows 8 angles in degrees from the Windows 8 rotation matrix
void fWin8AnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg,
float *pfRhoDeg, float *pfChiDeg)
{
// calculate the roll angle -90.0 <= Phi <= 90.0 deg
if (R[Z][Z] == 0.0F)
{
if (R[X][Z] >= 0.0F)
{
// tan(phi) is -infinity
*pfPhiDeg = -90.0F;
}
else
{
// tan(phi) is +infinity
*pfPhiDeg = 90.0F;
}
}
else
{
// general case
*pfPhiDeg = fatan_deg(-R[X][Z] / R[Z][Z]);
}
// first calculate the pitch angle The in the range -90.0 <= The <= 90.0 deg
*pfTheDeg = fasin_deg(R[Y][Z]);
// use R[Z][Z]=cos(Phi)*cos(The) to correct the quadrant of The remembering
// cos(Phi) is non-negative so that cos(The) has the same sign as R[Z][Z].
if (R[Z][Z] < 0.0F)
{
// wrap The around +90 deg and -90 deg giving result 90 to 270 deg
*pfTheDeg = 180.0F - *pfTheDeg;
}
// map the pitch angle The to the range -180.0 <= The < 180.0 deg
if (*pfTheDeg >= 180.0F)
{
*pfTheDeg -= 360.0F;
}
// calculate the yaw angle Psi
if (*pfTheDeg == 90.0F)
{
// vertical upwards gimbal lock case: -270 <= Psi < 90 deg
*pfPsiDeg = fatan2_deg(R[X][Y], R[X][X]) - *pfPhiDeg;
}
else if (*pfTheDeg == -90.0F)
{
// vertical downwards gimbal lock case: -270 <= Psi < 90 deg
*pfPsiDeg = fatan2_deg(R[X][Y], R[X][X]) + *pfPhiDeg;
}
else
{
// general case: -180 <= Psi < 180 deg
*pfPsiDeg = fatan2_deg(-R[Y][X], R[Y][Y]);
// correct the quadrant for Psi using the value of The (deg) to give -180 <= Psi < 380 deg
if (fabs(*pfTheDeg) >= 90.0F)
{
*pfPsiDeg += 180.0F;
}
}
// map yaw angle Psi onto range 0.0 <= Psi < 360.0 deg
if (*pfPsiDeg < 0.0F)
{
*pfPsiDeg += 360.0F;
}
// check for any rounding error mapping small negative angle to 360 deg
if (*pfPsiDeg >= 360.0F)
{
*pfPsiDeg = 0.0F;
}
// compute the compass angle Rho = 360 - Psi
*pfRhoDeg = 360.0F - *pfPsiDeg;
// check for rounding errors mapping small negative angle to 360 deg and zero degree case
if (*pfRhoDeg >= 360.0F)
{
*pfRhoDeg = 0.0F;
}
// calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
*pfChiDeg = facos_deg(R[Z][Z]);
return;
}
