Blade Math Lib数学库

BMatirx.cpp矩阵运算文件内容

#include "BMath2.h"
//#include <xmmintrin.h>

 B_FLOAT DetVal(B_FLOAT* m)
{
	B_FLOAT a = m[0] * (m[4] * m[8] - m[7] * m[5]) ;
	B_FLOAT b = m[1] * (m[3] * m[8] - m[5] * m[6]);
	B_FLOAT c = m[2] * (m[3] * m[7] - m[4] * m[6]);

	return  a - b + c;
}

BMatrix::BMatrix()
{
	int n = 0;
	for(n=0; n<16; n++)
		bm[n] = 0.0f;

	bm[0] =  1.0f;
	bm[5] =  1.0f;
	bm[10] = 1.0f;
	bm[15] = 1.0f;
}

BMatrix::BMatrix(B_FLOAT* val)
{
	int n = 0;
	for(n=0; n<16; n++)
		bm[n] = val[n];
}


BMatrix BMatrix::operator +=( BMatrix m )
{
	int n = 0;
	for(n=0; n<4; n++)
		BAdd4(&bm[n*4],&bm[n*4],&m.bm[n*4]);

	return *this;
}

BMatrix BMatrix::operator -=( BMatrix m )
{
	int n = 0;
	for(n=0; n<4; n++)
		BSub4(&bm[n*4],&bm[n*4],&m.bm[n*4]);

	return *this;
}

BMatrix BMatrix::operator *=( B_FLOAT v)
{
	int n = 0;
	for(n=0; n<4; n++)
		BMul4(&bm[n*4],&bm[n*4],v);

	return *this;
}

BMatrix BMatrix::operator /=( B_FLOAT v)
{
	int n = 0;
	for(n=0; n<4; n++)
		BDiv4(&bm[n*4],&bm[n*4],v);

	return *this;
}

BMatrix BMatrix::operator +( BMatrix m )
{
	BMatrix tm;

	int n = 0;
	for(n=0; n<4; n++)
		BAdd4(&tm.bm[n*4],&bm[n*4],&m.bm[n*4]);

	return tm;
}

BMatrix BMatrix::operator -( BMatrix m )
{
	BMatrix tm;

	int n = 0;
	for(n=0; n<4; n++)
		BSub4(&tm.bm[n*4],&bm[n*4],&m.bm[n*4]);

	return tm;
}

BMatrix BMatrix::operator *( B_FLOAT v)
{
	BMatrix tm;

	int n = 0;
	for(n=0; n<4; n++)
		BMul4(&tm.bm[n*4],&bm[n*4],v);

	return tm;
}

BMatrix BMatrix::operator /( B_FLOAT v)
{
	BMatrix tm;

	int n = 0;
	for(n=0; n<4; n++)
		BDiv4(&tm.bm[n*4],&bm[n*4],v);

	return tm;
}

bool BMatrix::operator ==( BMatrix m)
{
	int n = 0;

	for(n=0;n<16;n++)
	{
		if(bm[n] != m.bm[n])
			return false;
	}

	return true;
}

bool BMatrix::operator !=( BMatrix m)
{
	int n = 0;

	for(n=0;n<16;n++)
	{
		if(bm[n] == m.bm[n])
			return false;
	}

	return true;
}

B_FLOAT BMatrix::operator ()(int n)
{
	return bm[n];
}

B_FLOAT BMatrix::operator ()(int row,int col)
{
	return bm[row*4+col];
}


void BMatrix::Identity()
{
	int n = 0;
	for(n=0; n<16; n++)
		bm[n] = 0.0f;

	bm[0] =  1.0f;
	bm[5] =  1.0f;
	bm[10] = 1.0f;
	bm[15] = 1.0f;
}


