private teVec3 EulerFromQuat(int i, int j, int k, int h, EulerParity parity, EulerRepeat repeat, EulerFrame frame) { double[,] mat = new double[4, 4]; double num1 = X * (double)X + Y * (double)Y + Z * (double)Z + W * (double)W; double num2 = num1 <= 0.0 ? 0.0 : 2.0 / num1; double num3 = X * num2; double num4 = Y * num2; double num5 = Z * num2; double num6 = W * num3; double num7 = W * num4; double num8 = W * num5; double num9 = X * num3; double num10 = X * num4; double num11 = X * num5; double num12 = Y * num4; double num13 = Y * num5; double num14 = Z * num5; mat[0, 0] = 1.0 - (num12 + num14); mat[0, 1] = num10 - num8; mat[0, 2] = num11 + num7; mat[1, 0] = num10 + num8; mat[1, 1] = 1.0 - (num9 + num14); mat[1, 2] = num13 - num6; mat[2, 0] = num11 - num7; mat[2, 1] = num13 + num6; mat[2, 2] = 1.0 - (num9 + num12); mat[3, 3] = 1.0; return(EulerFromHMatrix(mat, i, j, k, h, parity, repeat, frame)); }
private static Vector3 Eul_FromQuat(Quaternion q, int i, int j, int k, int h, EulerParity parity, EulerRepeat repeat, EulerFrame frame) { float[,] M = { { 0, 0, 0, 0 }, { 0, 0, 0, 0 }, { 0, 0, 0, 0 }, { 0, 0, 0, 0 } }; float Nq = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w; float s; if (Nq > 0) { s = 2.0f / Nq; } else { s = 0; } float xs = q.x * s; float ys = q.y * s; float zs = q.z * s; float wx = q.w * xs; float wy = q.w * ys; float wz = q.w * zs; float xx = q.x * xs; float xy = q.x * ys; float xz = q.x * zs; float yy = q.y * ys; float yz = q.y * zs; float zz = q.z * zs; M[0, 0] = 1.0f - (yy + zz); M[0, 1] = xy - wz; M[0, 2] = xz + wy; M[1, 0] = xy + wz; M[1, 1] = 1.0f - (xx + zz); M[1, 2] = yz - wx; M[2, 0] = xz - wy; M[2, 1] = yz + wx; M[2, 2] = 1.0f - (xx + yy); M[3, 3] = 1.0f; return(Eul_FromHMatrix(M, i, j, k, h, parity, repeat, frame)); }
private static Vector3 Eul_FromHMatrix(float[,] M, int i, int j, int k, int h, EulerParity parity, EulerRepeat repeat, EulerFrame frame) { Vector3 ea = new Vector3(); if (repeat == EulerRepeat.Yes) { float sy = Mathf.Sqrt(M[i, j] * M[i, j] + M[i, k] * M[i, k]); if (sy > 16 * Mathf.Epsilon) { ea.x = Mathf.Atan2(M[i, j], M[i, k]); ea.y = Mathf.Atan2(sy, M[i, i]); ea.z = Mathf.Atan2(M[j, i], -M[k, i]); } else { ea.x = Mathf.Atan2(-M[j, k], M[j, j]); ea.y = Mathf.Atan2(sy, M[i, i]); ea.z = 0; } } else { float cy = Mathf.Sqrt(M[i, i] * M[i, i] + M[j, i] * M[j, i]); if (cy > 16 * Mathf.Epsilon) { ea.x = Mathf.Atan2(M[k, j], M[k, k]); ea.y = Mathf.Atan2(-M[k, i], cy); ea.z = Mathf.Atan2(M[j, i], M[i, i]); } else { ea.x = Mathf.Atan2(-M[j, k], M[j, j]); ea.y = Mathf.Atan2(-M[k, i], cy); ea.z = 0; } } if (parity == EulerParity.Odd) { ea.x = -ea.x; ea.y = -ea.y; ea.z = -ea.z; } if (frame == EulerFrame.R) { float t = ea.x; ea.x = ea.z; ea.z = t; } //ea.w = order return(ea); }
private static Vector3D Eul_FromHMatrix(double[,] M, int i, int j, int k, int h, EulerParity parity, EulerRepeat repeat, EulerFrame frame) { Vector3D vector3D = new Vector3D(); if (repeat == EulerRepeat.Yes) { double y = Math.Sqrt(M[i, j] * M[i, j] + M[i, k] * M[i, k]); if (y > 0.00016) { vector3D.X = (float)Math.Atan2(M[i, j], M[i, k]); vector3D.Y = (float)Math.Atan2(y, M[i, i]); vector3D.Z = (float)Math.Atan2(M[j, i], -M[k, i]); } else { vector3D.X = (float)Math.Atan2(-M[j, k], M[j, j]); vector3D.Y = (float)Math.Atan2(y, M[i, i]); vector3D.Z = 0.0f; } } else { double x = Math.Sqrt(M[i, i] * M[i, i] + M[j, i] * M[j, i]); if (x > 0.00016) { vector3D.X = (float)Math.Atan2(M[k, j], M[k, k]); vector3D.Y = (float)Math.Atan2(-M[k, i], x); vector3D.Z = (float)Math.Atan2(M[j, i], M[i, i]); } else { vector3D.X = (float)Math.Atan2(-M[j, k], M[j, j]); vector3D.Y = (float)Math.Atan2(-M[k, i], x); vector3D.Z = 0.0f; } } if (parity == EulerParity.Odd) { vector3D.X = -vector3D.X; vector3D.Y = -vector3D.Y; vector3D.Z = -vector3D.Z; } if (frame == EulerFrame.R) { double x = (double)vector3D.X; vector3D.X = vector3D.Z; vector3D.Z = (float)x; } return(vector3D); }
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)); }
private static teVec3 EulerFromHMatrix(double[,] mat, int i, int j, int k, int h, EulerParity parity, EulerRepeat repeat, EulerFrame frame) { teVec3 vec3 = new teVec3(); if (repeat == EulerRepeat.Yes) { double y = Sqrt(mat[i, j] * mat[i, j] + mat[i, k] * mat[i, k]); if (y > 0.00016) { vec3.X = (float)Atan2(mat[i, j], mat[i, k]); vec3.Y = (float)Atan2(y, mat[i, i]); vec3.Z = (float)Atan2(mat[j, i], -mat[k, i]); } else { vec3.X = (float)Atan2(-mat[j, k], mat[j, j]); vec3.Y = (float)Atan2(y, mat[i, i]); vec3.Z = 0.0f; } } else { double x = Sqrt(mat[i, i] * mat[i, i] + mat[j, i] * mat[j, i]); if (x > 0.00016) { vec3.X = (float)Atan2(mat[k, j], mat[k, k]); vec3.Y = (float)Atan2(-mat[k, i], x); vec3.Z = (float)Atan2(mat[j, i], mat[i, i]); } else { vec3.X = (float)Atan2(-mat[j, k], mat[j, j]); vec3.Y = (float)Atan2(-mat[k, i], x); vec3.Z = 0.0f; } } if (parity == EulerParity.Odd) { vec3.X = -vec3.X; vec3.Y = -vec3.Y; vec3.Z = -vec3.Z; } if (frame == EulerFrame.R) { double x = vec3.X; vec3.X = vec3.Z; vec3.Z = (float)x; } // ReSharper disable CompareOfFloatsByEqualityOperator if (vec3.X == -3.14159274f && vec3.Y == 0 && vec3.Z == 0) { vec3 = new teVec3(0, 3.14159274f, 3.14159274f); // effectively the same but you know, eulers. } // ReSharper restore CompareOfFloatsByEqualityOperator return(vec3); }