// ================================================================================
/// <summary>
/// Calculate the dot product (inner product) between two quaternions
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
static public float DotProduct( Quaternion a, Quaternion b ) {
return a.X * b.X +
a.Y * b.Y +
a.Z * b.Z +
a.W * b.W;
}
/// <summary>
/// Get the spherically interpolated quaternion at position t between
/// a and b (UNVERIFIED)
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="t"></param>
/// <returns></returns>
static public Quaternion Slerp( Quaternion a, Quaternion b, float t ) {
// TODO: verify the validity of this function
Debug.WriteLine( "Quaternion.Slerp() - warning, this function has not be checked for validity." );
// Calculate the cosine of the angle between the two
float fScale0, fScale1;
double dCos = Quaternion.DotProduct( a, b );
// If the angle is significant, use the spherical interpolation
if( ( 1.0 - Math.Abs( dCos ) ) > 1e-6f ) {
double dTemp = Math.Acos( Math.Abs( dCos ) );
double dSin = Math.Sin( dTemp );
fScale0 = (float)( Math.Sin( ( 1.0 - t ) * dTemp ) / dSin );
fScale1 = (float)( Math.Sin( t * dTemp ) / dSin );
}
// Else use the cheaper linear interpolation
else {
fScale0 = 1.0f - t;
fScale1 = t;
}
if(dCos < 0.0)
fScale1 = -fScale1;
// Return the interpolated result
return (a * fScale0) + (b * fScale1);
}
/// <summary>
/// Create a quaternion from a 3D homogeneous transform
/// </summary>
/// <param name="xfrm"></param>
/// <returns></returns>
static public Quaternion FromTransform( Matrix3D xfrm ) {
Quaternion quat = new Quaternion();
// Check the sum of the diagonal
float tr = xfrm[ 0, 0 ] + xfrm[ 1, 1 ] + xfrm[ 2, 2 ];
if(tr > 0.0f) {
// The sum is positive
// 4 muls, 1 div, 6 adds, 1 trig function call
float s = (float) Math.Sqrt( tr + 1.0f );
quat.W = s * 0.5f;
s = 0.5f / s;
quat.X = ( xfrm[ 1, 2 ] - xfrm[ 2, 1 ] ) * s;
quat.Y = ( xfrm[ 2, 0 ] - xfrm[ 0, 2 ] ) * s;
quat.Z = ( xfrm[ 0, 1 ] - xfrm[ 1, 0 ] ) * s;
}
else {
// The sum is negative
// 4 muls, 1 div, 8 adds, 1 trig function call
int[] nIndex = { 1, 2, 0 };
int i, j, k;
i = 0;
if( xfrm[ 1, 1 ] > xfrm[ i, i ] )
i = 1;
if( xfrm[ 2, 2 ] > xfrm[ i, i ] )
i = 2;
j = nIndex[i];
k = nIndex[j];
float s = (float) Math.Sqrt(( xfrm[ i, i ] - ( xfrm[ j, j ] + xfrm[ k, k ] ) ) + 1.0f );
quat[ i ] = s * 0.5f;
if( s != 0.0 ) {
s = 0.5f / s;
}
quat[ j ] = ( xfrm[ i, j ] + xfrm[ j, i ] ) * s;
quat[ k ] = ( xfrm[ i, k ] + xfrm[ k, i ] ) * s;
quat[ 3 ] = ( xfrm[ j, k ] - xfrm[ k, j ] ) * s;
}
return quat;
}