BMatrix*    BMatrixMultiply(BMatrix* dest,BMatrix* src1,BMatrix* src2)
{
    /*
    BMatrix mat = BMatrix();

    __m128 a;
    __m128 b[4];
    __m128 c,d;

    b[0] = _mm_load_ps(&(src2->bm[0]));
    b[1] = _mm_load_ps(&(src2->bm[4]));
    b[2] = _mm_load_ps(&(src2->bm[8]));
    b[3] = _mm_load_ps(&(src2->bm[12]));

    _MM_TRANSPOSE4_PS(b[0],b[1],b[2],b[3]);

    int m=0,n=0;

    for(m=0;m<4;m++)
    {
        a = _mm_load_ps(&(src1->bm[m*4]));
        d = _mm_load_ps(&(mat.bm[m*4]));

        for(n=0;n<4;n++)
        {
            c = _mm_mul_ps(a,b[n]);
            d = _mm_add_ps(d,c);
        }
        _mm_store_ps(&(mat.bm[m*4]),d);
    }

    //(*dest) = mat;
*/


    BMatrix mat;
	mat.bm[0] = B_FLOAT(double(src1->bm[0]) * double(src2->bm[0]) + double(src1->bm[1]) * double(src2->bm[4]) + double(src1->bm[2]) * double(src2->bm[8]) + double(src1->bm[3]) * double(src2->bm[12])) ;
	mat.bm[1] = B_FLOAT(double(src1->bm[0]) * double(src2->bm[1]) + double(src1->bm[1])* double(src2->bm[5]) + double(src1->bm[2]) * double(src2->bm[9]) + double(src1->bm[3]) * double(src2->bm[13])) ;
	mat.bm[2] = B_FLOAT(double(src1->bm[0]) * double(src2->bm[2]) + double(src1->bm[1]) * double(src2->bm[6]) + double(src1->bm[2]) * double(src2->bm[10]) + double(src1->bm[3]) * double(src2->bm[14])) ;
	mat.bm[3] = B_FLOAT(double(src1->bm[0]) * double(src2->bm[3]) + double(src1->bm[1]) * double(src2->bm[7]) + double(src1->bm[2]) * double(src2->bm[11]) + double(src1->bm[3]) * double(src2->bm[15])) ;

	mat.bm[4] = B_FLOAT(double(src1->bm[4]) * double(src2->bm[0]) + double(src1->bm[5]) * double(src2->bm[4]) + double(src1->bm[6]) * double(src2->bm[8]) + double(src1->bm[7]) * double(src2->bm[12])) ;
	mat.bm[5] = B_FLOAT(double(src1->bm[4]) * double(src2->bm[1]) + double(src1->bm[5]) * double(src2->bm[5]) + double(src1->bm[6]) * double(src2->bm[9]) + double(src1->bm[7]) * double(src2->bm[13])) ;
	mat.bm[6] = B_FLOAT(double(src1->bm[4]) * double(src2->bm[2]) + double(src1->bm[5]) * double(src2->bm[6]) + double(src1->bm[6]) * double(src2->bm[10]) + double(src1->bm[7]) * double(src2->bm[14])) ;
	mat.bm[7] = B_FLOAT(double(src1->bm[4]) * double(src2->bm[3]) + double(src1->bm[5]) * double(src2->bm[7]) + double(src1->bm[6]) * double(src2->bm[11]) + double(src1->bm[7]) * double(src2->bm[15])) ;

	mat.bm[8] = B_FLOAT(double(src1->bm[8]) * double(src2->bm[0]) + double(src1->bm[9]) * double(src2->bm[4]) + double(src1->bm[10]) * double(src2->bm[8]) + double(src1->bm[11]) * double(src2->bm[12])) ;
	mat.bm[9] = B_FLOAT(double(src1->bm[8]) * double(src2->bm[1]) + double(src1->bm[9]) * double(src2->bm[5]) + double(src1->bm[10]) * double(src2->bm[9]) + double(src1->bm[11]) * double(src2->bm[13])) ;
	mat.bm[10] = B_FLOAT(double(src1->bm[8]) * double(src2->bm[2]) + double(src1->bm[9]) * double(src2->bm[6]) + double(src1->bm[10]) * double(src2->bm[10]) + double(src1->bm[11]) * double(src2->bm[14])) ;
	mat.bm[11] = B_FLOAT(double(src1->bm[8]) * double(src2->bm[3]) + double(src1->bm[9]) * double(src2->bm[7]) + double(src1->bm[10]) * double(src2->bm[11]) + double(src1->bm[11]) * double(src2->bm[15])) ;

	mat.bm[12] = B_FLOAT(double(src1->bm[12]) * double(src2->bm[0]) + double(src1->bm[13]) * double(src2->bm[4]) + double(src1->bm[14]) * double(src2->bm[8]) + double(src1->bm[15]) * double(src2->bm[12])) ;
	mat.bm[13] = B_FLOAT(double(src1->bm[12]) * double(src2->bm[1]) + double(src1->bm[13]) * double(src2->bm[5]) + double(src1->bm[14]) * double(src2->bm[9]) + double(src1->bm[15]) * double(src2->bm[13])) ;
	mat.bm[14] = B_FLOAT(double(src1->bm[12]) * double(src2->bm[2]) + double(src1->bm[13]) * double(src2->bm[6]) + double(src1->bm[14]) * double(src2->bm[10]) + double(src1->bm[15]) * double(src2->bm[14])) ;
	mat.bm[15] = B_FLOAT(double(src1->bm[12]) * double(src2->bm[3]) + double(src1->bm[13]) * double(src2->bm[7]) + double(src1->bm[14]) * double(src2->bm[11]) + double(src1->bm[15]) * double(src2->bm[15])) ;

    int n=0;

    for(n=0;n<16;n++)
       dest->bm[n] = mat.bm[n];

	return dest;
}


