图形学矩阵、向量类C++实现

#ifndef _MATRIX4_H_ #define _MATRIX4_H_ #include <iostream> #include "Vector4.h" #include "Vector3.h" #include <math.h> using namespace std; template <class T> class Matrix4 { public: Matrix4<T>(); Matrix4<T>(T m0,T m1,T m2,T m3, T m4,T m5,T m6,T m7, T m8,T m9,T m10,T m11, T m12,T m13,T m14,T m15); Matrix4<T>(T arr[16]); inline void setIdentity(); inline void setInverse(); inline void setValue(T m0,T m1,T m2,T m3, T m4,T m5,T m6,T m7, T m8,T m9,T m10,T m11, T m12,T m13,T m14,T m15); inline void displayValue(); inline void setTranspose(); inline Matrix4<T> &operator=(const Matrix4<T> &mat); inline Matrix4<T> operator*(const Matrix4<T> &mat); inline bool operator==(const Matrix4<T> &mat)const; inline bool operator!=(const Matrix4<T> &mat)const; inline Matrix4<T> operator+(const Matrix4<T> &mat)const; inline Matrix4<T> operator-(const Matrix4<T> &mat)const; inline void setTranslation(const Vector3<T> &vec); inline void setRotate(float angle,float x,float y,float z); inline void setScale(T x, T y, T z); public: __declspec(align(16)) T m[16]; //the friend funciton is not the member function of the Matrix4, but the have friend declare //so the function can access the private member of the Class Mattix4 friend Vector4<T> operator*(const Vector4<T> &vec,const Matrix4<T> &mat) { return Vector4<T>(vec.x*mat.m[0] + vec.y *mat.m[1] + vec.z *mat.m[2] + vec.w *mat.m[3], vec.x*mat.m[4] + vec.y *mat.m[5] + vec.z *mat.m[6] + vec.w *mat.m[7], vec.x*mat.m[8] + vec.y *mat.m[9] + vec.z *mat.m[10] + vec.w *mat.m[11], vec.x*mat.m[12] + vec.y *mat.m[13] + vec.z *mat.m[14] + vec.w *mat.m[15]); } friend Vector4<T> operator*(const Matrix4<T> &mat,const Vector4<T> &vec) { return Vector4<T>(vec.x*mat.m[0] + vec.y *mat.m[4] + vec.z *mat.m[8] + vec.w *mat.m[12], vec.x*mat.m[1] + vec.y *mat.m[5] + vec.z *mat.m[9] + vec.w *mat.m[13], vec.x*mat.m[2] + vec.y *mat.m[6] + vec.z *mat.m[10] + vec.w *mat.m[14], vec.x*mat.m[3] + vec.y *mat.m[7] + vec.z *mat.m[11] + vec.w *mat.m[15]); } friend ostream& operator << (ostream& os,const Matrix4<T> &mat) { for( int i = 0 ; i< 4; i++) { os<<mat.m[i]<<" "<<mat.m[i+4]<<" "<<mat.m[i+8]<<" "<<mat.m[i+12]<<endl; } return os; } friend Matrix4<T> operator*(const T &s, const Matrix4<T> &mat) { Matrix4<T> ret; for( int i = 0; i < 16; i++) ret.m[i] = s * (mat.m[i]); return ret; } }; template<class T> inline Matrix4<T>::Matrix4() { setIdentity(); } template<class T> inline Matrix4<T>::Matrix4(T m0, T m1, T m2, T m3, T m4, T m5, T m6, T m7, T m8, T m9, T m10, T m11, T m12, T m13, T m14, T m15) { m[0] = m0;m[1] = m1;m[2] = m2;m[3] = m3; m[4] = m4;m[5] = m5;m[6] = m6;m[7] = m7; m[8] = m8;m[9] = m9;m[10] = m10;m[11] = m11; m[12] = m12;m[13] = m13;m[14] = m14;m[15] = m15; } template<class T> inline Matrix4<T>::Matrix4(T arr[16]) { for(int i = 0; i < 16; i++) { m[i] = arr[i]; } } template<class T> inline void Matrix4<T>::setIdentity() { for(int i = 0; i < 16; i++) { m[i] = 0; } m[0] = 1; m[5]=1; m[10] = 1; m[15] =1; } template<class T> inline void Matrix4<T>::displayValue() { for( int i = 0 ; i< 4; i++) { cout<<m[i]<<" "<<m[i+4]<<" "<<m[i+8]<<" "<<m[i+12]<<endl; } } template<class T> inline Matrix4<T>& Matrix4<T>::operator =(const Matrix4<T> &mat) { memcpy(m,mat.m,sizeof(T)*16); return *this; } template<class T> inline bool Matrix4<T>::operator==(const Matrix4<T> &mat)const { int i=0; while( m[i++] == mat.m[i++] ) { if(i==16) break; }; return (i==16)?true:false; } template<class T> inline bool Matrix4<T>::operator!=(const Matrix4<T> &mat)const { int i=0; while( m[i++] == mat.m[i++] ) { if(i==16) break; }; return (i==16)?false:true; } template<class T> inline void Matrix4<T>::setValue(T m0, T m1, T m2, T m3, T m4, T m5, T m6, T m7, T m8, T m9, T m10, T m11, T m12, T m13, T m14, T m15) { m[0] = m0;m[1] = m1;m[2] = m2;m[3] = m3; m[4] = m4;m[5] = m5;m[6] = m6;m[7] = m7; m[8] = m8;m[9] = m9;m[10] = m10;m[11] = m11; m[12] = m12;m[13] = m13;m[14] = m14;m[15] = m15; } template<class T> inline Matrix4<T> Matrix4<T>::operator+(const Matrix4<T> &mat)const { return Matrix4<T>(m[0]+mat.m[0],m[1]+mat.m[1],m[2]+mat.m[2],m[3]+mat.m[3], m[4]+mat.m[4],m[5]+mat.m[5],m[6]+mat.m[6],m[7]+mat.m[7], m[8]+mat.m[8],m[9]+mat.m[9],m[10]+mat.m[10],m[11]+mat.m[11], m[12]+mat.m[12],m[13]+mat.m[13],m[14]+mat.m[14],m[15]+mat.m[15]); } template<class T> inline Matrix4<T> Matrix4<T>::operator-(const Matrix4<T> &mat)const { return Matrix4<T>(m[0]-mat.m[0],m[1]-mat.m[1],m[2]-mat.m[2],m[3]-mat.m[3], m[4]-mat.m[4],m[5]-mat.m[5],m[6]-mat.m[6],m[7]-mat.m[7], m[8]-mat.m[8],m[9]-mat.m[9],m[10]-mat.m[10],m[11]-mat.m[11], m[12]-mat.m[12],m[13]-mat.m[13],m[14]-mat.m[14],m[15]-mat.m[15]); } template<class T> inline void Matrix4<T>::setInverse() { T src[16]; for(int i = 0; i< 16; i++) { src[i] = m[i]; } this->setIdentity(); for(int i=0; i<4; i++) { int curr = i; //当前处理的行号 int col = i+1; //需要消除的初行 int j = i*5; //对角元素脚标 /*cout <<"------------------"<<endl; cout << "this is time:"<< i<<endl;*/ //find the max of the i column int max=j; for(int k = j+1; k<j+4-i; k++) { if(abs(src[k])> abs(src[max])) max = k; } //cout<<"the current max sub is "<<max<<" "<<src[max]<<endl; //由对角元素脚标取得列号 max = max%4; //cout<<"the max row is "<<max<<endl; //inter the max row and i row if( max != i) { for ( int k = 0,p=i; k< 4; k++) { T temp1 = src[p]; src[p] = src[max]; src[max] = temp1; //对应单位矩阵变换 T temp2 = m[p]; m[p] = m[max]; m[max] = temp2; p+=4; max+=4; } } T tempMax=src[j]; //除以对角元素化为1 for( int k=0;k<4;k++) { src[curr]=src[curr]/tempMax; m[curr] = m[curr]/tempMax; curr+=4; } //消除i-> 3行对应元素为0 int r = j+1; for(int k = 0; k <3-i; k++) { r = j+k+1; T temp = -src[r]; col = i+k+1; curr = i; //cout<<"deal with the col is "<<col<<endl; for(int n=0;n<4;n++) { src[col] = temp*src[curr] + src[col]; m[col] = temp*m[curr]+m[col]; curr+=4; col+=4; } } } for(int i=3; i>0; i--) { //cout<<"the begin right is " << i<<endl; int curr = i; int col = i-1; //需要处理i行 for( int j=i-1;j>=0; j--) { int r = i*5-(i-j); T temp = -src[r];//得到上三角对角元素该列的值 col=j; curr=i; //cout<<"the col "<<col<<endl; for( int n=0;n<4;n++) { src[col] = temp * src[curr] + src[col]; m[col] = temp * m[curr] + m[col]; col+=4; curr+=4; } } } } template<class T> inline void Matrix4<T>::setTranspose() { T src[16]; for(int i = 0; i < 16; i++) { src[i] = m[i]; } setIdentity(); for( int i = 0,j = 0; i < 4; i++) { m[i] = src[j++]; m[i+4] = src[j++]; m[i+8] = src[j++]; m[i+12] = src[j++]; } } template <class T> inline Matrix4<T> Matrix4<T>::operator *(const Matrix4<T> &mat) { return Matrix4<T>( m[0]*mat.m[0]+m[4]*mat.m[1]+m[8]*mat.m[2]+m[12]*mat.m[3], m[1]*mat.m[0]+m[5]*mat.m[1]+m[9]*mat.m[2]+m[13]*mat.m[3], m[2]*mat.m[0]+m[6]*mat.m[1]+m[10]*mat.m[2]+m[14]*mat.m[3], m[3]*mat.m[0]+m[7]*mat.m[1]+m[11]*mat.m[2]+m[15]*mat.m[3], m[0]*mat.m[4]+m[4]*mat.m[5]+m[8]*mat.m[6]+m[12]*mat.m[7], m[1]*mat.m[4]+m[5]*mat.m[5]+m[9]*mat.m[6]+m[13]*mat.m[7], m[2]*mat.m[4]+m[6]*mat.m[5]+m[10]*mat.m[6]+m[14]*mat.m[7], m[3]*mat.m[4]+m[7]*mat.m[5]+m[11]*mat.m[6]+m[15]*mat.m[7], m[0]*mat.m[8]+m[4]*mat.m[9]+m[8]*mat.m[10]+m[12]*mat.m[11],