private static Vector3D Eul_FromQuat(Quaternion3D q, int i, int j, int k, int h, EulerParity parity, EulerRepeat repeat, EulerFrame frame) { double[,] M = new double[4, 4]; double num1 = (double)q.i * (double)q.i + (double)q.j * (double)q.j + (double)q.k * (double)q.k + (double)q.real * (double)q.real; double num2 = num1 <= 0.0 ? 0.0 : 2.0 / num1; double num3 = (double)q.i * num2; double num4 = (double)q.j * num2; double num5 = (double)q.k * num2; double num6 = (double)q.real * num3; double num7 = (double)q.real * num4; double num8 = (double)q.real * num5; double num9 = (double)q.i * num3; double num10 = (double)q.i * num4; double num11 = (double)q.i * num5; double num12 = (double)q.j * num4; double num13 = (double)q.j * num5; double num14 = (double)q.k * num5; M[0, 0] = 1.0 - (num12 + num14); M[0, 1] = num10 - num8; M[0, 2] = num11 + num7; M[1, 0] = num10 + num8; M[1, 1] = 1.0 - (num9 + num14); M[1, 2] = num13 - num6; M[2, 0] = num11 - num7; M[2, 1] = num13 + num6; M[2, 2] = 1.0 - (num9 + num12); M[3, 3] = 1.0; return(Eul_FromHMatrix(M, i, j, k, h, parity, repeat, frame)); }
public static Vector3D Multiply(Vector3D vec, Quaternion3D q) { float num1 = Convert.ToSingle(2) * (float)((double)q.i * (double)vec.X + (double)q.j * (double)vec.Y + (double)q.k * (double)vec.Z); float num2 = Convert.ToSingle(2) * q.real; float num3 = num2 * q.real - Convert.ToSingle(1); return(new Vector3D((float)((double)num3 * (double)vec.X + (double)num1 * (double)q.i + (double)num2 * ((double)q.k * (double)vec.Z - (double)q.k * (double)vec.Y)), (float)((double)num3 * (double)vec.Y + (double)num1 * (double)q.j + (double)num2 * ((double)q.k * (double)vec.X - (double)q.i * (double)vec.Z)), (float)((double)num3 * (double)vec.Z + (double)num1 * (double)q.k + (double)num2 * ((double)q.i * (double)vec.Y - (double)q.j * (double)vec.X)))); }
public static Vector3D ToEulerAngles(Quaternion3D q) { return(Eul_FromQuat(q, 0, 1, 2, 0, EulerParity.Even, EulerRepeat.No, EulerFrame.S)); }