BMatrix*	BMatrixLookAtRH(BMatrix* dest,BVector3* pEye,BVector3* pAt,BVector3* pUp)
{
	BVector3 xaxis,yaxis,zaxis;
    zaxis = *pEye - *pAt;
	BVec3Normalize(&zaxis,&zaxis);

	BVec3Cross(&xaxis,pUp,&zaxis);
	BVec3Normalize(&xaxis,&xaxis);

	BVec3Cross(&yaxis,&zaxis,&xaxis);

	dest->bm[0] = xaxis._x;							dest->bm[1] = yaxis._x;							dest->bm[2] = zaxis._x;							dest->bm[3] = 0;
	dest->bm[4] = xaxis._y;							dest->bm[5] = yaxis._y;							dest->bm[6] = zaxis._y;							dest->bm[7] = 0;
	dest->bm[8] = xaxis._z;							dest->bm[9] = yaxis._z;							dest->bm[10] = zaxis._z;						dest->bm[11] = 0;
	dest->bm[12] = -BVec3Dot(&xaxis,pEye);			dest->bm[13] = -BVec3Dot(&yaxis,pEye);			dest->bm[14] = -BVec3Dot(&zaxis,pEye);			dest->bm[15] = 1;

	return dest;
}

BMatrix*	BMatrixLookAtLH(BMatrix* dest,BVector3* pEye,BVector3* pAt,BVector3* pUp)
{
	BVector3 xaxis,yaxis,zaxis;
    zaxis = *pAt - *pEye;
	BVec3Normalize(&zaxis,&zaxis);
	BVec3Cross(&xaxis,pUp,&zaxis);
	BVec3Normalize(&xaxis,&xaxis);

	BVec3Cross(&yaxis,&zaxis,&xaxis);

	dest->bm[0] = xaxis._x;								dest->bm[1] = yaxis._x;								dest->bm[2] = zaxis._x;								dest->bm[3] = 0;
	dest->bm[4] = xaxis._y;								dest->bm[5] = yaxis._y;								dest->bm[6] = zaxis._y;								dest->bm[7] = 0;
	dest->bm[8] = xaxis._z;								dest->bm[9] = yaxis._z;								dest->bm[10] = zaxis._z;							dest->bm[11] = 0;
	dest->bm[12] = -BVec3Dot(&xaxis,pEye);				dest->bm[13] = -BVec3Dot(&yaxis,pEye);				dest->bm[14] = -BVec3Dot(&zaxis,pEye);				dest->bm[15] = 1;

	return dest;
}

BMatrix*	BMatrixOrthoLH(BMatrix* dest,B_FLOAT w,B_FLOAT h,B_FLOAT zn,B_FLOAT zf)
{
	dest->bm[0] = 2.0f/w;				dest->bm[1] = 0.0f;				dest->bm[2] = 0.0f;							dest->bm[3] = 0.0f;
	dest->bm[4] = 0.0f;					dest->bm[5] = 2.0f/h;			dest->bm[6] = 0.0f;							dest->bm[7] = 0.0f;
	dest->bm[8] = 0.0f;					dest->bm[9] = 0.0f;				dest->bm[10] = 1.0f/(zf-zn);				dest->bm[11] = 0.0f;
	dest->bm[12] = 0.0f;				dest->bm[13] = 0.0f;			dest->bm[14] = -zn/(zf-zn);					dest->bm[15] = 1.0f;
	return dest;
}

