/// <summary>
/// Convert the current quaternion to axis angle representation
/// </summary>
/// <param name="axis">The resultant axis</param>
/// <param name="angle">The resultant angle</param>
public void ToAxisAngle(out GVector3 axis, out float angle)
{
GVector4 result = ToAxisAngle();
axis = result.Xyz;
angle = result.W;
}
/// <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>The equivalent quaternion</returns>
public static GQuaternion FromAxisAngle(GVector3 axis, float angle)
{
if (Math.Abs(axis.LengthSquared) < 0.00001f)
{
return(Identity);
}
GQuaternion result = Identity;
angle *= 0.5f;
axis.Normalize();
result.Xyz = axis * (float)System.Math.Sin(angle);
result.W = (float)System.Math.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 GQuaternion Slerp(GQuaternion q1, GQuaternion q2, float blend)
{
// if either input is zero, return the other.
if (Math.Abs(q1.LengthSquared) < MathHelper.EPSILON)
{
if (Math.Abs(q2.LengthSquared) < MathHelper.EPSILON)
{
return(Identity);
}
return(q2);
}
else if (Math.Abs(q2.LengthSquared) < MathHelper.EPSILON)
{
return(q1);
}
float cosHalfAngle = q1.W * q2.W + GVector3.Dot(q1.Xyz, q2.Xyz);
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f)
{
// angle = 0.0f, so just return one input.
return(q1);
}
else if (cosHalfAngle < 0.0f)
{
q2.Xyz = -q2.Xyz;
q2.W = -q2.W;
cosHalfAngle = -cosHalfAngle;
}
float blendA;
float blendB;
if (cosHalfAngle < 0.99f)
{
// do proper slerp for big angles
float halfAngle = (float)System.Math.Acos(cosHalfAngle);
float sinHalfAngle = (float)System.Math.Sin(halfAngle);
float oneOverSinHalfAngle = 1.0f / sinHalfAngle;
blendA = (float)System.Math.Sin(halfAngle * (1.0f - blend)) * oneOverSinHalfAngle;
blendB = (float)System.Math.Sin(halfAngle * blend) * oneOverSinHalfAngle;
}
else
{
// do lerp if angle is really small.
blendA = 1.0f - blend;
blendB = blend;
}
GQuaternion result = new GQuaternion(blendA * q1.Xyz + blendB * q2.Xyz, blendA * q1.W + blendB * q2.W);
if (result.LengthSquared > 0.0f)
{
return(Normalize(result));
}
else
{
return(Identity);
}
}
/// <summary>
/// Multiplies two instances.
/// </summary>
/// <param name="left">The first instance.</param>
/// <param name="right">The second instance.</param>
/// <param name="result">A new instance containing the result of the calculation.</param>
public static void Multiply(ref GQuaternion left, ref GQuaternion right, out GQuaternion result)
{
result = new GQuaternion(
right.W * left.Xyz + left.W * right.Xyz + GVector3.Cross(left.Xyz, right.Xyz),
left.W * right.W - GVector3.Dot(left.Xyz, right.Xyz));
}
public static GQuaternion Mult(GQuaternion left, GQuaternion right)
{
return(new GQuaternion(
right.W * left.Xyz + left.W * right.Xyz + GVector3.Cross(left.Xyz, right.Xyz),
left.W * right.W - GVector3.Dot(left.Xyz, right.Xyz)));
}
/// <summary>
/// Construct a new Quaternion from vector and w components
/// </summary>
/// <param name="v">The vector part</param>
/// <param name="w">The w part</param>
public GQuaternion(GVector3 v, float w)
{
this.xyz = v;
this.w = w;
}