//https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
public FixQuaternion(FixVec3 e)
{
w = FixMath.Cos(e._z / 2) * FixMath.Cos(e._y / 2) * FixMath.Cos(e._x / 2) + FixMath.Sin(e._z / 2) * FixMath.Sin(e._y / 2) * FixMath.Sin(e._x / 2);
x = FixMath.Cos(e._z / 2) * FixMath.Cos(e._y / 2) * FixMath.Sin(e._x / 2) - FixMath.Sin(e._z / 2) * FixMath.Sin(e._y / 2) * FixMath.Cos(e._x / 2);
y = FixMath.Sin(e._z / 2) * FixMath.Cos(e._y / 2) * FixMath.Sin(e._x / 2) + FixMath.Cos(e._z / 2) * FixMath.Sin(e._y / 2) * FixMath.Cos(e._x / 2);
z = FixMath.Sin(e._z / 2) * FixMath.Cos(e._y / 2) * FixMath.Cos(e._x / 2) - FixMath.Cos(e._z / 2) * FixMath.Sin(e._y / 2) * FixMath.Sin(e._x / 2);
}
//https://math.stackexchange.com/questions/2975109/how-to-convert-euler-angles-to-quaternions-and-get-the-same-euler-angles-back-fr
public FixQuaternion(FixVec3 e)
{
x = FixMath.Cos(e.z / 2) * FixMath.Cos(e.y / 2) * FixMath.Sin(e.x / 2) - FixMath.Sin(e.z / 2) * FixMath.Sin(e.y / 2) * FixMath.Cos(e.x / 2);
y = FixMath.Sin(e.z / 2) * FixMath.Cos(e.y / 2) * FixMath.Sin(e.x / 2) + FixMath.Cos(e.z / 2) * FixMath.Sin(e.y / 2) * FixMath.Cos(e.x / 2);
z = FixMath.Sin(e.z / 2) * FixMath.Cos(e.y / 2) * FixMath.Cos(e.x / 2) - FixMath.Cos(e.z / 2) * FixMath.Sin(e.y / 2) * FixMath.Sin(e.x / 2);
w = FixMath.Cos(e.z / 2) * FixMath.Cos(e.y / 2) * FixMath.Cos(e.x / 2) + FixMath.Sin(e.z / 2) * FixMath.Sin(e.y / 2) * FixMath.Sin(e.x / 2);
}
public FixTrans2(FixVec2 position, FixVec2 scale, Fix rotation)
{
Fix cos = FixMath.Cos(rotation);
Fix sin = FixMath.Sin(rotation);
_m11 = cos * scale.X; _m12 = -sin * scale.X; _m13 = position.X;
_m21 = sin * scale.Y; _m22 = cos * scale.Y; _m23 = position.Y;
}
public static FixTrans2 MakeRotation(Fix degrees)
{
Fix cos = FixMath.Cos(degrees);
Fix sin = FixMath.Sin(degrees);
return(new FixTrans2(
cos, -sin, 0,
sin, cos, 0
));
}
public static FixTrans3 MakeRotationX(Fix degrees)
{
Fix cos = FixMath.Cos(degrees);
Fix sin = FixMath.Sin(degrees);
return(new FixTrans3(
1, 0, 0, 0,
0, cos, -sin, 0,
0, sin, cos, 0,
0, 0, 0, 1
));
}
//https://gamedev.stackexchange.com/questions/50963/how-to-extract-euler-angles-from-transformation-matrix
public FixVec3 EulerAngle()
{
FixVec3 ea = new FixVec3();
ea.x = FixMath.Atan2(-m[1, 2], m[2, 2]);
Fix cosYangle = FixMath.Sqrt(FixMath.Pow(m[0, 0], 2) + FixMath.Pow(m[0, 1], 2));
ea.y = FixMath.Atan2(m[0, 2], cosYangle);
Fix sinXangle = FixMath.Sin(ea.x);
Fix cosXangle = FixMath.Cos(ea.x);
ea.z = FixMath.Atan2((cosXangle * m[1, 0]) + (sinXangle * m[2, 0]), (cosXangle * m[1, 1]) + (sinXangle * m[2, 1]));
return(ea);
}
/// <summary>
/// Build a quaternion from the given axis and angle
/// </summary>
/// <param name="axis">The axis to rotate about</param>
/// <param name="angle">The rotation angle in radians</param>
/// <returns></returns>
public static FixQuaternion FromAxisAngle(FixVec3 axis, Fix angle)
{
if (axis.GetSqrMagnitude() == Fix.Zero)
{
return(Identity);
}
FixQuaternion result = Identity;
angle *= Fix.One / 2;
axis.Normalize();
result.Xyz = axis * FixMath.Sin(angle);
result.W = FixMath.Cos(angle);
return(Normalize(result));
}
/// <summary>
/// Do Spherical linear interpolation between two quaternions
/// </summary>
/// <param name="q1">The first quaternion</param>
/// <param name="q2">The second quaternion</param>
/// <param name="blend">The blend factor</param>
/// <returns>A smooth blend between the given quaternions</returns>
public static FixQuaternion Slerp(FixQuaternion q1, FixQuaternion q2, Fix blend)
{
// if either input is zero, return the other.
if (q1.LengthSquared == Fix.Zero)
{
if (q2.LengthSquared == Fix.Zero)
{
return(Identity);
}
return(q2);
}
else if (q2.LengthSquared == Fix.Zero)
{
return(q1);
}
Fix cosHalfAngle = q1.W * q2.W + q1.Xyz.Dot(q2.Xyz);
if (cosHalfAngle >= Fix.One || cosHalfAngle <= -Fix.One)
{
// angle = 0.0f, so just return one input.
return(q1);
}
else if (cosHalfAngle < Fix.Zero)
{
q2.Xyz = -q2.Xyz;
q2.W = -q2.W;
cosHalfAngle = -cosHalfAngle;
}
Fix blendA;
Fix blendB;
if (cosHalfAngle < (Fix.One / 100) * 99)
{
// do proper slerp for big angles
Fix halfAngle = FixMath.Acos(cosHalfAngle);
Fix sinHalfAngle = FixMath.Sin(halfAngle);
Fix oneOverSinHalfAngle = Fix.One / sinHalfAngle;
blendA = FixMath.Sin(halfAngle * (Fix.One - blend)) * oneOverSinHalfAngle;
blendB = FixMath.Sin(halfAngle * blend) * oneOverSinHalfAngle;
}
else
{
// do lerp if angle is really small.
blendA = Fix.One - blend;
blendB = blend;
}
FixQuaternion result = new FixQuaternion(blendA * q1.Xyz + blendB * q2.Xyz, blendA * q1.W + blendB * q2.W);
if (result.LengthSquared > Fix.Zero)
{
return(Normalize(result));
}
else
{
return(Identity);
}
}