BMatrix*	BMatrixOrthoRH(BMatrix* dest,B_FLOAT w,B_FLOAT h,B_FLOAT zn,B_FLOAT zf)
{
	dest->bm[0] = 2.0f/w;				dest->bm[1] = 0.0f;				dest->bm[2] = 0.0f;							dest->bm[3] = 0.0f;
	dest->bm[4] = 0.0f;					dest->bm[5] = 2.0f/h;			dest->bm[6] = 0.0f;							dest->bm[7] = 0.0f;
	dest->bm[8] = 0.0f;					dest->bm[9] = 0.0f;				dest->bm[10] = 1.0f/(zn-zf);				dest->bm[11] = 0.0f;
	dest->bm[12] = 0.0f;				dest->bm[13] = 0.0f;			dest->bm[14] = zn/(zn-zf);					dest->bm[15] = 1.0f;
	return dest;
}

BMatrix*	BMatrixPerspectiveLH(BMatrix* dest,B_FLOAT w,B_FLOAT h,B_FLOAT zn,B_FLOAT zf)
{
	dest->bm[0] = 2.0f*zn/w;				dest->bm[1] = 0.0f;							dest->bm[2] = 0.0f;									dest->bm[3] = 0.0f;
	dest->bm[4] = 0.0f;						dest->bm[5] = 2.0f*zn/h;					dest->bm[6] = 0.0f;									dest->bm[7] = 0.0f;
	dest->bm[8] = 0.0f;						dest->bm[9] = 0.0f;							dest->bm[10] = zf/(zf-zn);							dest->bm[11] = 1.0f;
	dest->bm[12] = 0.0f;					dest->bm[13] = 0.0f;						dest->bm[14] = (zn * zf)/(zn - zf);					dest->bm[15] = 0.0f;
	return dest;
}

BMatrix*	BMatrixPerspectiveRH(BMatrix* dest,B_FLOAT w,B_FLOAT h,B_FLOAT zn,B_FLOAT zf)
{
	dest->bm[0] = 2.0f*zn/w;				dest->bm[1] = 0.0f;							dest->bm[2] = 0.0f;									dest->bm[3] = 0.0f;
	dest->bm[4] = 0.0f;						dest->bm[5] = 2.0f*zn/h;					dest->bm[6] = 0.0f;									dest->bm[7] = 0.0f;
	dest->bm[8] = 0.0f;						dest->bm[9] = 0.0f;							dest->bm[10] =zf/(zn-zf);							dest->bm[11] = -1.0f;
	dest->bm[12] = 0.0f;					dest->bm[13] = 0.0f;						dest->bm[14] = (zn*zf)/(zn-zf);						dest->bm[15] = 0.0f;
	return dest;
}

BMatrix*	BMatrixPerspectiveFovLH(BMatrix* dest,B_FLOAT fovy,B_FLOAT aspect,B_FLOAT zn,B_FLOAT zf)
{
	B_FLOAT yscale = tan(B_PI/2-fovy/2);
	B_FLOAT xscale = yscale / aspect;

	dest->bm[0] = xscale;					dest->bm[1] = 0.0f;							dest->bm[2] = 0.0f;									dest->bm[3] = 0.0f;
	dest->bm[4] = 0.0f;						dest->bm[5] = yscale;						dest->bm[6] = 0.0f;									dest->bm[7] = 0.0f;
	dest->bm[8] = 0.0f;						dest->bm[9] = 0.0f;							dest->bm[10] = zf/(zf-zn);							dest->bm[11] = 1.0f;
	dest->bm[12] = 0.0f;					dest->bm[13] = 0.0f;						dest->bm[14] = -(zn * zf)/(zf - zn);				dest->bm[15] = 0.0f;
	return dest;
}

