ntvmath.h

Lido PXA270平台开发板的最新BSP,包括源代码

	Returns		: The dot product of the input vectors
<function/>
*********************************************************************************/
__inline NTV_TYPE DotProduct(PVR_VECTOR3 *psA, PVR_VECTOR3 *psB)
{
	return (NTV_TYPE)Add3(Mul(psA->x, psB->x),
						  Mul(psA->y, psB->y),
						  Mul(psA->z, psB->z));
}
/********************************************************************************
<function>
	Function	: DotProduct_PR
	Parameters	:	In:  psA - First input vector
					In:  psB - Second input vector
	Description : Finds the high precision dot product of the input vectors
	Returns		: The dot product of the input vectors
<function/>
*********************************************************************************/
__inline NTV_TYPE DotProduct_PR(PVR_VECTOR3 *psA, PVR_VECTOR3 *psB)
{
	return Add3(MulPR(psA->x, psB->x),
			    MulPR(psA->y, psB->y),
				MulPR(psA->z, psB->z));
}

/********************************************************************************
<function>
	Function	: DotProduct4
	Parameters	:	In:  psA - First input vector
					In:  psB - Second input vector
	Description : Finds the dot product of the input vectors
	Returns		: The dot product of the input vectors
<function/>
*********************************************************************************/
__inline NTV_TYPE DotProduct4(PVR_VECTOR4 *psA, PVR_VECTOR4 *psB)
{
	return (NTV_TYPE)Add4(Mul(psA->x, psB->x),
						  Mul(psA->y, psB->y),
						  Mul(psA->z, psB->z),
						  Mul(psA->w, psB->w));
}
/********************************************************************************
<function>
	Function	: DotProduct4
	Parameters	:	In:  psA - First input vector
					In:  psB - Second input vector
	Description : Finds the dot product of the input vectors
				: NB: psA is treated as a high precision vector

	Returns		: The (NORMAL PRECISION) dot product of the input vectors
<function/>
*********************************************************************************/
__inline NTV_TYPE DotProduct4PR_A(PVR_VECTOR4 *psA, PVR_VECTOR4 *psB)
{
	return (NTV_TYPE)PR_UNSHIFT(Add4(MulPR_X(psA->x, psB->x),
									 MulPR_X(psA->y, psB->y),
									 MulPR_X(psA->z, psB->z),
									 MulPR_X(psA->w, psB->w)));
}
/********************************************************************************
<function>
	Function	: MultiplyMatrix
	Parameters	:	In:  psA - First Matrix
					In:  psB - Second Matrix
					In:  psOut - Output matrix
					
	Description : Multiplies 2 matrices
	Returns		: 
<function/>
*********************************************************************************/
__inline void MultiplyMatrix(PVR_44_MATRIX *psA, PVR_44_MATRIX *psB, PVR_44_MATRIX *psOut)
{
	WORD i;
	WORD j;
	for(j=0; j<4; j++)
	{
		for(i=0; i<4; i++)
		{
			psOut->m[i][j] = Add4(Mul(psA->m[i][0], psB->m[0][j]),
								  Mul(psA->m[i][1], psB->m[1][j]),
								  Mul(psA->m[i][2], psB->m[2][j]),
								  Mul(psA->m[i][3], psB->m[3][j]));
		}
	}
}
/********************************************************************************
<function>
	Function	: TransformVector
	Parameters	:	In:  psA - First Matrix
					In:  psVec - Vector
					
	Description : Multiplies a vector by a matrix
	Returns		: transformed vector
<function/>
*********************************************************************************/
__inline void TransformVector(PVR_44_MATRIX *psA, PVR_VECTOR3 *psVec, PVR_VECTOR3 *psOut)
{
	psOut->x = Add3(Mul(psVec->x, psA->_11),
					Mul(psVec->y, psA->_21),
					Mul(psVec->z, psA->_31));
	psOut->y = Add3(Mul(psVec->x, psA->_12),
					Mul(psVec->y, psA->_22),
					Mul(psVec->z, psA->_32));
	psOut->z = Add3(Mul(psVec->x, psA->_13),
					Mul(psVec->y, psA->_23),
					Mul(psVec->z, psA->_33));
}
/********************************************************************************
<function>
	Function	: TransformVector4
	Parameters	:	In:  psA - First Matrix
					In:  psVec - Vector
					
	Description : Multiplies a vector4 by a matrix
	Returns		: transformed vector4
<function/>
*********************************************************************************/
__inline void TransformVector4(PVR_44_MATRIX *psA, 
							   PVR_VECTOR4 *psVec, 
							   PVR_VECTOR4 *psOut)
{
	psOut->x = Add4(Mul(psVec->x, psA->_11),
					Mul(psVec->y, psA->_21),
					Mul(psVec->z, psA->_31),
					Mul(psVec->w, psA->_41));
	psOut->y = Add4(Mul(psVec->x, psA->_12),
					Mul(psVec->y, psA->_22),
					Mul(psVec->z, psA->_32),
					Mul(psVec->w, psA->_42));
	psOut->z = Add4(Mul(psVec->x, psA->_13),
					Mul(psVec->y, psA->_23),
					Mul(psVec->z, psA->_33),
					Mul(psVec->w, psA->_43));
	psOut->w = Add4(Mul(psVec->x, psA->_14),
					Mul(psVec->y, psA->_24),
					Mul(psVec->z, psA->_34),
					Mul(psVec->w, psA->_44));
}
/********************************************************************************
<function>
	Function	: TransformVector4_PRZW
	Parameters	:	In:  psA - First Matrix
					In:  psVec - Vector
					
	Description : Multiplies a vector4 by a matrix - creates high precision z and w.
				: NB: no difference in floating point
	Returns		: transformed vector4
<function/>
*********************************************************************************/
__inline void TransformVector4_PRZW(PVR_44_MATRIX *psA, 
									PVR_VECTOR4 *psVec, 
									PVR_VECTOR4 *psOut)
{
	psOut->x = Add4(Mul(psVec->x, psA->_11),
					Mul(psVec->y, psA->_21),
					Mul(psVec->z, psA->_31),
					Mul(psVec->w, psA->_41));
	psOut->y = Add4(Mul(psVec->x, psA->_12),
					Mul(psVec->y, psA->_22),
					Mul(psVec->z, psA->_32),
					Mul(psVec->w, psA->_42));
	psOut->z = Add4(MulPR(psVec->x, psA->_13),
					MulPR(psVec->y, psA->_23),
					MulPR(psVec->z, psA->_33),
					MulPR(psVec->w, psA->_43));
	psOut->w = Add4(MulPR(psVec->x, psA->_14),
					MulPR(psVec->y, psA->_24),
					MulPR(psVec->z, psA->_34),
					MulPR(psVec->w, psA->_44));
}

