SOC_Vector3 Math::QuaternionToEuler(SOC_Quaternion &quaternion)
{
const float Epsilon = 0.0009765625f;
const float Threshold = 0.5f - Epsilon;
float yaw;
float pitch;
float roll;
float XY = quaternion.x * quaternion.y;
float ZW = quaternion.z * quaternion.w;
float TEST = XY + ZW;
if (TEST < -Threshold || TEST > Threshold)
{
int sign = TEST > 0 ? 1 : TEST == 0 ? 0 : -1;
yaw = sign * 2 * (float)atan2(quaternion.x, quaternion.w);
pitch = sign * SOCM_PI / 2.0f;
roll = 0;
}
else
{
float XX = quaternion.x * quaternion.x;
float XZ = quaternion.x * quaternion.z;
float XW = quaternion.x * quaternion.w;
float YY = quaternion.y * quaternion.y;
float YW = quaternion.y * quaternion.w;
float YZ = quaternion.y * quaternion.z;
float ZZ = quaternion.z * quaternion.z;
yaw = (float)atan2(2 * YW - 2 * XZ, 1 - 2 * YY - 2 * ZZ);
pitch = (float)atan2(2 * XW - 2 * YZ, 1 - 2 * XX - 2 * ZZ);
roll = (float)asin(2 * TEST);
}
return SOC_Vector3(pitch, yaw, roll);
}
Sphere::Sphere(void)
{
center = SOC_Vector3(0, 0, 0);
radius = 0.0f;
}