BMatrix*	BMatrixPerspectiveFovRH(BMatrix* dest,B_FLOAT fovy,B_FLOAT aspect,B_FLOAT zn,B_FLOAT zf)
{
	B_FLOAT yscale = tan(B_PI/2-fovy/2);
	B_FLOAT xscale = yscale / aspect;

	dest->bm[0] = xscale;					dest->bm[1] = 0.0f;							dest->bm[2] = 0.0f;									dest->bm[3] = 0.0f;
	dest->bm[4] = 0.0f;						dest->bm[5] = yscale;						dest->bm[6] = 0.0f;									dest->bm[7] = 0.0f;
	dest->bm[8] = 0.0f;						dest->bm[9] = 0.0f;							dest->bm[10] =zf/(zn-zf);							dest->bm[11] = -1.0f;
	dest->bm[12] = 0.0f;					dest->bm[13] = 0.0f;						dest->bm[14] = (zn*zf)/(zn-zf);						dest->bm[15] = 0.0f;
	return dest;
}

BMatrix*	BMatrixTranslation(BMatrix* dest,B_FLOAT x,B_FLOAT y,B_FLOAT z)
{
	dest->Identity();

	dest->bm[12] = x;
	dest->bm[13] = y;
	dest->bm[14] = z;

	return dest;
}

BMatrix*    BMatrixScaling(BMatrix* dest,B_FLOAT x,B_FLOAT y,B_FLOAT z)
{
	dest->Identity();

	dest->bm[0] = x;
	dest->bm[5] = y;
	dest->bm[10] = z;

	return dest;
}

BMatrix*	BMatrixRotateX(BMatrix* dest,B_FLOAT angle)
{
	B_FLOAT a = sin(angle);
	B_FLOAT b = cos(angle);

	dest->Identity();

	dest->bm[5] = b;
	dest->bm[6] = a;
	dest->bm[9] = -a;
	dest->bm[10] = b;

	return dest;
}

BMatrix*	BMatrixRotateY(BMatrix* dest,B_FLOAT angle)
{
	B_FLOAT a = sin(angle);
	B_FLOAT b = cos(angle);

	dest->Identity();

	dest->bm[0] = b;
	dest->bm[2] = -a;
	dest->bm[8] = a;
	dest->bm[10] = b;

	return dest;
}

BMatrix*	BMatrixRotateZ(BMatrix* dest,B_FLOAT angle)
{
	B_FLOAT a = sin(angle);
	B_FLOAT b = cos(angle);

	dest->Identity();

	dest->bm[0] = b;
	dest->bm[1] = a;
	dest->bm[4] = -a;
	dest->bm[5] = b;

	return dest;
}

BMatrix*	BMatrixTranspose(BMatrix* dest,BMatrix* src)
{
	dest->_11 = src->_11;   dest->_12 = src->_21;   dest->_13 = src->_31;   dest->_14 = src->_41;
	dest->_21 = src->_12;   dest->_22 = src->_22;   dest->_23 = src->_32;   dest->_24 = src->_42;
	dest->_31 = src->_13;   dest->_32 = src->_23;   dest->_33 = src->_33;   dest->_34 = src->_43;
	dest->_41 = src->_14;   dest->_42 = src->_24;   dest->_43 = src->_34;   dest->_44 = src->_44;
	return dest;
}

