/******************************************************************************
@File PVRTQuaternionX.cpp
@Title PVRTQuaternionX
@Version
@Copyright Copyright (c) Imagination Technologies Limited.
@Platform ANSI compatible
@Description Set of mathematical functions for quaternions.
******************************************************************************/
#include "PVRTContext.h"
#include <math.h>
#include <string.h>
#include "PVRTFixedPoint.h"
#include "PVRTQuaternion.h"
/****************************************************************************
** Functions
****************************************************************************/
/*!***************************************************************************
@Function PVRTMatrixQuaternionIdentityX
@Output qOut Identity quaternion
@Description Sets the quaternion to (0, 0, 0, 1), the identity quaternion.
*****************************************************************************/
void PVRTMatrixQuaternionIdentityX(PVRTQUATERNIONx &qOut)
{
qOut.x = PVRTF2X(0.0f);
qOut.y = PVRTF2X(0.0f);
qOut.z = PVRTF2X(0.0f);
qOut.w = PVRTF2X(1.0f);
}
/*!***************************************************************************
@Function PVRTMatrixQuaternionRotationAxisX
@Output qOut Rotation quaternion
@Input vAxis Axis to rotate around
@Input fAngle Angle to rotate
@Description Create quaternion corresponding to a rotation of fAngle
radians around submitted vector.
*****************************************************************************/
void PVRTMatrixQuaternionRotationAxisX(
PVRTQUATERNIONx &qOut,
const PVRTVECTOR3x &vAxis,
const int fAngle)
{
int fSin, fCos;
fSin = PVRTXSIN(fAngle>>1);
fCos = PVRTXCOS(fAngle>>1);
/* Create quaternion */
qOut.x = PVRTXMUL(vAxis.x, fSin);
qOut.y = PVRTXMUL(vAxis.y, fSin);
qOut.z = PVRTXMUL(vAxis.z, fSin);
qOut.w = fCos;
/* Normalise it */
PVRTMatrixQuaternionNormalizeX(qOut);
}
/*!***************************************************************************
@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 PVRTMatrixQuaternionSlerpX
@Output qOut Result of the interpolation
@Input qA First quaternion to interpolate from
@Input qB Second quaternion to interpolate from
@Input t Coefficient of interpolation
@Description Perform a Spherical Linear intERPolation between quaternion A
and quaternion B at time t. t must be between 0.0f and 1.0f
Requires input quaternions to be normalized
*****************************************************************************/
void PVRTMatrixQuaternionSlerpX(
PVRTQUATERNIONx &qOut,
const PVRTQUATERNIONx &qA,
const PVRTQUATERNIONx &qB,
const int t)
{
int fCosine, fAngle, A, B;
/* Parameter checking */
if (t<PVRTF2X(0.0f) || t>PVRTF2X(1.0f))
{
_RPT0(_CRT_WARN, "PVRTMatrixQuaternionSlerp : Bad parameters\n");
qOut.x = PVRTF2X(0.0f);
qOut.y = PVRTF2X(0.0f);
qOut.z = PVRTF2X(0.0f);
qOut.w = PVRTF2X(1.0f);
return;
}
/* Find sine of Angle between Quaternion A and B (dot product between quaternion A and B) */
fCosine = PVRTXMUL(qA.w, qB.w) +
PVRTXMUL(qA.x, qB.x) + PVRTXMUL(qA.y, qB.y) + PVRTXMUL(qA.z, qB.z);
if(fCosine < PVRTF2X(0.0f))
{
PVRTQUATERNIONx qi;
/*
<http://www.magic-software.com/Documentation/Quaternions.pdf>
"It is important to note that the quaternions q and -q represent
the same rotation... while either quaternion will do, the
interpolation methods require choosing one over the other.
"Although q1 and -q1 represent the same rotation, the values of
Slerp(t; q0, q1) and Slerp(t; q0,-q1) are not the same. It is
customary to choose the sign... on q1 so that... the angle
between q0 and q1 is acute. This choice avoids extra
spinning caused by the interpolated rotations."
*/
qi.x = -qB.x;
qi.y = -qB.y;
qi.z = -qB.z;
qi.w = -qB.w;
PVRTMatrixQuaternionSlerpX(qOut, qA, qi, t);
return;
}
fCosine = PVRT_MIN(fCosine, PVRTF2X(1.0f));
fAngle = PVRTXACOS(fCosine);
/* Avoid a division by zero */
if (fAngle==PVRTF2X(0.0f))
{
qOut = qA;
return;
}
/* Precompute some values */
A = PVRTXDIV(PVRTXSIN(PVRTXMUL((PVRTF2X(1.0f)-t), fAngle)), PVRTXSIN(fAngle));
B = PVRTXDIV(PVRTXSIN(PVRTXMUL(t, fAngle)), PVRTXSIN(fAngle));
/* Compute resulting quaternion */
qOut.x = PVRTXMUL(A, qA.x) + PVRTXMUL(B, qB.x);
qOut.y = PVRTXMUL(A, qA.y) + PVRTXMUL(B, qB.y);
qOut.z = PVRTXMUL(A, qA.z) + PVRTXMUL(B, qB.z);
qOut.w = PVRTXMUL(A, qA.w) + PVRTXMUL(B, qB.w);
/* Normalise result */
PVRTMatrixQuaternionNormalizeX(qOut);
}
/*!***************************************************************************
@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 PVRTMatrixRotationQuaternionX
@Output mOut Resulting rotation matrix
@Input quat Quaternion to transform
@Description Create rotation matrix from submitted quaternion.
Assuming the quaternion is of the form [X Y Z W]:
| 2 2 |
| 1 - 2Y - 2Z 2XY - 2ZW 2XZ + 2YW 0 |
| |
| 2 2 |
M = | 2XY + 2ZW 1 - 2X - 2Z 2YZ - 2XW 0 |
| |
| 2 2 |
| 2XZ - 2YW 2YZ + 2XW 1 - 2X - 2Y 0 |
| |
| 0 0 0 1 |
*****************************************************************************/
void PVRTMatrixRotationQuaternionX(
PVRTMATRIXx &mOut,
const PVRTQUATERNIONx &quat)
{
const PVRTQUATERNIONx *pQ;
#if defined(BUILD_DX11)
PVRTQUATERNIONx qInv;
qInv.x = -quat.x;
qInv.y = -quat.y;
qInv.z = -quat.z;
qInv.w = quat.w;
pQ = &qInv;
#else
pQ = &quat;
#endif
/* Fill matrix members */
mOut.f[0] = PVRTF2X(1.0f) - (PVRTXMUL(pQ->y, pQ->y)<<1) - (PVRTXMUL(pQ->z, pQ->z)<<1);
mOut.f[1] = (PVRTXMUL(pQ->x, pQ->y)<<1) - (PVRTXMUL(pQ->z, pQ->w)<<1);
mOut.f[2] = (PVRTXMUL(pQ->x, pQ->z)<<1) + (PVRTXMUL(pQ->y, pQ->w)<<1);
mOut.f[3] = PVRTF2X(0.0f);
mOut.f[4] = (PVRTXMUL(pQ->x, pQ->y)<<1) + (PVRTXMUL(pQ->z, pQ->w)<<1);
mOut.f[5] = PVRTF2X(1.0f) - (PVRTXMUL(pQ->x, pQ->x)<<1) - (PVRTXMUL(pQ->z, pQ->z)<<1);
mOut.f[6] = (PVRTXMUL(pQ->y, pQ->z)<<1) - (PVRTXMUL(pQ->x, pQ->w)<<1);
mOut.f[7] = PVRTF2X(0.0f);
mOut.f[8] = (PVRTXMUL(pQ->x, pQ->z)<<1) - (PVRTXMUL(pQ->y, pQ->w)<<1);
mOut.f[9] = (PVRTXMUL(pQ->y, pQ->z)<<1) + (PVRTXMUL(pQ->x, pQ->w)<<1);
mOut.f[10] = PVRTF2X(1.0f) - (PVRTXMUL(pQ->x, pQ->x)<<1) - (PVRTXMUL(pQ->y, pQ->y)<<1);
mOut.f[11] = PVRTF2X(0.0f);
mOut.f[12] = PVRTF2X(0.0f);
mOut.f[13] = PVRTF2X(0.0f);
mOut.f[14] = PVRTF2X(0.0f);
mOut.f[15] = PVRTF2X(1.0f);
}
/*!***************************************************************************
@Function PVRTMatrixQuaternionMultiplyX
@Output qOut Resulting quaternion
@Input qA First quaternion to multiply
@Input qB Second quaternion to multiply
@Description Multiply quaternion A with quaternion B and return the
result in qOut.
Input quaternions must be normalized.
*****************************************************************************/
void PVRTMatrixQuaternionMultiplyX(
PVRTQUATERNIONx &qOut,
const PVRTQUATERNIONx &qA,
const PVRTQUATERNIONx &qB)
{
PVRTVECTOR3x CrossProduct;
/* Compute scalar component */
qOut.w = PVRTXMUL(qA.w, qB.w) -
(PVRTXMUL(qA.x, qB.x) + PVRTXMUL(qA.y, qB.y) + PVRTXMUL(qA.z, qB.z));
/* Compute cross product */
CrossProduct.x = PVRTXMUL(qA.y, qB.z) - PVRTXMUL(qA.z, qB.y);
CrossProduct.y = PVRTXMUL(qA.z, qB.x) - PVRTXMUL(qA.x, qB.z);
CrossProduct.z = PVRTXMUL(qA.x, qB.y) - PVRTXMUL(qA.y, qB.x);
/* Compute result vector */
qOut.x = PVRTXMUL(qA.w, qB.x) + PVRTXMUL(qB.w, qA.x) + CrossProduct.x;
qOut.y = PVRTXMUL(qA.w, qB.y) + PVRTXMUL(qB.w, qA.y) + CrossProduct.y;
qOut.z = PVRTXMUL(qA.w, qB.z) + PVRTXMUL(qB.w, qA.z) + CrossProduct.z;
/* Normalize resulting quaternion */
PVRTMatrixQuaternionNormalizeX(qOut);
}
/*****************************************************************************
End of file (PVRTQuaternionX.cpp)
*****************************************************************************/