/// <summary>
/// Performs spherical interpolation between two quaternions. Spherical interpolation neatly interpolates between
/// two rotations without modifying the size of the vector it is applied to (unlike linear interpolation).
/// </summary>
/// <param name="from">Start quaternion.</param>
/// <param name="to">End quaternion.</param>
/// <param name="t">Interpolation factor in range [0, 1] that determines how much to interpolate between
/// <paramref name="from"/> and <paramref name="to"/>.</param>
/// <param name="shortestPath">Should the interpolation be performed between the shortest or longest path between
/// the two quaternions.</param>
/// <returns>Interpolated quaternion representing a rotation between <paramref name="from"/> and
/// <paramref name="to"/>.</returns>
public static Quaternion Slerp(Quaternion from, Quaternion to, float t, bool shortestPath = true)
{
float dot = Dot(from, to);
Quaternion quat;
if (dot < 0.0f && shortestPath)
{
dot = -dot;
quat = -to;
}
else
{
quat = to;
}
if (MathEx.Abs(dot) < (1 - epsilon))
{
float sin = MathEx.Sqrt(1 - (dot * dot));
Radian angle = MathEx.Atan2(sin, dot);
float invSin = 1.0f / sin;
float a = MathEx.Sin((1.0f - t) * angle) * invSin;
float b = MathEx.Sin(t * angle) * invSin;
return(a * from + b * quat);
}
else
{
Quaternion ret = (1.0f - t) * from + t * quat;
ret.Normalize();
return(ret);
}
}
/// <summary>
/// Converts an orthonormal matrix to euler angle (pitch/yaw/roll) representation.
/// </summary>
/// <returns>Euler angles in degrees representing the rotation in this matrix.</returns>
public Vector3 ToEulerAngles()
{
Radian xAngle = -MathEx.Asin(this[1, 2]);
if (xAngle < MathEx.HalfPi)
{
if (xAngle > -MathEx.HalfPi)
{
Radian yAngle = MathEx.Atan2(this[0, 2], this[2, 2]);
Radian zAngle = MathEx.Atan2(this[1, 0], this[1, 1]);
return(new Vector3(xAngle.Degrees, yAngle.Degrees, zAngle.Degrees));
}
else
{
// Note: Not an unique solution.
xAngle = -MathEx.HalfPi;
Radian yAngle = MathEx.Atan2(-this[0, 1], this[0, 0]);
Radian zAngle = (Radian)0.0f;
return(new Vector3(xAngle.Degrees, yAngle.Degrees, zAngle.Degrees));
}
}
else
{
// Note: Not an unique solution.
xAngle = MathEx.HalfPi;
Radian yAngle = MathEx.Atan2(this[0, 1], this[0, 0]);
Radian zAngle = (Radian)0.0f;
return(new Vector3(xAngle.Degrees, yAngle.Degrees, zAngle.Degrees));
}
}
/// <summary>
/// Converts an orthonormal matrix to euler angle (pitch/yaw/roll) representation.
/// </summary>
/// <returns>Euler angles in degrees representing the rotation in this matrix.</returns>
public Vector3 ToEulerAngles()
{
float xAngle = -MathEx.Asin(this[1, 2]);
if (xAngle < MathEx.HalfPi)
{
if (xAngle > -MathEx.HalfPi)
{
float yAngle = MathEx.Atan2(this[0, 2], this[2, 2]);
float zAngle = MathEx.Atan2(this[1, 0], this[1, 1]);
return(new Vector3(xAngle * MathEx.Rad2Deg, yAngle * MathEx.Rad2Deg, zAngle * MathEx.Rad2Deg));
}
else
{
// Note: Not an unique solution.
xAngle = -MathEx.HalfPi;
float yAngle = MathEx.Atan2(-this[0, 1], this[0, 0]);
float zAngle = 0.0f;
return(new Vector3(xAngle * MathEx.Rad2Deg, yAngle * MathEx.Rad2Deg, zAngle * MathEx.Rad2Deg));
}
}
else
{
// Note: Not an unique solution.
xAngle = MathEx.HalfPi;
float yAngle = MathEx.Atan2(this[0, 1], this[0, 0]);
float zAngle = 0.0f;
return(new Vector3(xAngle * MathEx.Rad2Deg, yAngle * MathEx.Rad2Deg, zAngle * MathEx.Rad2Deg));
}
}