BMatrix*	BMatrixInverse(BMatrix* dest,BMatrix* src)
{
	B_FLOAT A1[] = {src->bm[5],src->bm[6],src->bm[7],
				 src->bm[9],src->bm[10],src->bm[11],
				 src->bm[13],src->bm[14],src->bm[15]};


	B_FLOAT A2[] = {src->bm[1],src->bm[2],src->bm[3],
				 src->bm[9],src->bm[10],src->bm[11],
				 src->bm[13],src->bm[14],src->bm[15]};


	B_FLOAT A3[] = {src->bm[1],src->bm[2],src->bm[3],
				 src->bm[5],src->bm[6],src->bm[7],
				 src->bm[13],src->bm[14],src->bm[15]};


	B_FLOAT A4[] = {src->bm[1],src->bm[2],src->bm[3],
				 src->bm[5],src->bm[6],src->bm[7],
				 src->bm[9],src->bm[10],src->bm[11]};

	B_FLOAT B1[] = {src->bm[4],src->bm[6],src->bm[7],
				 src->bm[8],src->bm[10],src->bm[11],
				 src->bm[12],src->bm[14],src->bm[15]};


	B_FLOAT B2[] = {src->bm[0],src->bm[2],src->bm[3],
				 src->bm[8],src->bm[10],src->bm[11],
				 src->bm[12],src->bm[14],src->bm[15]};

	B_FLOAT B3[] = {src->bm[0],src->bm[2],src->bm[3],
				 src->bm[4],src->bm[6],src->bm[7],
				 src->bm[12],src->bm[14],src->bm[15]};

	B_FLOAT B4[] = {src->bm[0],src->bm[2],src->bm[3],
				 src->bm[4],src->bm[6],src->bm[7],
				 src->bm[8],src->bm[10],src->bm[11]};

	B_FLOAT C1[] = {src->bm[4],src->bm[5],src->bm[7],
				 src->bm[8],src->bm[9],src->bm[11],
				 src->bm[12],src->bm[13],src->bm[15]};

	B_FLOAT C2[] = {src->bm[0],src->bm[1],src->bm[3],
				 src->bm[8],src->bm[9],src->bm[11],
				 src->bm[12],src->bm[13],src->bm[15]};

	B_FLOAT C3[] = {src->bm[0],src->bm[1],src->bm[3],
				 src->bm[4],src->bm[5],src->bm[7],
				 src->bm[12],src->bm[13],src->bm[15]};

	B_FLOAT C4[] = {src->bm[0],src->bm[1],src->bm[3],
				 src->bm[4],src->bm[5],src->bm[7],
				 src->bm[8],src->bm[9],src->bm[11]};

	B_FLOAT D1[] = {src->bm[4],src->bm[5],src->bm[6],
				 src->bm[8],src->bm[9],src->bm[10],
				 src->bm[12],src->bm[13],src->bm[14]};

	B_FLOAT D2[] = {src->bm[0],src->bm[1],src->bm[2],
				 src->bm[8],src->bm[9],src->bm[10],
				 src->bm[12],src->bm[13],src->bm[14]};

	B_FLOAT D3[] = {src->bm[0],src->bm[1],src->bm[2],
				 src->bm[4],src->bm[5],src->bm[6],
				 src->bm[12],src->bm[13],src->bm[14]};

	B_FLOAT D4[] = {src->bm[0],src->bm[1],src->bm[2],
				 src->bm[4],src->bm[5],src->bm[6],
				 src->bm[8],src->bm[9],src->bm[10]};


	B_FLOAT det = src->bm[0]*DetVal(A1) - (src->bm[4]*DetVal(A2)) + (src->bm[8]*DetVal(A3)) - (src->bm[12]*DetVal(A4));

	if(det == 0.0f)
		det = 1.0f;


	dest->bm[0] = DetVal(A1);	dest->bm[1] = -DetVal(A2);	dest->bm[2] = DetVal(A3);	dest->bm[3] = -DetVal(A4);
	dest->bm[4] = -DetVal(B1);	dest->bm[5] = DetVal(B2);	dest->bm[6] = -DetVal(B3);	dest->bm[7] = DetVal(B4);
	dest->bm[8] = DetVal(C1);	dest->bm[9] = -DetVal(C2);	dest->bm[10] = DetVal(C3);	dest->bm[11] = -DetVal(C4);
	dest->bm[12] = -DetVal(D1);	dest->bm[13] = DetVal(D2);	dest->bm[14] = -DetVal(D3);	dest->bm[15] = DetVal(D4);

	*dest /=det;

	return dest;
}


BVector3*    BMatrixUnProj(BVector3 pt,BMatrix* matView,BMatrix* matProj,int* viewport,BVector3* dest)
{
    BMatrix tm;
    BMatrix itm;

    BMatrixMultiply(&tm,matView,matProj);
    BMatrixInverse(&itm,&tm);

	BVector4 t,t1;

    t._x = pt._x;
	t._y = pt._y;
	t._z = pt._z;
	t._w = 1;


    /* Map x and y from window coordinates */
    t._x = (t._x - (viewport[0])) / (viewport[2]);
    t._y = (t._y + (viewport[1])) / (viewport[3]);

    /* Map to range -1 to 1 */
    t._x = t._x * 2.0f - 1;
    t._y = t._y *2.0f - 1;
    t._z = t._z * 2.0f - 1;

    BVec4Transform(&t1,&t,&itm);

    *dest = BVector3(t1._x,t1._y,t1._z);
    if(t1._w!=0.0)
    {
        dest->_x = t1._x / t1._w;
        dest->_y = t1._y / t1._w;
        dest->_z = t1._z / t1._w;
    }

    return dest;
}