/******************************************************************************
|
|
@File PVRTVector.cpp
|
|
@Title PVRTVector
|
|
@Version
|
|
@Copyright Copyright (c) Imagination Technologies Limited.
|
|
@Platform ANSI compatible
|
|
@Description Vector and matrix mathematics library
|
|
******************************************************************************/
|
|
#include "PVRTVector.h"
|
|
#include <math.h>
|
|
/*!***************************************************************************
|
** PVRTVec2 2 component vector
|
****************************************************************************/
|
|
/*!***************************************************************************
|
@Function PVRTVec2
|
@Input v3Vec a Vec3
|
@Description Constructor from a Vec3
|
*****************************************************************************/
|
PVRTVec2::PVRTVec2(const PVRTVec3& vec3)
|
{
|
x = vec3.x; y = vec3.y;
|
}
|
|
/*!***************************************************************************
|
** PVRTVec3 3 component vector
|
****************************************************************************/
|
|
/*!***************************************************************************
|
@Function PVRTVec3
|
@Input v4Vec a PVRTVec4
|
@Description Constructor from a PVRTVec4
|
*****************************************************************************/
|
PVRTVec3::PVRTVec3(const PVRTVec4& vec4)
|
{
|
x = vec4.x; y = vec4.y; z = vec4.z;
|
}
|
|
/*!***************************************************************************
|
@Function *
|
@Input rhs a PVRTMat3
|
@Returns result of multiplication
|
@Description matrix multiplication operator PVRTVec3 and PVRTMat3
|
****************************************************************************/
|
PVRTVec3 PVRTVec3::operator*(const PVRTMat3& rhs) const
|
{
|
PVRTVec3 out;
|
|
out.x = VERTTYPEMUL(x,rhs.f[0])+VERTTYPEMUL(y,rhs.f[1])+VERTTYPEMUL(z,rhs.f[2]);
|
out.y = VERTTYPEMUL(x,rhs.f[3])+VERTTYPEMUL(y,rhs.f[4])+VERTTYPEMUL(z,rhs.f[5]);
|
out.z = VERTTYPEMUL(x,rhs.f[6])+VERTTYPEMUL(y,rhs.f[7])+VERTTYPEMUL(z,rhs.f[8]);
|
|
return out;
|
}
|
|
/*!***************************************************************************
|
@Function *=
|
@Input rhs a PVRTMat3
|
@Returns result of multiplication and assignment
|
@Description matrix multiplication and assignment operator for PVRTVec3 and PVRTMat3
|
****************************************************************************/
|
PVRTVec3& PVRTVec3::operator*=(const PVRTMat3& rhs)
|
{
|
VERTTYPE tx = VERTTYPEMUL(x,rhs.f[0])+VERTTYPEMUL(y,rhs.f[1])+VERTTYPEMUL(z,rhs.f[2]);
|
VERTTYPE ty = VERTTYPEMUL(x,rhs.f[3])+VERTTYPEMUL(y,rhs.f[4])+VERTTYPEMUL(z,rhs.f[5]);
|
z = VERTTYPEMUL(x,rhs.f[6])+VERTTYPEMUL(y,rhs.f[7])+VERTTYPEMUL(z,rhs.f[8]);
|
x = tx;
|
y = ty;
|
|
return *this;
|
}
|
|
/*!***************************************************************************
|
** PVRTVec4 4 component vector
|
****************************************************************************/
|
|
/*!***************************************************************************
|
@Function *
|
@Input rhs a PVRTMat4
|
@Returns result of multiplication
|
@Description matrix multiplication operator PVRTVec4 and PVRTMat4
|
****************************************************************************/
|
PVRTVec4 PVRTVec4::operator*(const PVRTMat4& rhs) const
|
{
|
PVRTVec4 out;
|
out.x = VERTTYPEMUL(x,rhs.f[0])+VERTTYPEMUL(y,rhs.f[1])+VERTTYPEMUL(z,rhs.f[2])+VERTTYPEMUL(w,rhs.f[3]);
|
out.y = VERTTYPEMUL(x,rhs.f[4])+VERTTYPEMUL(y,rhs.f[5])+VERTTYPEMUL(z,rhs.f[6])+VERTTYPEMUL(w,rhs.f[7]);
|
out.z = VERTTYPEMUL(x,rhs.f[8])+VERTTYPEMUL(y,rhs.f[9])+VERTTYPEMUL(z,rhs.f[10])+VERTTYPEMUL(w,rhs.f[11]);
|
out.w = VERTTYPEMUL(x,rhs.f[12])+VERTTYPEMUL(y,rhs.f[13])+VERTTYPEMUL(z,rhs.f[14])+VERTTYPEMUL(w,rhs.f[15]);
|
return out;
|
}
|
|
/*!***************************************************************************
|
@Function *=
|
@Input rhs a PVRTMat4
|
@Returns result of multiplication and assignment
|
@Description matrix multiplication and assignment operator for PVRTVec4 and PVRTMat4
|
****************************************************************************/
|
PVRTVec4& PVRTVec4::operator*=(const PVRTMat4& rhs)
|
{
|
VERTTYPE tx = VERTTYPEMUL(x,rhs.f[0])+VERTTYPEMUL(y,rhs.f[1])+VERTTYPEMUL(z,rhs.f[2])+VERTTYPEMUL(w,rhs.f[3]);
|
VERTTYPE ty = VERTTYPEMUL(x,rhs.f[4])+VERTTYPEMUL(y,rhs.f[5])+VERTTYPEMUL(z,rhs.f[6])+VERTTYPEMUL(w,rhs.f[7]);
|
VERTTYPE tz = VERTTYPEMUL(x,rhs.f[8])+VERTTYPEMUL(y,rhs.f[9])+VERTTYPEMUL(z,rhs.f[10])+VERTTYPEMUL(w,rhs.f[11]);
|
w = VERTTYPEMUL(x,rhs.f[12])+VERTTYPEMUL(y,rhs.f[13])+VERTTYPEMUL(z,rhs.f[14])+VERTTYPEMUL(w,rhs.f[15]);
|
x = tx;
|
y = ty;
|
z = tz;
|
return *this;
|
}
|
|
/*!***************************************************************************
|
** PVRTMat3 3x3 matrix
|
****************************************************************************/
|
/*!***************************************************************************
|
@Function PVRTMat3
|
@Input mat a PVRTMat4
|
@Description constructor to form a PVRTMat3 from a PVRTMat4
|
****************************************************************************/
|
PVRTMat3::PVRTMat3(const PVRTMat4& mat)
|
{
|
VERTTYPE *dest = (VERTTYPE*)f, *src = (VERTTYPE*)mat.f;
|
for(int i=0;i<3;i++)
|
{
|
for(int j=0;j<3;j++)
|
{
|
(*dest++) = (*src++);
|
}
|
src++;
|
}
|
}
|
|
/*!***************************************************************************
|
@Function RotationX
|
@Input angle the angle of rotation
|
@Returns rotation matrix
|
@Description generates a 3x3 rotation matrix about the X axis
|
****************************************************************************/
|
PVRTMat3 PVRTMat3::RotationX(VERTTYPE angle)
|
{
|
PVRTMat4 out;
|
PVRTMatrixRotationX(out,angle);
|
return PVRTMat3(out);
|
}
|
/*!***************************************************************************
|
@Function RotationY
|
@Input angle the angle of rotation
|
@Returns rotation matrix
|
@Description generates a 3x3 rotation matrix about the Y axis
|
****************************************************************************/
|
PVRTMat3 PVRTMat3::RotationY(VERTTYPE angle)
|
{
|
PVRTMat4 out;
|
PVRTMatrixRotationY(out,angle);
|
return PVRTMat3(out);
|
}
|
/*!***************************************************************************
|
@Function RotationZ
|
@Input angle the angle of rotation
|
@Returns rotation matrix
|
@Description generates a 3x3 rotation matrix about the Z axis
|
****************************************************************************/
|
PVRTMat3 PVRTMat3::RotationZ(VERTTYPE angle)
|
{
|
PVRTMat4 out;
|
PVRTMatrixRotationZ(out,angle);
|
return PVRTMat3(out);
|
}
|
|
|
/*!***************************************************************************
|
** PVRTMat4 4x4 matrix
|
****************************************************************************/
|
/*!***************************************************************************
|
@Function RotationX
|
@Input angle the angle of rotation
|
@Returns rotation matrix
|
@Description generates a 4x4 rotation matrix about the X axis
|
****************************************************************************/
|
PVRTMat4 PVRTMat4::RotationX(VERTTYPE angle)
|
{
|
PVRTMat4 out;
|
PVRTMatrixRotationX(out,angle);
|
return out;
|
}
|
/*!***************************************************************************
|
@Function RotationY
|
@Input angle the angle of rotation
|
@Returns rotation matrix
|
@Description generates a 4x4 rotation matrix about the Y axis
|
****************************************************************************/
|
PVRTMat4 PVRTMat4::RotationY(VERTTYPE angle)
|
{
|
PVRTMat4 out;
|
PVRTMatrixRotationY(out,angle);
|
return out;
|
}
|
/*!***************************************************************************
|
@Function RotationZ
|
@Input angle the angle of rotation
|
@Returns rotation matrix
|
@Description generates a 4x4 rotation matrix about the Z axis
|
****************************************************************************/
|
PVRTMat4 PVRTMat4::RotationZ(VERTTYPE angle)
|
{
|
PVRTMat4 out;
|
PVRTMatrixRotationZ(out,angle);
|
return out;
|
}
|
|
/*!***************************************************************************
|
@Function *
|
@Input rhs another PVRTMat4
|
@Returns result of multiplication
|
@Description Matrix multiplication of two 4x4 matrices.
|
*****************************************************************************/
|
PVRTMat4 PVRTMat4::operator*(const PVRTMat4& rhs) const
|
{
|
PVRTMat4 out;
|
// col 1
|
out.f[0] = VERTTYPEMUL(f[0],rhs.f[0])+VERTTYPEMUL(f[4],rhs.f[1])+VERTTYPEMUL(f[8],rhs.f[2])+VERTTYPEMUL(f[12],rhs.f[3]);
|
out.f[1] = VERTTYPEMUL(f[1],rhs.f[0])+VERTTYPEMUL(f[5],rhs.f[1])+VERTTYPEMUL(f[9],rhs.f[2])+VERTTYPEMUL(f[13],rhs.f[3]);
|
out.f[2] = VERTTYPEMUL(f[2],rhs.f[0])+VERTTYPEMUL(f[6],rhs.f[1])+VERTTYPEMUL(f[10],rhs.f[2])+VERTTYPEMUL(f[14],rhs.f[3]);
|
out.f[3] = VERTTYPEMUL(f[3],rhs.f[0])+VERTTYPEMUL(f[7],rhs.f[1])+VERTTYPEMUL(f[11],rhs.f[2])+VERTTYPEMUL(f[15],rhs.f[3]);
|
|
// col 2
|
out.f[4] = VERTTYPEMUL(f[0],rhs.f[4])+VERTTYPEMUL(f[4],rhs.f[5])+VERTTYPEMUL(f[8],rhs.f[6])+VERTTYPEMUL(f[12],rhs.f[7]);
|
out.f[5] = VERTTYPEMUL(f[1],rhs.f[4])+VERTTYPEMUL(f[5],rhs.f[5])+VERTTYPEMUL(f[9],rhs.f[6])+VERTTYPEMUL(f[13],rhs.f[7]);
|
out.f[6] = VERTTYPEMUL(f[2],rhs.f[4])+VERTTYPEMUL(f[6],rhs.f[5])+VERTTYPEMUL(f[10],rhs.f[6])+VERTTYPEMUL(f[14],rhs.f[7]);
|
out.f[7] = VERTTYPEMUL(f[3],rhs.f[4])+VERTTYPEMUL(f[7],rhs.f[5])+VERTTYPEMUL(f[11],rhs.f[6])+VERTTYPEMUL(f[15],rhs.f[7]);
|
|
// col3
|
out.f[8] = VERTTYPEMUL(f[0],rhs.f[8])+VERTTYPEMUL(f[4],rhs.f[9])+VERTTYPEMUL(f[8],rhs.f[10])+VERTTYPEMUL(f[12],rhs.f[11]);
|
out.f[9] = VERTTYPEMUL(f[1],rhs.f[8])+VERTTYPEMUL(f[5],rhs.f[9])+VERTTYPEMUL(f[9],rhs.f[10])+VERTTYPEMUL(f[13],rhs.f[11]);
|
out.f[10] = VERTTYPEMUL(f[2],rhs.f[8])+VERTTYPEMUL(f[6],rhs.f[9])+VERTTYPEMUL(f[10],rhs.f[10])+VERTTYPEMUL(f[14],rhs.f[11]);
|
out.f[11] = VERTTYPEMUL(f[3],rhs.f[8])+VERTTYPEMUL(f[7],rhs.f[9])+VERTTYPEMUL(f[11],rhs.f[10])+VERTTYPEMUL(f[15],rhs.f[11]);
|
|
// col3
|
out.f[12] = VERTTYPEMUL(f[0],rhs.f[12])+VERTTYPEMUL(f[4],rhs.f[13])+VERTTYPEMUL(f[8],rhs.f[14])+VERTTYPEMUL(f[12],rhs.f[15]);
|
out.f[13] = VERTTYPEMUL(f[1],rhs.f[12])+VERTTYPEMUL(f[5],rhs.f[13])+VERTTYPEMUL(f[9],rhs.f[14])+VERTTYPEMUL(f[13],rhs.f[15]);
|
out.f[14] = VERTTYPEMUL(f[2],rhs.f[12])+VERTTYPEMUL(f[6],rhs.f[13])+VERTTYPEMUL(f[10],rhs.f[14])+VERTTYPEMUL(f[14],rhs.f[15]);
|
out.f[15] = VERTTYPEMUL(f[3],rhs.f[12])+VERTTYPEMUL(f[7],rhs.f[13])+VERTTYPEMUL(f[11],rhs.f[14])+VERTTYPEMUL(f[15],rhs.f[15]);
|
return out;
|
}
|
|
|
/*!***************************************************************************
|
@Function inverse
|
@Returns inverse mat4
|
@Description Calculates multiplicative inverse of this matrix
|
The matrix must be of the form :
|
A 0
|
C 1
|
Where A is a 3x3 matrix and C is a 1x3 matrix.
|
*****************************************************************************/
|
PVRTMat4 PVRTMat4::inverse() const
|
{
|
PVRTMat4 out;
|
VERTTYPE det_1;
|
VERTTYPE 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 = f2vt(0.0);
|
temp = VERTTYPEMUL(VERTTYPEMUL(f[ 0], f[ 5]), f[10]);
|
if (temp >= 0) pos += temp; else neg += temp;
|
temp = VERTTYPEMUL(VERTTYPEMUL(f[ 4], f[ 9]), f[ 2]);
|
if (temp >= 0) pos += temp; else neg += temp;
|
temp = VERTTYPEMUL(VERTTYPEMUL(f[ 8], f[ 1]), f[ 6]);
|
if (temp >= 0) pos += temp; else neg += temp;
|
temp = VERTTYPEMUL(VERTTYPEMUL(-f[ 8], f[ 5]), f[ 2]);
|
if (temp >= 0) pos += temp; else neg += temp;
|
temp = VERTTYPEMUL(VERTTYPEMUL(-f[ 4], f[ 1]), f[10]);
|
if (temp >= 0) pos += temp; else neg += temp;
|
temp = VERTTYPEMUL(VERTTYPEMUL(-f[ 0], f[ 9]), f[ 6]);
|
if (temp >= 0) pos += temp; else neg += temp;
|
det_1 = pos + neg;
|
|
/* Is the submatrix A singular? */
|
if (det_1 == f2vt(0.0)) //|| (VERTTYPEABS(det_1 / (pos - neg)) < 1.0e-15)
|
{
|
/* Matrix M has no inverse */
|
_RPT0(_CRT_WARN, "Matrix has no inverse : singular matrix\n");
|
}
|
else
|
{
|
/* Calculate inverse(A) = adj(A) / det(A) */
|
//det_1 = 1.0 / det_1;
|
det_1 = VERTTYPEDIV(f2vt(1.0f), det_1);
|
out.f[ 0] = VERTTYPEMUL(( VERTTYPEMUL(f[ 5], f[10]) - VERTTYPEMUL(f[ 9], f[ 6]) ), det_1);
|
out.f[ 1] = - VERTTYPEMUL(( VERTTYPEMUL(f[ 1], f[10]) - VERTTYPEMUL(f[ 9], f[ 2]) ), det_1);
|
out.f[ 2] = VERTTYPEMUL(( VERTTYPEMUL(f[ 1], f[ 6]) - VERTTYPEMUL(f[ 5], f[ 2]) ), det_1);
|
out.f[ 4] = - VERTTYPEMUL(( VERTTYPEMUL(f[ 4], f[10]) - VERTTYPEMUL(f[ 8], f[ 6]) ), det_1);
|
out.f[ 5] = VERTTYPEMUL(( VERTTYPEMUL(f[ 0], f[10]) - VERTTYPEMUL(f[ 8], f[ 2]) ), det_1);
|
out.f[ 6] = - VERTTYPEMUL(( VERTTYPEMUL(f[ 0], f[ 6]) - VERTTYPEMUL(f[ 4], f[ 2]) ), det_1);
|
out.f[ 8] = VERTTYPEMUL(( VERTTYPEMUL(f[ 4], f[ 9]) - VERTTYPEMUL(f[ 8], f[ 5]) ), det_1);
|
out.f[ 9] = - VERTTYPEMUL(( VERTTYPEMUL(f[ 0], f[ 9]) - VERTTYPEMUL(f[ 8], f[ 1]) ), det_1);
|
out.f[10] = VERTTYPEMUL(( VERTTYPEMUL(f[ 0], f[ 5]) - VERTTYPEMUL(f[ 4], f[ 1]) ), det_1);
|
|
/* Calculate -C * inverse(A) */
|
out.f[12] = - ( VERTTYPEMUL(f[12], out.f[ 0]) + VERTTYPEMUL(f[13], out.f[ 4]) + VERTTYPEMUL(f[14], out.f[ 8]) );
|
out.f[13] = - ( VERTTYPEMUL(f[12], out.f[ 1]) + VERTTYPEMUL(f[13], out.f[ 5]) + VERTTYPEMUL(f[14], out.f[ 9]) );
|
out.f[14] = - ( VERTTYPEMUL(f[12], out.f[ 2]) + VERTTYPEMUL(f[13], out.f[ 6]) + VERTTYPEMUL(f[14], out.f[10]) );
|
|
/* Fill in last row */
|
out.f[ 3] = f2vt(0.0f);
|
out.f[ 7] = f2vt(0.0f);
|
out.f[11] = f2vt(0.0f);
|
out.f[15] = f2vt(1.0f);
|
}
|
|
return out;
|
}
|
|
/*!***************************************************************************
|
@Function PVRTLinearEqSolve
|
@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 PVRTLinearEqSolve(VERTTYPE * const pRes, VERTTYPE ** const pSrc, const int nCnt)
|
{
|
int i, j, k;
|
VERTTYPE f;
|
|
if (nCnt == 1)
|
{
|
_ASSERT(pSrc[0][1] != 0);
|
pRes[0] = VERTTYPEDIV(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] != f2vt(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] != f2vt(0.0f));
|
f = VERTTYPEDIV(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] -= VERTTYPEMUL(f, pSrc[nCnt-1][k]);
|
}
|
}
|
|
break;
|
}
|
}
|
|
// Solve the top-left sub matrix
|
PVRTLinearEqSolve(pRes, pSrc, nCnt - 1);
|
|
// Now calc the solution for the bottom row
|
f = pSrc[nCnt-1][0];
|
for(k = 1; k < nCnt; ++k)
|
{
|
f -= VERTTYPEMUL(pSrc[nCnt-1][k], pRes[k-1]);
|
}
|
_ASSERT(pSrc[nCnt-1][nCnt] != f2vt(0));
|
f = VERTTYPEDIV(f, pSrc[nCnt-1][nCnt]);
|
pRes[nCnt-1] = f;
|
}
|
|
/*****************************************************************************
|
End of file (PVRTVector.cpp)
|
*****************************************************************************/
|
