/// <summary>
///
/// </summary>
/// <param name="euler"></param>
/// <returns></returns>
public static Matrix4 MakeRotationFromEuler(Euler euler)
{
var m = new Matrix4();
var x = euler.X; var y = euler.Y; var z = euler.Z;
var a = (float)Math.Cos(x); var b = (float)Math.Sin(x);
var c = (float)Math.Cos(y); var d = (float)Math.Sin(y);
var e = (float)Math.Cos(z); var f = (float)Math.Sin(z);
if (euler.Order == Euler.RotationOrders.XYZ)
{
var ae = a * e; var af = a * f; var be = b * e; var bf = b * f;
m.M11 = c * e;
m.M21 = -c * f;
m.M31 = d;
m.M12 = af + be * d;
m.M22 = ae - bf * d;
m.M32 = -b * c;
m.M13 = bf - ae * d;
m.M23 = be + af * d;
m.M33 = a * c;
}
else if (euler.Order == Euler.RotationOrders.YXZ)
{
throw new NotImplementedException();
}
else if (euler.Order == Euler.RotationOrders.ZXY)
{
throw new NotImplementedException();
}
else if (euler.Order == Euler.RotationOrders.ZYX)
{
throw new NotImplementedException();
}
else if (euler.Order == Euler.RotationOrders.YZX)
{
throw new NotImplementedException();
}
else if (euler.Order == Euler.RotationOrders.XZY)
{
throw new NotImplementedException();
}
// last column
m.M14 = 0;
m.M24 = 0;
m.M34 = 0;
// bottom row
m.M41 = 0;
m.M42 = 0;
m.M43 = 0;
m.M44 = 1;
return(m);
}
/// <summary>
///
/// </summary>
/// <param name="euler"></param>
/// <returns></returns>
public Quaternion SetFromEuler(Euler euler)
{
var q = this;
var c1 = (float)System.Math.Cos(euler.X / 2);
var c2 = (float)System.Math.Cos(euler.Y / 2);
var c3 = (float)System.Math.Cos(euler.Z / 2);
var s1 = (float)System.Math.Sin(euler.X / 2);
var s2 = (float)System.Math.Sin(euler.Y / 2);
var s3 = (float)System.Math.Sin(euler.Z / 2);
if (euler.Order == Euler.RotationOrder.XYZ)
{
q.x = s1 * c2 * c3 + c1 * s2 * s3;
q.y = c1 * s2 * c3 - s1 * c2 * s3;
q.z = c1 * c2 * s3 + s1 * s2 * c3;
q.w = c1 * c2 * c3 - s1 * s2 * s3;
}
else if (euler.Order == Euler.RotationOrder.YXZ)
{
q.x = s1 * c2 * c3 + c1 * s2 * s3;
q.y = c1 * s2 * c3 - s1 * c2 * s3;
q.z = c1 * c2 * s3 - s1 * s2 * c3;
q.w = c1 * c2 * c3 + s1 * s2 * s3;
}
else if (euler.Order == Euler.RotationOrder.ZXY)
{
q.x = s1 * c2 * c3 - c1 * s2 * s3;
q.y = c1 * s2 * c3 + s1 * c2 * s3;
q.z = c1 * c2 * s3 + s1 * s2 * c3;
q.w = c1 * c2 * c3 - s1 * s2 * s3;
}
else if (euler.Order == Euler.RotationOrder.ZYX)
{
q.x = s1 * c2 * c3 - c1 * s2 * s3;
q.y = c1 * s2 * c3 + s1 * c2 * s3;
q.z = c1 * c2 * s3 - s1 * s2 * c3;
q.w = c1 * c2 * c3 + s1 * s2 * s3;
}
else if (euler.Order == Euler.RotationOrder.YZX)
{
q.x = s1 * c2 * c3 + c1 * s2 * s3;
q.y = c1 * s2 * c3 + s1 * c2 * s3;
q.z = c1 * c2 * s3 - s1 * s2 * c3;
q.w = c1 * c2 * c3 - s1 * s2 * s3;
}
else if (euler.Order == Euler.RotationOrder.XZY)
{
q.x = s1 * c2 * c3 - c1 * s2 * s3;
q.y = c1 * s2 * c3 - s1 * c2 * s3;
q.z = c1 * c2 * s3 + s1 * s2 * c3;
q.w = c1 * c2 * c3 + s1 * s2 * s3;
}
this.OnPropertyChanged();
return(q);
}
/// <summary>
///
/// </summary>
/// <param name="q"></param>
/// <param name="order"></param>
/// <param name="update"></param>
/// <returns></returns>
public static Euler SetFromQuaternion(Quaternion q, Euler.RotationOrders order = Euler.RotationOrders.XYZ, bool update = false)
{
var euler = new Euler();
// q is assumed to be normalized
// http://www.mathworks.com/matlabcentral/fileexchange/20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors/content/SpinCalc.m
var sqx = q.X * q.X;
var sqy = q.Y * q.Y;
var sqz = q.Z * q.Z;
var sqw = q.W * q.W;
if (order == Euler.RotationOrders.XYZ)
{
euler.X = (float)Math.Atan2(2 * (q.X * q.W - q.Y * q.Z), (sqw - sqx - sqy + sqz));
euler.Y = (float)Math.Asin((2 * (q.X * q.Z + q.Y * q.W).Clamp(-1, 1)));
euler.Z = (float)Math.Atan2(2 * (q.Z * q.W - q.X * q.Y), (sqw + sqx - sqy - sqz));
}
else if (order == Euler.RotationOrders.YXZ)
{
euler.X = (float)Math.Asin((2 * (q.X * q.W - q.Y * q.Z).Clamp(-1, 1)));
euler.Y = (float)Math.Atan2(2 * (q.X * q.Z + q.Y * q.W), (sqw - sqx - sqy + sqz));
euler.Z = (float)Math.Atan2(2 * (q.X * q.Y + q.Z * q.W), (sqw - sqx + sqy - sqz));
}
else if (order == Euler.RotationOrders.ZXY)
{
euler.X = (float)Math.Asin((2 * (q.X * q.W + q.Y * q.Z).Clamp(-1, 1)));
euler.Y = (float)Math.Atan2(2 * (q.Y * q.W - q.Z * q.X), (sqw - sqx - sqy + sqz));
euler.Z = (float)Math.Atan2(2 * (q.Z * q.W - q.X * q.Y), (sqw - sqx + sqy - sqz));
}
else if (order == Euler.RotationOrders.ZYX)
{
euler.X = (float)Math.Atan2(2 * (q.X * q.W + q.Z * q.Y), (sqw - sqx - sqy + sqz));
euler.Y = (float)Math.Asin((2 * (q.Y * q.W - q.X * q.Z).Clamp(-1, 1)));
euler.Z = (float)Math.Atan2(2 * (q.X * q.Y + q.Z * q.W), (sqw + sqx - sqy - sqz));
}
else if (order == Euler.RotationOrders.YZX)
{
euler.X = (float)Math.Atan2(2 * (q.X * q.W - q.Z * q.Y), (sqw - sqx + sqy - sqz));
euler.Y = (float)Math.Atan2(2 * (q.Y * q.W - q.X * q.Z), (sqw + sqx - sqy - sqz));
euler.Z = (float)Math.Asin((2 * (q.X * q.Y + q.Z * q.W).Clamp(-1, 1)));
}
else if (order == Euler.RotationOrders.XZY)
{
euler.X = (float)Math.Atan2(2 * (q.X * q.W + q.Y * q.Z), (sqw - sqx + sqy - sqz));
euler.Y = (float)Math.Atan2(2 * (q.X * q.Z + q.Y * q.W), (sqw + sqx - sqy - sqz));
euler.Z = (float)Math.Asin((2 * (q.Z * q.W - q.X * q.Y).Clamp(-1, 1)));
}
euler.Order = order;
// if ( update !== false ) this.onChangeCallback();
return(euler);
}
public Vector4 ApplyEuler(Euler Euler)
{
this.ApplyQuaternion(new Quaternion().SetFromEuler(Euler));
return(this);
}
/// <summary>
///
/// </summary>
/// <returns></returns>
public Matrix4 MakeRotationFromEuler(Euler euler)
{
var x = euler.X; var y = euler.Y; var z = euler.Z;
var a = (float)Math.Cos(x); var b = (float)Math.Sin(x);
var c = (float)Math.Cos(y); var d = (float)Math.Sin(y);
var e = (float)Math.Cos(z); var f = (float)Math.Sin(z);
if (euler.Order == Euler.RotationOrder.XYZ)
{
var ae = a * e; var af = a * f; var be = b * e; var bf = b * f;
this.Elements[0] = c * e;
this.Elements[4] = -c * f;
this.Elements[8] = d;
this.Elements[1] = af + be * d;
this.Elements[5] = ae - bf * d;
this.Elements[9] = -b * c;
this.Elements[2] = bf - ae * d;
this.Elements[6] = be + af * d;
this.Elements[10] = a * c;
}
else if (euler.Order == Euler.RotationOrder.YXZ)
{
var ce = c * e; var cf = c * f; var de = d * e; var df = d * f;
this.Elements[0] = ce + df * b;
this.Elements[4] = de * b - cf;
this.Elements[8] = a * d;
this.Elements[1] = a * f;
this.Elements[5] = a * e;
this.Elements[9] = -b;
this.Elements[2] = cf * b - de;
this.Elements[6] = df + ce * b;
this.Elements[10] = a * c;
}
else if (euler.Order == Euler.RotationOrder.ZXY)
{
var ce = c * e; var cf = c * f; var de = d * e; var df = d * f;
this.Elements[0] = ce - df * b;
this.Elements[4] = -a * f;
this.Elements[8] = de + cf * b;
this.Elements[1] = cf + de * b;
this.Elements[5] = a * e;
this.Elements[9] = df - ce * b;
this.Elements[2] = -a * d;
this.Elements[6] = b;
this.Elements[10] = a * c;
}
else if (euler.Order == Euler.RotationOrder.ZYX)
{
var ae = a * e; var af = a * f; var be = b * e; var bf = b * f;
this.Elements[0] = c * e;
this.Elements[4] = be * d - af;
this.Elements[8] = ae * d + bf;
this.Elements[1] = c * f;
this.Elements[5] = bf * d + ae;
this.Elements[9] = af * d - be;
this.Elements[2] = -d;
this.Elements[6] = b * c;
this.Elements[10] = a * c;
}
else if (euler.Order == Euler.RotationOrder.YZX)
{
var ac = a * c; var ad = a * d; var bc = b * c; var bd = b * d;
this.Elements[0] = c * e;
this.Elements[4] = bd - ac * f;
this.Elements[8] = bc * f + ad;
this.Elements[1] = f;
this.Elements[5] = a * e;
this.Elements[9] = -b * e;
this.Elements[2] = -d * e;
this.Elements[6] = ad * f + bc;
this.Elements[10] = ac - bd * f;
}
else if (euler.Order == Euler.RotationOrder.XZY)
{
var ac = a * c; var ad = a * d; var bc = b * c; var bd = b * d;
this.Elements[0] = c * e;
this.Elements[4] = -f;
this.Elements[8] = d * e;
this.Elements[1] = ac * f + bd;
this.Elements[5] = a * e;
this.Elements[9] = ad * f - bc;
this.Elements[2] = bc * f - ad;
this.Elements[6] = b * e;
this.Elements[10] = bd * f + ac;
}
// last column
this.Elements[3] = 0;
this.Elements[7] = 0;
this.Elements[11] = 0;
// bottom row
this.Elements[12] = 0;
this.Elements[13] = 0;
this.Elements[14] = 0;
this.Elements[15] = 1;
return(this);
}
