public void ToEulerAnglesTest(double real, double x, double y, double z, double[] expectedAsArray)
{
var quat = new Quaternion(real, x, y, z);
var eulerAngles = quat.ToEulerAngles();
var expected = new EulerAngles(Angle.FromRadians(expectedAsArray[0]), Angle.FromRadians(expectedAsArray[1]), Angle.FromRadians(expectedAsArray[2]));
Assert.AreEqual(expected,eulerAngles);
}
public static List <double> QuaternionToEuler(double w, double x, double y, double z)
{
double sqw = w * w;
double sqx = x * x;
double sqy = y * y;
double sqz = z * z;
// invs (inverse square length) is only required if quaternion is not already normalised
double invs = 1 / (sqx + sqy + sqz + sqw);
double m00 = (sqx - sqy - sqz + sqw) * invs; // since sqw + sqx + sqy + sqz =1/invs*invs
double m11 = (-sqx + sqy - sqz + sqw) * invs;
double m22 = (-sqx - sqy + sqz + sqw) * invs;
double tmp1 = x * y;
double tmp2 = z * w;
double m10 = 2.0 * (tmp1 + tmp2) * invs;
double m01 = 2.0 * (tmp1 - tmp2) * invs;
tmp1 = x * z;
tmp2 = y * w;
double m20 = 2.0 * (tmp1 - tmp2) * invs;
double m02 = 2.0 * (tmp1 + tmp2) * invs;
tmp1 = y * z;
tmp2 = x * w;
double m21 = 2.0 * (tmp1 + tmp2) * invs;
double m12 = 2.0 * (tmp1 - tmp2) * invs;
w = Math.Sqrt(1.0 + m00 + m11 + m22) / 2.0;
double w4 = (4.0 * w);
x = (m21 - m12) / w4;
y = (m02 - m20) / w4;
z = (m10 - m01) / w4;
Q1 q;
if (w < 0)
{
q = new Q1(-w, -x, -y, -z);
}
else
{
q = new Q1(w, x, y, z);
}
EulerAngles e = q.ToEulerAngles();
// Console.WriteLine($"q = [{w}, {x}, {y}, {z}]: rzyx = [{e.Gamma.Radians}, {e.Beta.Radians}, {e.Alpha.Radians}]");
// Console.WriteLine($"q = [{w}, {x}, {y}, {z}]: rzyx = [{e.Gamma.Degrees}, {e.Beta.Degrees}, {e.Alpha.Degrees}]");
return(new List <double>
{
e.Gamma.Degrees, // z
e.Beta.Degrees, // y
e.Alpha.Degrees, // x
});
}
public static List <double> YZXToZYX(double y, double z, double x)
{
z = z * Math.PI / 180.0;
y = y * Math.PI / 180.0;
x = x * Math.PI / 180.0;
double c1 = Math.Cos(y), s1 = Math.Sin(y);
double c2 = Math.Cos(z), s2 = Math.Sin(z);
double c3 = Math.Cos(x), s3 = Math.Sin(x);
double m00 = c1 * c2;
double m01 = s1 * s3 - c1 * c3 * s2;
double m02 = c3 * s1 + c1 * s2 * s3;
double m10 = s2;
double m11 = c2 * c3;
double m12 = -c2 * s3;
double m20 = -c2 * s1;
double m21 = c1 * s3 + c3 * s1 * s2;
double m22 = c1 * c3 - s1 * s2 * s3;
double w = Math.Sqrt(1.0 + m00 + m11 + m22) / 2.0;
double w4 = (4.0 * w);
x = (m21 - m12) / w4;
y = (m02 - m20) / w4;
z = (m10 - m01) / w4;
Q1 q;
if (w < 0)
{
q = new Q1(-w, -x, -y, -z);
}
else
{
q = new Q1(w, x, y, z);
}
EulerAngles e = q.ToEulerAngles();
return(new List <double>
{
e.Gamma.Degrees, // z
e.Beta.Degrees, // y
e.Alpha.Degrees, // x
});
}
