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;
}