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