forums.naturalpoint.com

Code: Select all

void GetEulers(float qx, float qy, float qz, float qw, float *angle1,float *angle2, float *angle3)
{
    float &heading = *angle1;
    float &attitude = *angle2;
    float &bank = *angle3;

	double test = qx*qy + qz*qw;
	if (test > 0.499) { // singularity at north pole
		heading = (float) 2.0f * atan2(qx,qw);
		attitude = 3.14159265f/2.0f;
		bank = 0;

        RadiansToDegrees(&heading);
        RadiansToDegrees(&attitude);
        RadiansToDegrees(&bank);
		return;
	}
	if (test < -0.499) { // singularity at south pole
		heading = (float) -2.0f * atan2(qx,qw);
		attitude = - 3.14159265f/2.0f;
		bank = 0;

        RadiansToDegrees(&heading);
        RadiansToDegrees(&attitude);
        RadiansToDegrees(&bank);
		return;
	}
    double sqx = qx*qx;
    double sqy = qy*qy;
    double sqz = qz*qz;
    heading = (float) atan2((double)2.0*qy*qw-2.0*qx*qz , (double)1 - 2.0*sqy - 2.0*sqz);
	attitude = (float)asin(2.0*test);
	bank = (float) atan2((double)2.0*qx*qw-2.0*qy*qz , (double)1.0 - 2.0*sqx - 2.0*sqz);

    RadiansToDegrees(&heading);
    RadiansToDegrees(&attitude);
    RadiansToDegrees(&bank);
}

void RadiansToDegrees(float *value)
{
    *value = (*value)*(180.0f/3.14159265f);
}