/*!***************************************************************************
@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 PVRTMatrixPerspectiveFovRHX
@Output mOut Perspective matrix
@Input fFOVy Field of view
@Input fAspect Aspect ratio
@Input fNear Near clipping distance
@Input fFar Far clipping distance
@Input bRotate Should we rotate it ? (for upright screens)
@Description Create a perspective matrix.
*****************************************************************************/
void PVRTMatrixPerspectiveFovRHX(
PVRTMATRIXx &mOut,
const int fFOVy,
const int fAspect,
const int fNear,
const int fFar,
const bool bRotate)
{
int f;
int fCorrectAspect = fAspect;
if (bRotate)
{
fCorrectAspect = PVRTXDIV(PVRTF2X(1.0f), fAspect);
}
f = PVRTXDIV(PVRTF2X(1.0f), PVRTXTAN(PVRTXMUL(fFOVy, PVRTF2X(0.5f))));
mOut.f[ 0] = PVRTXDIV(f, fCorrectAspect);
mOut.f[ 1] = PVRTF2X(0.0f);
mOut.f[ 2] = PVRTF2X(0.0f);
mOut.f[ 3] = PVRTF2X(0.0f);
mOut.f[ 4] = PVRTF2X(0.0f);
mOut.f[ 5] = f;
mOut.f[ 6] = PVRTF2X(0.0f);
mOut.f[ 7] = PVRTF2X(0.0f);
mOut.f[ 8] = PVRTF2X(0.0f);
mOut.f[ 9] = PVRTF2X(0.0f);
mOut.f[10] = PVRTXDIV(fFar + fNear, fNear - fFar);
mOut.f[11] = PVRTF2X(-1.0f);
mOut.f[12] = PVRTF2X(0.0f);
mOut.f[13] = PVRTF2X(0.0f);
mOut.f[14] = PVRTXMUL(PVRTXDIV(fFar, fNear - fFar), fNear) << 1; // Cheap 2x
mOut.f[15] = PVRTF2X(0.0f);
if (bRotate)
{
PVRTMATRIXx mRotation, mTemp = mOut;
PVRTMatrixRotationZX(mRotation, PVRTF2X(-90.0f*PVRT_PIf/180.0f));
PVRTMatrixMultiplyX(mOut, mTemp, mRotation);
}
}
/*!***************************************************************************
@Function PVRTMatrixPerspectiveFovLHX
@Output mOut Perspective matrix
@Input fFOVy Field of view
@Input fAspect Aspect ratio
@Input fNear Near clipping distance
@Input fFar Far clipping distance
@Input bRotate Should we rotate it ? (for upright screens)
@Description Create a perspective matrix.
*****************************************************************************/
void PVRTMatrixPerspectiveFovLHX(
PVRTMATRIXx &mOut,
const int fFOVy,
const int fAspect,
const int fNear,
const int fFar,
const bool bRotate)
{
int f, fRealAspect;
if (bRotate)
fRealAspect = PVRTXDIV(PVRTF2X(1.0f), fAspect);
else
fRealAspect = fAspect;
f = PVRTXDIV(PVRTF2X(1.0f), PVRTXTAN(PVRTXMUL(fFOVy, PVRTF2X(0.5f))));
mOut.f[ 0] = PVRTXDIV(f, fRealAspect);
mOut.f[ 1] = PVRTF2X(0.0f);
mOut.f[ 2] = PVRTF2X(0.0f);
mOut.f[ 3] = PVRTF2X(0.0f);
mOut.f[ 4] = PVRTF2X(0.0f);
mOut.f[ 5] = f;
mOut.f[ 6] = PVRTF2X(0.0f);
mOut.f[ 7] = PVRTF2X(0.0f);
mOut.f[ 8] = PVRTF2X(0.0f);
mOut.f[ 9] = PVRTF2X(0.0f);
mOut.f[10] = PVRTXDIV(fFar, fFar - fNear);
mOut.f[11] = PVRTF2X(1.0f);
mOut.f[12] = PVRTF2X(0.0f);
mOut.f[13] = PVRTF2X(0.0f);
mOut.f[14] = -PVRTXMUL(PVRTXDIV(fFar, fFar - fNear), fNear);
mOut.f[15] = PVRTF2X(0.0f);
if (bRotate)
{
PVRTMATRIXx mRotation, mTemp = mOut;
PVRTMatrixRotationZX(mRotation, PVRTF2X(90.0f*PVRT_PIf/180.0f));
PVRTMatrixMultiplyX(mOut, mTemp, mRotation);
}
}
/*!***************************************************************************
@Function PVRTMatrixOrthoRHX
@Output mOut Orthographic matrix
@Input w Width of the screen
@Input h Height of the screen
@Input zn Near clipping distance
@Input zf Far clipping distance
@Input bRotate Should we rotate it ? (for upright screens)
@Description Create an orthographic matrix.
*****************************************************************************/
void PVRTMatrixOrthoRHX(
PVRTMATRIXx &mOut,
const int w,
const int h,
const int zn,
const int zf,
const bool bRotate)
{
int fCorrectW = w;
int fCorrectH = h;
if (bRotate)
{
fCorrectW = h;
fCorrectH = w;
}
mOut.f[ 0] = PVRTXDIV(PVRTF2X(2.0f), fCorrectW);
mOut.f[ 1] = PVRTF2X(0.0f);
mOut.f[ 2] = PVRTF2X(0.0f);
mOut.f[ 3] = PVRTF2X(0.0f);
mOut.f[ 4] = PVRTF2X(0.0f);
mOut.f[ 5] = PVRTXDIV(PVRTF2X(2.0f), fCorrectH);
mOut.f[ 6] = PVRTF2X(0.0f);
mOut.f[ 7] = PVRTF2X(0.0f);
mOut.f[ 8] = PVRTF2X(0.0f);
mOut.f[ 9] = PVRTF2X(0.0f);
mOut.f[10] = PVRTXDIV(PVRTF2X(1.0f), zn - zf);
mOut.f[11] = PVRTXDIV(zn, zn - zf);
mOut.f[12] = PVRTF2X(0.0f);
mOut.f[13] = PVRTF2X(0.0f);
mOut.f[14] = PVRTF2X(0.0f);
mOut.f[15] = PVRTF2X(1.0f);
if (bRotate)
{
PVRTMATRIXx mRotation, mTemp = mOut;
PVRTMatrixRotationZX(mRotation, PVRTF2X(-90.0f*PVRT_PIf/180.0f));
PVRTMatrixMultiplyX(mOut, mRotation, mTemp);
}
}
/*!***************************************************************************
@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);
}
/*!***************************************************************************
@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 PVRTMatrixLinearEqSolveX
@Input pSrc 2D array of floats. 4 Eq linear problem is 5x4
matrix, constants in first column
@Input nCnt Number of equations to solve
@Output pRes Result
@Description Solves 'nCnt' simultaneous equations of 'nCnt' variables.
pRes should be an array large enough to contain the
results: the values of the 'nCnt' variables.
This fn recursively uses Gaussian Elimination.
*****************************************************************************/
void PVRTMatrixLinearEqSolveX(
int * const pRes,
int ** const pSrc,
const int nCnt)
{
int i, j, k;
int f;
if (nCnt == 1)
{
_ASSERT(pSrc[0][1] != 0);
pRes[0] = PVRTXDIV(pSrc[0][0], pSrc[0][1]);
return;
}
// Loop backwards in an attempt avoid the need to swap rows
i = nCnt;
while(i)
{
--i;
if(pSrc[i][nCnt] != PVRTF2X(0.0f))
{
// Row i can be used to zero the other rows; let's move it to the bottom
if(i != (nCnt-1))
{
for(j = 0; j <= nCnt; ++j)
{
// Swap the two values
f = pSrc[nCnt-1][j];
pSrc[nCnt-1][j] = pSrc[i][j];
pSrc[i][j] = f;
}
}
// Now zero the last columns of the top rows
for(j = 0; j < (nCnt-1); ++j)
{
_ASSERT(pSrc[nCnt-1][nCnt] != PVRTF2X(0.0f));
f = PVRTXDIV(pSrc[j][nCnt], pSrc[nCnt-1][nCnt]);
// No need to actually calculate a zero for the final column
for(k = 0; k < nCnt; ++k)
{
pSrc[j][k] -= PVRTXMUL(f, pSrc[nCnt-1][k]);
}
}
break;
}
}
// Solve the top-left sub matrix
PVRTMatrixLinearEqSolveX(pRes, pSrc, nCnt - 1);
// Now calc the solution for the bottom row
f = pSrc[nCnt-1][0];
for(k = 1; k < nCnt; ++k)
{
f -= PVRTXMUL(pSrc[nCnt-1][k], pRes[k-1]);
}
_ASSERT(pSrc[nCnt-1][nCnt] != PVRTF2X(0));
f = PVRTXDIV(f, pSrc[nCnt-1][nCnt]);
pRes[nCnt-1] = f;
}
/*!***************************************************************************
@Function PVRTMatrixInverseX
@Output mOut Inversed matrix
@Input mIn Original matrix
@Description Compute the inverse matrix of mIn.
The matrix must be of the form :
A 0
C 1
Where A is a 3x3 matrix and C is a 1x3 matrix.
*****************************************************************************/
void PVRTMatrixInverseX(
PVRTMATRIXx &mOut,
const PVRTMATRIXx &mIn)
{
PVRTMATRIXx mDummyMatrix;
int det_1;
int pos, neg, temp;
/* Calculate the determinant of submatrix A and determine if the
the matrix is singular as limited by the double precision
floating-point data representation. */
pos = neg = 0;
temp = PVRTXMUL(PVRTXMUL(mIn.f[ 0], mIn.f[ 5]), mIn.f[10]);
if (temp >= 0) pos += temp; else neg += temp;
temp = PVRTXMUL(PVRTXMUL(mIn.f[ 4], mIn.f[ 9]), mIn.f[ 2]);
if (temp >= 0) pos += temp; else neg += temp;
temp = PVRTXMUL(PVRTXMUL(mIn.f[ 8], mIn.f[ 1]), mIn.f[ 6]);
if (temp >= 0) pos += temp; else neg += temp;
temp = PVRTXMUL(PVRTXMUL(-mIn.f[ 8], mIn.f[ 5]), mIn.f[ 2]);
if (temp >= 0) pos += temp; else neg += temp;
temp = PVRTXMUL(PVRTXMUL(-mIn.f[ 4], mIn.f[ 1]), mIn.f[10]);
if (temp >= 0) pos += temp; else neg += temp;
temp = PVRTXMUL(PVRTXMUL(-mIn.f[ 0], mIn.f[ 9]), mIn.f[ 6]);
if (temp >= 0) pos += temp; else neg += temp;
det_1 = pos + neg;
/* Is the submatrix A singular? */
if (det_1 == 0)
{
/* Matrix M has no inverse */
_RPT0(_CRT_WARN, "Matrix has no inverse : singular matrix\n");
return;
}
else
{
/* Calculate inverse(A) = adj(A) / det(A) */
//det_1 = 1.0 / det_1;
det_1 = PVRTXDIV(PVRTF2X(1.0f), det_1);
mDummyMatrix.f[ 0] = PVRTXMUL(( PVRTXMUL(mIn.f[ 5], mIn.f[10]) - PVRTXMUL(mIn.f[ 9], mIn.f[ 6]) ), det_1);
mDummyMatrix.f[ 1] = - PVRTXMUL(( PVRTXMUL(mIn.f[ 1], mIn.f[10]) - PVRTXMUL(mIn.f[ 9], mIn.f[ 2]) ), det_1);
mDummyMatrix.f[ 2] = PVRTXMUL(( PVRTXMUL(mIn.f[ 1], mIn.f[ 6]) - PVRTXMUL(mIn.f[ 5], mIn.f[ 2]) ), det_1);
mDummyMatrix.f[ 4] = - PVRTXMUL(( PVRTXMUL(mIn.f[ 4], mIn.f[10]) - PVRTXMUL(mIn.f[ 8], mIn.f[ 6]) ), det_1);
mDummyMatrix.f[ 5] = PVRTXMUL(( PVRTXMUL(mIn.f[ 0], mIn.f[10]) - PVRTXMUL(mIn.f[ 8], mIn.f[ 2]) ), det_1);
mDummyMatrix.f[ 6] = - PVRTXMUL(( PVRTXMUL(mIn.f[ 0], mIn.f[ 6]) - PVRTXMUL(mIn.f[ 4], mIn.f[ 2]) ), det_1);
mDummyMatrix.f[ 8] = PVRTXMUL(( PVRTXMUL(mIn.f[ 4], mIn.f[ 9]) - PVRTXMUL(mIn.f[ 8], mIn.f[ 5]) ), det_1);
mDummyMatrix.f[ 9] = - PVRTXMUL(( PVRTXMUL(mIn.f[ 0], mIn.f[ 9]) - PVRTXMUL(mIn.f[ 8], mIn.f[ 1]) ), det_1);
mDummyMatrix.f[10] = PVRTXMUL(( PVRTXMUL(mIn.f[ 0], mIn.f[ 5]) - PVRTXMUL(mIn.f[ 4], mIn.f[ 1]) ), det_1);
/* Calculate -C * inverse(A) */
mDummyMatrix.f[12] = - ( PVRTXMUL(mIn.f[12], mDummyMatrix.f[ 0]) + PVRTXMUL(mIn.f[13], mDummyMatrix.f[ 4]) + PVRTXMUL(mIn.f[14], mDummyMatrix.f[ 8]) );
mDummyMatrix.f[13] = - ( PVRTXMUL(mIn.f[12], mDummyMatrix.f[ 1]) + PVRTXMUL(mIn.f[13], mDummyMatrix.f[ 5]) + PVRTXMUL(mIn.f[14], mDummyMatrix.f[ 9]) );
mDummyMatrix.f[14] = - ( PVRTXMUL(mIn.f[12], mDummyMatrix.f[ 2]) + PVRTXMUL(mIn.f[13], mDummyMatrix.f[ 6]) + PVRTXMUL(mIn.f[14], mDummyMatrix.f[10]) );
/* Fill in last row */
mDummyMatrix.f[ 3] = PVRTF2X(0.0f);
mDummyMatrix.f[ 7] = PVRTF2X(0.0f);
mDummyMatrix.f[11] = PVRTF2X(0.0f);
mDummyMatrix.f[15] = PVRTF2X(1.0f);
}
/* Copy contents of dummy matrix in pfMatrix */
mOut = mDummyMatrix;
}