/*!***************************************************************************
@Function PVRTMatrixQuaternionToAxisAngleX
@Input qIn Quaternion to transform
@Output vAxis Axis of rotation
@Output fAngle Angle of rotation
@Description Convert a quaternion to an axis and angle. Expects a unit
quaternion.
*****************************************************************************/
void PVRTMatrixQuaternionToAxisAngleX(
const PVRTQUATERNIONx &qIn,
PVRTVECTOR3x &vAxis,
int &fAngle)
{
int fCosAngle, fSinAngle;
int temp;
/* Compute some values */
fCosAngle = qIn.w;
temp = PVRTF2X(1.0f) - PVRTXMUL(fCosAngle, fCosAngle);
fAngle = PVRTXMUL(PVRTXACOS(fCosAngle), PVRTF2X(2.0f));
fSinAngle = PVRTF2X(((float)sqrt(PVRTX2F(temp))));
/* This is to avoid a division by zero */
if (PVRTABS(fSinAngle)<PVRTF2X(0.0005f))
{
fSinAngle = PVRTF2X(1.0f);
}
/* Get axis vector */
vAxis.x = PVRTXDIV(qIn.x, fSinAngle);
vAxis.y = PVRTXDIV(qIn.y, fSinAngle);
vAxis.z = PVRTXDIV(qIn.z, fSinAngle);
}
/*!***************************************************************************
@Function PVRTMatrixVec3LengthX
@Input vIn Vector to get the length of
@Return The length of the vector
@Description Gets the length of the supplied vector
*****************************************************************************/
int PVRTMatrixVec3LengthX(
const PVRTVECTOR3x &vIn)
{
int temp;
temp = PVRTXMUL(vIn.x,vIn.x) + PVRTXMUL(vIn.y,vIn.y) + PVRTXMUL(vIn.z,vIn.z);
return PVRTF2X(sqrt(PVRTX2F(temp)));
}
/*!***************************************************************************
@Function PVRTMatrixQuaternionNormalizeX
@Modified quat Vector to normalize
@Description Normalize quaternion.
Original quaternion is scaled down prior to be normalized in
order to avoid overflow issues.
*****************************************************************************/
void PVRTMatrixQuaternionNormalizeX(PVRTQUATERNIONx &quat)
{
PVRTQUATERNIONx qTemp;
int f, n;
/* Scale vector by uniform value */
n = PVRTABS(quat.w) + PVRTABS(quat.x) + PVRTABS(quat.y) + PVRTABS(quat.z);
qTemp.w = PVRTXDIV(quat.w, n);
qTemp.x = PVRTXDIV(quat.x, n);
qTemp.y = PVRTXDIV(quat.y, n);
qTemp.z = PVRTXDIV(quat.z, n);
/* Compute quaternion magnitude */
f = PVRTXMUL(qTemp.w, qTemp.w) + PVRTXMUL(qTemp.x, qTemp.x) + PVRTXMUL(qTemp.y, qTemp.y) + PVRTXMUL(qTemp.z, qTemp.z);
f = PVRTXDIV(PVRTF2X(1.0f), PVRTF2X(sqrt(PVRTX2F(f))));
/* Multiply vector components by f */
quat.x = PVRTXMUL(qTemp.x, f);
quat.y = PVRTXMUL(qTemp.y, f);
quat.z = PVRTXMUL(qTemp.z, f);
quat.w = PVRTXMUL(qTemp.w, f);
}
/*!***************************************************************************
@Function PVRTMatrixVec3NormalizeX
@Output vOut Normalized vector
@Input vIn Vector to normalize
@Description Normalizes the supplied vector.
The square root function is currently still performed
in floating-point.
Original vector is scaled down prior to be normalized in
order to avoid overflow issues.
****************************************************************************/
void PVRTMatrixVec3NormalizeX(
PVRTVECTOR3x &vOut,
const PVRTVECTOR3x &vIn)
{
int f, n;
PVRTVECTOR3x vTemp;
/* Scale vector by uniform value */
n = PVRTABS(vIn.x) + PVRTABS(vIn.y) + PVRTABS(vIn.z);
vTemp.x = PVRTXDIV(vIn.x, n);
vTemp.y = PVRTXDIV(vIn.y, n);
vTemp.z = PVRTXDIV(vIn.z, n);
/* Calculate x2+y2+z2/sqrt(x2+y2+z2) */
f = PVRTMatrixVec3DotProductX(vTemp, vTemp);
f = PVRTXDIV(PVRTF2X(1.0f), PVRTF2X(sqrt(PVRTX2F(f))));
/* Multiply vector components by f */
vOut.x = PVRTXMUL(vTemp.x, f);
vOut.y = PVRTXMUL(vTemp.y, f);
vOut.z = PVRTXMUL(vTemp.z, f);
}
