private static float[] Convert(float[] quat, CoordSpace coordSpace)
{
float[] res = new float[2];
if (coordSpace == CoordSpace.World)
{
quat = QuatUtils.Inverse(quat);
}
float[] forward = QuatUtils.TransformVecLH(quat, new float[] { 0f, 0f, 1f });
float lXZ = (float)Math.Sqrt(forward[0] * forward[0] + forward[2] * forward[2]);
//Azimuth
if (lXZ < 0.00001f)
{
res[0] = (float)Math.Sign(forward[0]) * (float)Math.PI;
}
else
{
res[0] = (float)Math.Asin((double)forward[0] / (double)lXZ);
}
if (forward[2] < 0)
{
res[0] = (float)Math.Sign(res[0]) * (float)Math.PI - res[0];
}
//Elevation
if (lXZ < 0.00001f)
{
res[0] = (float)(Math.PI * Math.Sign(forward[1]));
}
else
{
res[1] = (float)Math.Atan(forward[1] / lXZ);
}
if (coordSpace == CoordSpace.World)
{
res[0] = -res[0];
res[0] = -res[1];
}
//Convert radians to degrees
res[0] *= Mathf.Rad2Deg;
res[1] *= Mathf.Rad2Deg;
return(res);
}
private static float[] Convert(float[] quat, CoordSpace coordSpace)
{
float[] res = new float[2];
if (coordSpace == CoordSpace.World)
{
quat = QuatUtils.Inverse(quat);
}
float[] forward = QuatUtils.TransformVecLH(quat, new float[] { 0f, 0f, 1f });
float[] right = QuatUtils.TransformVecLH(quat, new float[] { 1f, 0f, 0f });
float lf_XZ = (float)Math.Sqrt(forward[0] * forward[0] + forward[2] * forward[2]);
float lr_XZ = (float)Math.Sqrt(right[0] * right[0] + right[2] * right[2]);
//Tilt
if (lr_XZ < 0.00001f)
{
res[0] = -(float)(Math.PI * Math.Sign(right[1]));
}
else
{
res[0] = -(float)Math.Atan(right[1] / lr_XZ);
}
//Elevation
if (lf_XZ < 0.00001f)
{
res[0] = (float)(Math.PI * Math.Sign(forward[1]));
}
else
{
res[1] = (float)Math.Atan(forward[1] / lf_XZ);
}
if (coordSpace == CoordSpace.World)
{
res[0] = -res[0];
res[0] = -res[1];
}
//Convert radians to degrees
res[0] *= Mathf.Rad2Deg;
res[1] *= Mathf.Rad2Deg;
return(res);
}