?? xmath.h
字號:
// 點積
//
template<class T>
T operator * (const CVector<T,2> &v1, const CVector<T,2> &v2)
{
CVector<T,2> r=v1;
return r.Dot(v2);
}
template<class T>
T operator * (const CVector<T,3> &v1, const CVector<T,3> &v2)
{
CVector<T,3> r=v1;
return r.Dot(v2);
}
template<class T>
T operator * (const CHomoVector4D<T> &v1, const CHomoVector4D<T> &v2)
{
CHomoVector4D<T> r=v1;
return r.Dot(v2);
}
//
// 數(shù)量積
//
template<class T>
CVector<T,2> operator * (const CVector<T,2> &v1, T val)
{
CVector<T,2> r=v1;
return r.Scale(val);
}
template<class T>
CVector<T,3> operator * (const CVector<T,3> &v1, T val)
{
CVector<T,3> r=v1;
return r.Scale(val);
}
template<class T>
CHomoVector4D<T> operator * (const CHomoVector4D<T> &v1, T val)
{
CHomoVector4D<T> r=v1;
return r.Scale(val);
}
////////////////////////////////////////////////////////////////////////////////////////
//
// 坐標(biāo)轉(zhuǎn)換
//
// 2D極坐標(biāo)
template<class T>
struct CPolar
{
T r; // 半徑
T theta; // 角度
};
// 3D柱面坐標(biāo)
template<class T>
struct CCylindrical
{
T r; // 半徑
T theta; // 角度
T z; // Z坐標(biāo)
};
// 3D球坐標(biāo)
template<class T>
struct CSpherical
{
T r; // 半徑
T theta; // 向量在X-Y面上的投影于正X軸間的夾角
T phi; // 向量與正Z軸的夾角
};
//
// 2D笛卡爾坐標(biāo)與極坐標(biāo)之間的轉(zhuǎn)換
//
template<class T>
inline void PolarToCart (const CPolar<T> &p, CVector<T,2> &v)
{
v.x = p.r * cos(p.theta);
v.y = p.r * sin(p.theta);
}
template<class T>
inline void CartToPolar (const CVector<T,2> &v, CPolar<T> &p)
{
p.r = v.Length();
p.theta = atan2(v.y, v.x);
}
//
// 3D（4D齊次）笛卡爾坐標(biāo)與柱面坐標(biāo)之間的轉(zhuǎn)換
//
template<class T>
inline void CylindricalToCart (const CCylindrical<T> &c, CVector<T,3> &v)
{
v.x = c.r * cos(c.theta);
v.y = c.r * sin(c.theta);
v.z = c.z;
}
template<class T>
inline void CylindricalToCart (const CCylindrical<T> &c, CHomoVector4D<T> &v)
{
v.x = c.r * cos(c.theta);
v.y = c.r * sin(c.theta);
v.z = c.z;
v.w = 1;
}
template<class T>
inline void CartToCylindrical (const CVector<T,3> &v, CCylindrical<T> &c)
{
c.r = sqrt(v.x*v.x + v.y*v.y);
c.theta = atan2(v.y,v.x);
c.z = v.z;
}
template<class T>
inline void CartToCylindrical (const CHomoVector4D<T> &v, CCylindrical<T> &c)
{
c.r = sqrt(v.x*v.x + v.y*v.y);
c.theta = atan2(v.y,v.x);
c.z = v.z;
}
//
// 3D（4D齊次）笛卡爾坐標(biāo)與球坐標(biāo)之間的轉(zhuǎn)換
//
template<class T>
void SphericalToCart (const CSpherical<T> &s, CVector<T,3> &v)
{
T tmp = s.r * sin(s.phi);
v.x = tmp * cos(s.theta);
v.y = tmp * sin(s.theta);
v.z = s.r * cos(s.phi);
}
template<class T>
void SphericalToCart (const CSpherical<T> &s, CHomoVector4D<T> &v)
{
T tmp = s.r * sin(s.phi);
v.x = tmp * cos(s.theta);
v.y = tmp * sin(s.theta);
v.z = s.r * cos(s.phi);
v.w = 1;
}
template<class T>
void CartToSpherical (const CVector<T,3> &v, CSpherical<T> &s)
{
s.r = v.Length();
s.theta = atan2(v.y, v.x);
s.phi = asin(v.x/(s.r*cos(s.theta)));
}
template<class T>
void CartToSpherical (const CHomoVector4D<T> &v, CSpherical<T> &s)
{
s.r = v.Length();
s.theta = atan2(v.y, v.x);
s.phi = asin(v.x/(s.r*cos(s.theta)));
}
///////////////////////////////////////////////////////////////////////////////
//
// 矩陣
//
//
// 通用矩陣模板
//
template<class T, int R, int C>
class CMatrix
{
//
// 數(shù)據(jù)
//
public:
T M[R][C]; // 按行優(yōu)先的次序存儲矩陣元素
//
// 構(gòu)造/析構(gòu)函數(shù)
//
public:
CMatrix() {};
~CMatrix() {};
CMatrix(const CMatrix &m)
{
*this=m;
}
CMatrix& operator = (const CMatrix &m)
{
for(int i=0; i<R; ++i)
{
for(int j=0; j<C; ++j)
M[i][j]=m.M[i][j];
}
return *this;
}
//
// 外部接口
//
public:
// 設(shè)置為單位矩陣
void SetIdentity()
{
memset(&(M[0][0]),0,sizeof(T)*R*C);
if(R!=C)
return;
for(int i=0; i<R; ++i)
M[i][i]=1;
}
// 安全的存取操作
inline void SetVal(int r, int c, T val)
{
if(r>=0 && r<R && c>=0 && c<C)
M[r][c]=val;
}
inline void GetVal(int r, int c, T &val) const
{
if(r>=0 && r<R && c>=0 && c<C)
val=M[r][c];
}
// 以下的運算符重載使得存取元素更方便
// 由于不作任何檢查，小心內(nèi)存越界
inline T operator () (int r, int c) const
{
return M[r][c];
}
inline T& operator () (int r, int c)
{
return M[r][c];
}
// 返回行數(shù)和列數(shù)
inline int Row() const
{
return R;
}
inline int Col() const
{
return C;
}
// 檢測是否方陣
inline bool IsSquare() const
{
return (R==C);
}
//
// 矩陣的基本運算
//
inline CMatrix& operator += (const CMatrix &m)
{
return this->Add(m);
}
inline CMatrix& operator -= (const CMatrix &m)
{
return this->Sub(m);
}
// 矩陣加
inline CMatrix& operator *= (T val)
{
return this->Scale(val);
}
CMatrix& Add (const CMatrix &m)
{
for(int i=0; i<R; ++i)
{
for(int j=0; j<C; ++j)
M[i][j]+=m.M[i][j];
}
return *this;
}
CMatrix& Add (const CMatrix &m1, const CMatrix &m2)
{
for(int i=0; i<R; ++i)
{
for(int j=0; j<C; ++j)
M[i][j]=m1.M[i][j]+m2.M[i][j];
}
return *this;
}
// 矩陣減
CMatrix& Sub (const CMatrix &m)
{
for(int i=0; i<R; ++i)
{
for(int j=0; j<C; ++j)
M[i][j]-=m.M[i][j];
}
return *this;
}
CMatrix& Sub (const CMatrix &m1, const CMatrix &m2)
{
for(int i=0; i<R; ++i)
{
for(int j=0; j<C; ++j)
M[i][j]=m1.M[i][j]-m2.M[i][j];
}
return *this;
}
// 標(biāo)量乘法
CMatrix& Scale (T val)
{
for(int i=0; i<R; ++i)
{
for(int j=0; j<C; ++j)
M[i][j]*=val;
}
return *this;
}
// 矩陣乘法，this=m1*m2
template<int I>
CMatrix& Mul (const CMatrix<T,R,I> &m1, const CMatrix<T,I,C> &m2)
{
T sum;
for(int i=0; i<R; ++i)
{
for(int j=0; j<C; ++j)
{
sum=0;
for(int k=0; k<I; ++k)
{
sum+=m1(i,k)*m2(k,j);
}
M[i][j]=sum;
}
}
return *this;
}
// 將this設(shè)為m的轉(zhuǎn)置
CMatrix& Transpose (const CMatrix<T,C,R> &m)
{
for(int i=0; i<R; ++i)
{
for(int j=0; j<C; ++j)
{
M[i][j]=m.M[j][i];
}
}
return *this;
}
// 根據(jù)拉普拉斯展開式計算行列式
// 注意，只有方陣才能調(diào)用該函數(shù)
T Det () const
{
ASSERT(IsSquare());
// 1/2/3階方陣直接計算
if(Row()==1)
{
return M[0][0];
}
else if(Row()==2)
{
return (M[0][0]*M[1][1]-M[0][1]*M[1][0]);
}
else if(Row()==3)
{
return (
M[0][0]*M[1][1]*M[2][2]+
M[0][1]*M[1][2]*M[2][0]+
M[0][2]*M[1][0]*M[2][1]-
M[2][0]*M[1][1]*M[0][2]-
M[2][1]*M[1][2]*M[0][0]-
M[2][2]*M[1][0]*M[0][1]
);
}
// 4階以上遞歸展開
CMatrix<T,R-1,C-1> sub;
T v, r=0;
for(UINT i=0;i<C;++i)
{
// 填充子矩陣
SubMatrix(sub,0,i);
// 計算該子陣的代數(shù)余子式
v=(i%2) ? (-M[0][i]) : (M[0][i]);
v*=sub.Det();
// 將結(jié)果累加
r+=v;
}
return r;
}
// 基于Gauss-Jordan消元法計算m的逆陣并保存在this中
// 如果成功返回true，否則返回false
bool Inverse (const CMatrix &m)
{
if(!IsSquare())
return false;
*this=m;
int i, j, k;
int row[R], col[C];
for(k=0; k<R; ++k)
{
T max=0;
// 尋找主元
for(i=k;i<R;++i)
{
for(j=k;j<C;++j)
{
if(abs(M[i][j])>max)
{
max=M[i][j];
row[k]=i;
col[k]=j;
}
}
}
if(max==0) // 行列式為0，逆陣不存在
return false;
if(row[k]!=k)
{
// 行交換
T tmp;
for(i=row[k],j=0;j<C;++j)
SWAP(M[i][j], M[k][j], tmp);
}
if(col[k]!=k)
{
// 列交換
T tmp;
for(j=col[k],i=0;i<R;++i)
SWAP(M[i][j], M[i][k], tmp);
}
T mkk=M[k][k];
// 計算逆陣元素
mkk=1/mkk;
M[k][k]=mkk;
for(j=0;j<C;++j)
{
if(j!=k)
M[k][j]*=mkk;
}
for(i=0;i<R;++i)
{
if(i==k)
continue;
for(j=0;j<C;++j)
{
if(j==k)
continue;
M[i][j]-=M[k][j]*M[i][k];
}
}
for(i=0;i<R;++i)
{
if(i!=k)
M[i][k]*=-mkk;
}
}
// 如果之前執(zhí)行了行/列交換，則需要執(zhí)行逆交換
// 交換次序相反，且行（列）交換用列（行）交換代替
for(k=R-1;k>=0;--k)
{
T tmp;
if(col[k]!=k)
{
for(i=col[k],j=0;j<C;++j)
SWAP(M[i][j], M[k][j], tmp);
}
if(row[k]!=k)
{
for(j=row[k],i=0;i<R;++i)
SWAP(M[i][j], M[i][k], tmp);
}
}
return true;
}
//
// 內(nèi)部接口
//
protected:
// 獲取去除第rr行rc列后的子陣
void SubMatrix (CMatrix<T,R-1,C-1> &m, int rr, int rc) const
{
UINT i,j,k,l;
for(i=0,k=0;i<R;++i)
{
if(i==rr)
continue;
for(j=0,l=0;j<C;++j)
{
if(j==rc)
continue;
m.M[k][l]=M[i][j];
++l;
}
++k;
}
}
};
//////////////////////////////////////////////////////////////////////////
//
// 2X2方陣
//
template<class T>
class CMatrix<T,2,2>
{
// 數(shù)據(jù)
public:
// 按行優(yōu)先的次序存儲矩陣元素
union
{
T M[2][2];
struct
{
T
M00, M01,
M10, M11;
};
};
// 構(gòu)造/析構(gòu)函數(shù)
public:
CMatrix() {};
~CMatrix() {};
CMatrix(const CMatrix &m)
{
*this=m;
}
CMatrix& operator = (const CMatrix &m)
{
M00=m.M00; M01=m.M01;
M10=m.M10; M11=m.M11;
return *this;
}
// 外部接口
public:
//
inline void SetIdentity ()
{
M[0][0]=M[1][1]=0;
M[0][1]=M[1][0]=1;
}
// 安全的存取操作
inline void SetVal(int r, int c, T val)
{
if(r>=0 && r<2 && c>=0 && c<2)
M[r][c]=val;
}
inline void GetVal(int r, int c, T &val) const
{
if(r>=0 && r<2 && c>=0 && c<2)
val=M[r][c];
}
// 以下的運算符重載使得存取元素更方便
// 由于不作任何檢查，小心內(nèi)存越界
inline T operator () (int r, int c) const
{
return M[r][c];
}
inline T& operator () (int r, int c)
{
return M[r][c];
}
//
// 矩陣的基本運算
//
inline CMatrix& operator += (const CMatrix &m)
{
return this->Add(m);
}
inline CMatrix& operator -= (const CMatrix &m)
{
return this->Sub(m);
}
CMatrix& operator *= (T val)
{
return this->Scale(val);
}
CMatrix& Add (const CMatrix &m)
{
M00+=m.M00; M01+=m.M01;
M10+=m.M10; M11+=m.M11;
return *this;
}
CMatrix& Sub (const CMatrix &m)
{
M00-=m.M00; M01-=m.M01;
M10-=m.M10; M11-=m.M11;
return *this;
}
CMatrix& Add (const CMatrix &m1, const CMatrix &m2)
{
M00=m1.M00+m2.M00; M01=m1.M01+m2.M01;
M10=m1.M10+m2.M10; M11=m1.M11+m2.M11;
return *this;
}
CMatrix& Sub (const CMatrix &m1, const CMatrix &m2)
{
M00=m1.M00-m2.M00; M01=m1.M01-m2.M01;
M10=m1.M10-m2.M10; M11=m1.M11-m2.M11;
return *this;
}
CMatrix& Scale (T val)
{
M00*=val; M01*=val;
M10*=val; M11*=val;
return *this;
}
CMatrix& Mul (const CMatrix &m1, const CMatrix &m2)
{
M00=m1.M00*m2.M00+m1.M01*m2.M10;
M01=m1.M00*m2.M01+m1.M01*m2.M11;
M10=m1.M10*m2.M00+m1.M11*m2.M10;
M11=m1.M10*m2.M01+m1.M11*m2.M11;
return *this;
}
CMatrix& Transpose (const CMatrix &m)
{
M00=m.M00; M01=m.M10;
M10=m.M01; M11=m.M11;
return *this;
}
inline T Det () const
{
return (M00*M11-M01*M10);
}
bool Inverse (const CMatrix &m)
{
T det=m.Det();
if(det == 0)
return false; // 行列式為0，無逆陣
M00=m.M11/det; M01=-m.M10/det;
M10=-m.M01/det; M11=m.M00/det;
return true;
}
};
///////////////////////////////////////////////////////////////////////////////////
//
// 常用矩陣的類型定義
//
typedef CMatrix<int,2,2> MATRIX2Di;
typedef CMatrix<int,3,3> MATRIX3Di;
typedef CMatrix<int,4,4> MATRIX4Di;
typedef CMatrix<float,2,2> MATRIX2Df;
typedef CMatrix<float,3,3> MATRIX3Df;
typedef CMatrix<float,4,4> MATRIX4Df;
typedef CMatrix<double,2,2> MATRIX2Dd;
typedef CMatrix<double,3,3> MATRIX3Dd;
typedef CMatrix<double,4,4> MATRIX4Dd;
/////////////////////////////////////////////////////////////////////////////////////
//
// 向量與矩陣之間的運算
//
// 向量與矩陣的乘法
template<class T, int R, int C>
void vec_mul_mat(const CVector<T,R> &v, const CMatrix<T,R,C> &m, CVector<T,C> &r)
{
T sum;
for(int j=0; j<C; ++j)
{
sum=0;
for(int i=0; i<R; ++i)
sum+=v(i)*m(i,j);
r(j)=sum;
}
}
// 2D向量和3D齊次矩陣相乘
template<class T>
void vec_mul_mat(const CVector<T,2> &v, const CMatrix<T,3,2> &m, CVector<T,2> &r)
{
// 假設(shè)m的最后一列為[0,0,1]T，并使用3X2矩陣表示
r.x=v.x*m(0,0)+v.y*m(1,0)+m(2,0);
r.y=v.x*m(0,1)+v.y*m(1,1)+m(2,1);
}
template<class T>
void vec_mul_mat(const CVector<T,2> &v, const CMatrix<T,3,3> &m, CVector<T,2> &r)
?? 快捷鍵說明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -