//*****************************************************************************
//
//! Computes the angle between two quaternions
//!
//! \param pfQIn1 is a source quaternion in W,X,Y,Z form
//! \param pfQIn2 is a source quaternion in W,X,Y,Z form
//!
//! This function computes the angle between two quaternions.
//!
//! \return Returns the angle, in radians, between the two quaternions.
//
//*****************************************************************************
float
QuaternionAngle(float pfQIn1[4], float pfQIn2[4])
{
float pfQInv[4];
float pfQProd[4];
//
// Let Q1 and Q2 be two quaternions having components w,x,y,z. The angle
// between the orientations represented by Q1 and Q2 can be calculated
// with:
//
// angle = arccos( (Q2 * Q1').w ) * 2.0;
//
// where Q1' is the inverse of Q1
//
//
// Calculate the inverse of Q1
//
QuaternionInverse(pfQInv, pfQIn1);
//
// Find the product of Q2 x Q1`
//
QuaternionMult(pfQProd, pfQIn2, pfQInv);
//
// calculate the arccos of the w component of the previous product.
//
return(acosf(pfQProd[Q_W]) * 2.0);
}
PNT QUATERNION::VectorTransform(const PNT& V)
{
if( ((w>1-DELTA_ROT)&&(w<1+DELTA_ROT)) ||
((w>-1-DELTA_ROT)&&(w<-1+DELTA_ROT)) ) return V;
QUATERNION A, B, C;
A=VectorToQuaternion(V);
B=*this;
QuaternionInverse(B);
C=*this*A*B;
return C.QuaternionToVector();
}
