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