/********************************************************************************
<function>
	Function	: InverseMatrix
	Parameters	:	In:  psIn - Matrix to invert
					
	Description : This function uses Cramer's Rule to calculate the matrix inverse.
				  See nt\private\windows\opengl\serever\soft\so_math.c
	Returns		: 
<function/>
*********************************************************************************/

__inline void InverseMatrix(PVR_44_MATRIX *psIn, PVR_44_MATRIX *psOut)
{
    NTV_TYPE x00, x01, x02;
    NTV_TYPE x10, x11, x12;
    NTV_TYPE x20, x21, x22;
    NTV_TYPE rcp;
    NTV_TYPE x30, x31, x32;
    NTV_TYPE y01, y02, y03, y12, y13, y23;
    NTV_TYPE z02, z03, z12, z13, z22, z23, z32, z33;
	PVR_44_MATRIX* sIn;
	PVR_44_MATRIX sTemp;

#define x03 x01
#define x13 x11
#define x23 x21
#define x33 x31
#define z00 x02
#define z10 x12
#define z20 x22
#define z30 x32
#define z01 x03
#define z11 x13
#define z21 x23
#define z31 x33

	if (psIn == psOut)
	{
		sTemp = *psIn;
		sIn = &sTemp;
	}
	else
	{
		sIn = psIn;
	}

    /* read 1st two columns of matrix into registers */
    x00 = sIn->_11;
    x01 = sIn->_12;
    x10 = sIn->_21;
    x11 = sIn->_22;
    x20 = sIn->_31;
    x21 = sIn->_32;
    x30 = sIn->_41;
    x31 = sIn->_42;

    /* compute all six 2x2 determinants of 1st two columns */
    y01 = Sub(Mul(x00,x11), Mul(x10,x01));
    y02 = Sub(Mul(x00,x21), Mul(x20,x01));
    y03 = Sub(Mul(x00,x31), Mul(x30,x01));
    y12 = Sub(Mul(x10,x21), Mul(x20,x11));
    y13 = Sub(Mul(x10,x31), Mul(x30,x11));
    y23 = Sub(Mul(x20,x31), Mul(x30,x21));

    /* read 2nd two columns of matrix into registers */
    x02 = sIn->_13;
    x03 = sIn->_14;
    x12 = sIn->_23;
    x13 = sIn->_24;
    x22 = sIn->_33;
    x23 = sIn->_34;
    x32 = sIn->_43;
    x33 = sIn->_44;

    /* compute all 3x3 cofactors for 2nd two columns */
    z33 = Add(Sub(Mul(x02,y12), Mul(x12,y02)), Mul(x22,y01));
    z23 = Sub(Sub(Mul(x12,y03), Mul(x32,y01)), Mul(x02,y13));
    z13 = Add(Sub(Mul(x02,y23), Mul(x22,y03)), Mul(x32,y02));
    z03 = Sub(Sub(Mul(x22,y13), Mul(x32,y12)), Mul(x12,y23));
    z32 = Sub(Sub(Mul(x13,y02), Mul(x23,y01)), Mul(x03,y12));
    z22 = Add(Sub(Mul(x03,y13), Mul(x13,y03)), Mul(x33,y01));
    z12 = Sub(Sub(Mul(x23,y03), Mul(x33,y02)), Mul(x03,y23));
    z02 = Add(Sub(Mul(x13,y23), Mul(x23,y13)), Mul(x33,y12));

    /* compute all six 2x2 determinants of 2nd two columns */
    y01 = Sub(Mul(x02,x13), Mul(x12,x03));
    y02 = Sub(Mul(x02,x23), Mul(x22,x03));
    y03 = Sub(Mul(x02,x33), Mul(x32,x03));
    y12 = Sub(Mul(x12,x23), Mul(x22,x13));
    y13 = Sub(Mul(x12,x33), Mul(x32,x13));
    y23 = Sub(Mul(x22,x33), Mul(x32,x23));

    /* read 1st two columns of matrix in)to registers */
    x00 = sIn->_11;
    x01 = sIn->_12;
    x10 = sIn->_21;
    x11 = sIn->_22;
    x20 = sIn->_31;
    x21 = sIn->_32;
    x30 = sIn->_41;
    x31 = sIn->_42;

    /* compute all 3x3 cofactors for 1st column */
    z30 = Sub(Sub(Mul(x11,y02), Mul(x21,y01)), Mul(x01,y12));
    z20 = Add(Sub(Mul(x01,y13), Mul(x11,y03)), Mul(x31,y01));
    z10 = Sub(Sub(Mul(x21,y03), Mul(x31,y02)), Mul(x01,y23));
    z00 = Add(Sub(Mul(x11,y23), Mul(x21,y13)), Mul(x31,y12));

    /* compute 4x4 determinant & its reciprocal */
    rcp = Add4(Mul(x30,z30), Mul(x20,z20), Mul(x10,z10), Mul(x00,z00));
	if (rcp == 0)
	{
		////DPF("Singular matrix in MatrixInverse.\n");
		memset(psOut, 0, sizeof(PVR_44_MATRIX));
		psOut->_11 = psOut->_22 = psOut->_33 = psOut->_44 = D3DM_One;
		return;
	}
    rcp = Div(D3DM_One, rcp);

    /* compute all 3x3 cofactors for 2nd column */
    z31 = Add(Sub(Mul(x00,y12), Mul(x10,y02)), Mul(x20,y01));
    z21 = Sub(Sub(Mul(x10,y03), Mul(x30,y01)), Mul(x00,y13));
    z11 = Add(Sub(Mul(x00,y23), Mul(x20,y03)), Mul(x30,y02));
    z01 = Sub(Sub(Mul(x20,y13), Mul(x30,y12)), Mul(x10,y23));

    /* multiply all 3x3 cofactors by reciprocal */
    psOut->_11 = Mul(z00,rcp);
    psOut->_21 = Mul(z01,rcp);
    psOut->_12 = Mul(z10,rcp);
    psOut->_31 = Mul(z02,rcp);
    psOut->_13 = Mul(z20,rcp);
    psOut->_41 = Mul(z03,rcp);
    psOut->_14 = Mul(z30,rcp);
    psOut->_22 = Mul(z11,rcp);
    psOut->_32 = Mul(z12,rcp);
    psOut->_23 = Mul(z21,rcp);
    psOut->_42 = Mul(z13,rcp);
    psOut->_24 = Mul(z31,rcp);
    psOut->_33 = Mul(z22,rcp);
    psOut->_43 = Mul(z23,rcp);
    psOut->_34 = Mul(z32,rcp);
    psOut->_44 = Mul(z33,rcp);
}



/********************************************************************************
<function>
	Function	: TransposeMatrix
	Parameters	:	In:  psIn - Matrix to invert
					
	Description : transposes the input matrix
	Returns		: 
<function/>
*********************************************************************************/
__inline void TransposeMatrix(PVR_44_MATRIX *psIn, PVR_44_MATRIX *psOut)
{
	PVR_44_MATRIX sTemp;
	sTemp._11 = psIn->_11;
	sTemp._22 = psIn->_22;
	sTemp._33 = psIn->_33;
	sTemp._44 = psIn->_44;
	sTemp._21 = psIn->_12;
	sTemp._31 = psIn->_13;
	sTemp._41 = psIn->_14;
	sTemp._12 = psIn->_21;
	sTemp._32 = psIn->_23;
	sTemp._42 = psIn->_24;
	sTemp._13 = psIn->_31;
	sTemp._23 = psIn->_32;
	sTemp._43 = psIn->_34;
	sTemp._14 = psIn->_41;
	sTemp._24 = psIn->_42;
	sTemp._34 = psIn->_43;

	*psOut = sTemp;
}

#endif // _NTVMATH_H_
/*****************************************************************************
 End of file (NTVMath.h)
*****************************************************************************/