3D游戏中的数学方法（C++实现）3

这节实现矩阵类，包括3x3和4x4，多学学向量和矩阵：

matrix33.h

#ifndef OG3D_MATRIX33_H
#define OG3D_MATRIX33_H


#include
#include
#include
#include
#include"vector3d.h"


#pragma once


namespace OG3D
{
class CMatrix33
{
public:
CMatrix33() { memset(m,0.0f,9*sizeof(float)); }
CMatrix33(float const arr[3][3]) { memcpy(m,arr,9*sizeof(float)); }
CMatrix33(CMatrix33 const& rkMatrix) { memcpy(m,rkMatrix.m,9*sizeof(float)); }
CMatrix33(float const& f00,float const& f01,float const& f02,
float const& f10,float const& f11,float const& f12,
float const& f20,float const& f21,float const& f22)
{
m[0][0]=f00; m[0][1]=f01; m[0][2]=f02;
m[1][0]=f10; m[1][1]=f11; m[1][2]=f12;
m[2][0]=f20; m[2][1]=f21; m[2][2]=f22;
}


void swap(CMatrix33& other)
{
std::swap(m[0][0], other.m[0][0]);
std::swap(m[0][1], other.m[0][1]);
std::swap(m[0][2], other.m[0][2]);
std::swap(m[1][0], other.m[1][0]);
std::swap(m[1][1], other.m[1][1]);
std::swap(m[1][2], other.m[1][2]);
std::swap(m[2][0], other.m[2][0]);
std::swap(m[2][1], other.m[2][1]);
std::swap(m[2][2], other.m[2][2]);
}


float* operator[](size_t i)
{
return m[i];
}
float const* operator[](size_t i) const
{
return m[i];
}


CVector3D get_column(size_t i) const
{
i%=3;
return CVector3D(m[0][i],m[1][i],m[2][i]);
}


void set_column(size_t i,CVector3D const& rkVector)
{
i%=3;
m[0][i]=rkVector[0];
m[1][i]=rkVector[1];
m[2][i]=rkVector[2];
}


float det() const
{
return m[0][0]*(m[1][1]*m[2][2]-m[1][2]*m[2][1])
-m[0][1]*(m[1][0]*m[2][2]-m[1][2]*m[2][0])
+m[0][2]*(m[1][0]*m[2][1]-m[1][1]*m[2][0]);
}


bool inverse() const
{
if(fabs(det())<=FLT_EPSILON)
return false;
return true;
}


CMatrix33& invert()
{
float tm[3][3];


tm[0][0]=m[1][1]*m[2][2]-m[1][2]*m[2][1];
tm[0][1]=m[1][2]*m[2][0]-m[1][0]*m[2][2];
tm[0][2]=m[1][0]*m[2][1]-m[1][1]*m[2][0];
tm[1][0]=m[0][2]*m[2][1]-m[0][1]*m[2][2];
tm[1][1]=m[0][0]*m[2][2]-m[0][2]*m[2][0];
tm[1][2]=m[0][1]*m[2][0]-m[0][0]*m[2][1];
tm[2][0]=m[0][1]*m[1][2]-m[0][2]*m[1][1];
tm[2][1]=m[0][2]*m[1][0]-m[0][0]*m[1][2];
tm[2][2]=m[0][0]*m[1][1]-m[0][1]*m[1][0];


float const fDet(m[0][0]*tm[0][0]+m[0][1]*tm[0][1]+m[0][2]*tm[0][2]);


if(fabs(fDet)<=FLT_EPSILON)
return *this;


float const fInvDet(1.0f/fDet);
for(size_t i=0;i!=3;++i)
for(size_t j=0;j!=3;++j)
m[i][i]=tm[j][i]*fInvDet;


return *this;
}


CMatrix33& operator=(CMatrix33 const& other)
{
memcpy(m,other.m,9*sizeof(float));
return *this;
}


bool operator==(CMatrix33 const& other) const
{
for(size_t i=0;i!=3;++i)
for(size_t j=0;j!=3;++j)
if(m[i][j]!=other.m[i][j])
return false;
return true;
}
bool operator!=(CMatrix33 const& other) const
{
return !operator==(other);
}


CMatrix33& operator+=(CMatrix33 const& rhs)
{
for(size_t i=0;i!=3;++i)
for(size_t j=0;j!=3;++j)
m[i][j]+=rhs.m[i][j];
return *this;
}
CMatrix33& operator-=(CMatrix33 const& rhs)
{
for(size_t i=0;i!=3;++i)
for(size_t j=0;j!=3;++j)
m[i][j]-=rhs.m[i][j];
return *this;
}


CMatrix33& operator*=(float const& fc)
{
for(size_t i=0;i!=3;++i)
for(size_t j=0;j!=3;++j)
m[i][j]*=fc;
return *this;
}
CMatrix33& operator/=(float const& fc)
{
assert(fc!=0.0f);


float const cf(1.0f/fc);


for(size_t i=0;i!=3;++i)
for(size_t j=0;j!=3;++j)
m[i][j]*=cf;
return *this;
}


CMatrix33& operator*=(CMatrix33 const& rhs)
{
float tm[3][3];
memset(tm,0.0f,9*sizeof(float));


for(size_t i=0;i!=3;++i)
for(size_t j=0;j!=3;++j)
for(size_t k=0;k!=3;++k)
tm[i][j]+=m[i][k]*rhs.m[k][j];


memcpy(m,tm,9*sizeof(float));
return *this;
}


void transpose()
{
std::swap(m[0][1],m[1][0]);
std::swap(m[0][2],m[2][0]);
std::swap(m[1][2],m[2][1]);
}


protected:
float m[3][3];
};


inline CVector3D operator*(CMatrix33 const& rkMatrix,CVector3D const& rkVector)
{
CVector3D res;


for(size_t i=0;i!=3;++i)
for(size_t k=0;k!=3;++k)
res[i]+=rkMatrix[i][k]*rkVector[k];


return res;
}


inline CMatrix33 transpose(CMatrix33 const& rkMatrix)
{
CMatrix33 res(rkMatrix);


res.transpose();
return res;
}


inline CMatrix33 invert(CMatrix33 const& rkMatrix)
{
CMatrix33 res(rkMatrix);


res.invert();
return res;
}


static CMatrix33 const MATRIX_IDENTITY(1.0f,0.0f,0.0f,
0.0f,1.0f,0.0f,
0.0f,0.0f,1.0f);
static CMatrix33 const MATRIX_ZERO;
}


#endif

matrix44.h

#ifndef OG3D_MATRIX44_H
#define OG3D_MATRIX44_H


#include"vector4d.h"
#include"matrix33.h"


#pragma once


namespace OG3D
{
class CMatrix44
{
protected:
m[4][4];


public:
CMatrix44(float const& fm=0.0f) { memset(m,fm,16*sizeof(float)); }
CMatrix44(float const& m00,float const& m01,float const& m02,float const& m03,
float const& m10,float const& m11,float const& m12,float const& m13,
float const& m20,float const& m21,float const& m22,float const& m23,
float const& m30,float const& m31,float const& m32,float const& m33)
{
m[0][0]=m00; m[0][1]=m01; m[0][2]=m02; m[0][3]=m03;
m[1][0]=m10; m[1][1]=m11; m[1][2]=m12; m[1][3]=m13;
m[2][0]=m20; m[2][1]=m21; m[2][2]=m22; m[2][3]=m23;
m[3][0]=m30; m[3][1]=m31; m[3][2]=m32; m[3][3]=m33;
}
CMatrix44(CMatrix33 const& m3x3)
{
operator=(m3x3);
}
CMatrix44(CMatrix44 const& other) { memcpy(m,other.m,16*sizeof(float)); }


CMatrix44& operator=(CMatrix44 const& other)
{
memcpy(m,other.m,16*sizeof(float));
return *this;
}


CMatrix44& operator=(CMatrix33 const& m3x3)
{
for(size_t i=0;i!=3;++i)
for(size_t j=0;j!=3;++j)
m[i][j]=m3x3[i][j];


m[0][3]=m[1][3]=m[2][3]=0.0f;
m[3][0]=m[3][1]=m[3][2]=0.0f;
m[3][3]=1.0f;
return *this;
}


void swap(CMatrix44& other)
{
std::swap(m[0][0], other.m[0][0]);
std::swap(m[0][1], other.m[0][1]);
std::swap(m[0][2], other.m[0][2]);
std::swap(m[0][3], other.m[0][3]);
std::swap(m[1][0], other.m[1][0]);
std::swap(m[1][1], other.m[1][1]);
std::swap(m[1][2], other.m[1][2]);
std::swap(m[1][3], other.m[1][3]);
std::swap(m[2][0], other.m[2][0]);
std::swap(m[2][1], other.m[2][1]);
std::swap(m[2][2], other.m[2][2]);
std::swap(m[2][3], other.m[2][3]);
std::swap(m[3][0], other.m[3][0]);
std::swap(m[3][1], other.m[3][1]);
std::swap(m[3][2], other.m[3][2]);
std::swap(m[3][3], other.m[3][3]);
}


float* operator[](size_t i)
{
i%=4;
return m[i];
}
float const* operator[](size_t i) const
{
i%=4;
return m[i];
}


CMatrix44& operator*=(float const& fc)
{
m[0][0]*=fc; m[0][1]*=fc; m[0][2]*=fc; m[0][3]*=fc;
m[1][0]*=fc; m[1][1]*=fc; m[1][2]*=fc; m[1][3]*=fc;
m[2][0]*=fc; m[2][1]*=fc; m[2][2]*=fc; m[2][3]*=fc;
m[3][0]*=fc; m[3][1]*=fc; m[3][2]*=fc; m[3][3]*=fc;
return *this;
}
CMatrix44& operator/=(float const& fc)
{
assert(fc!=0.0f);


float const cf(1.0f/fc);


m[0][0]*=cf; m[0][1]*=cf; m[0][2]*=cf; m[0][3]*=cf;
m[1][0]*=cf; m[1][1]*=cf; m[1][2]*=cf; m[1][3]*=cf;
m[2][0]*=cf; m[2][1]*=cf; m[2][2]*=cf; m[2][3]*=cf;
m[3][0]*=cf; m[3][1]*=cf; m[3][2]*=cf; m[3][3]*=cf;
return *this;
}


CMatrix44& operator*=(CMatrix44 const& other)
{
float res[4][4];


res[0][0]=m[0][0]*other.m[0][0]+m[0][1]*other.m[1][0]+m[0][2]*other.m[2][0]+m[0][3]*other.m[3][0];
res[0][1]=m[0][0]*other.m[0][1]+m[0][1]*other.m[1][1]+m[0][2]*other.m[2][1]+m[0][3]*other.m[3][1];
res[0][2]=m[0][0]*other.m[0][2]+m[0][1]*other.m[1][2]+m[0][2]*other.m[2][2]+m[0][3]*other.m[3][2];
res[0][3]=m[0][0]*other.m[0][3]+m[0][1]*other.m[1][3]+m[0][2]*other.m[2][3]+m[0][3]*other.m[3][3];


res[1][0]=m[1][0]*other.m[0][0]+m[1][1]*other.m[1][0]+m[1][2]*other.m[2][0]+m[1][3]*other.m[3][0];
res[1][1]=m[1][0]*other.m[0][1]+m[1][1]*other.m[1][1]+m[1][2]*other.m[2][1]+m[1][3]*other.m[3][1];
res[1][2]=m[1][0]*other.m[0][2]+m[1][1]*other.m[1][2]+m[1][2]*other.m[2][2]+m[1][3]*other.m[3][2];
res[1][3]=m[1][0]*other.m[0][3]+m[1][1]*other.m[1][3]+m[1][2]*other.m[2][3]+m[1][3]*other.m[3][3];


res[2][0]=m[2][0]*other.m[0][0]+m[2][1]*other.m[1][0]+m[2][2]*other.m[2][0]+m[2][3]*other.m[3][0];
res[2][1]=m[2][0]*other.m[0][1]+m[2][1]*other.m[1][1]+m[2][2]*other.m[2][1]+m[2][3]*other.m[3][1];
res[2][2]=m[2][0]*other.m[0][2]+m[2][1]*other.m[1][2]+m[2][2]*other.m[2][2]+m[2][3]*other.m[3][2];
res[2][3]=m[2][0]*other.m[0][3]+m[2][1]*other.m[1][3]+m[2][2]*other.m[2][3]+m[2][3]*other.m[3][3];


res[3][0]=m[3][0]*other.m[0][0]+m[3][1]*other.m[1][0]+m[3][2]*other.m[2][0]+m[3][3]*other.m[3][0];
res[3][1]=m[3][0]*other.m[0][1]+m[3][1]*other.m[1][1]+m[3][2]*other.m[2][1]+m[3][3]*other.m[3][1];
res[3][2]=m[3][0]*other.m[0][2]+m[3][1]*other.m[1][2]+m[3][2]*other.m[2][2]+m[3][3]*other.m[3][2];
res[3][3]=m[3][0]*other.m[0][3]+m[3][1]*other.m[1][3]+m[3][2]*other.m[2][3]+m[3][3]*other.m[3][3];


return *this;
}


CVector3D operator*(CVector3D const& vec)
{
float const fInv=1.0f/(m[3][0]*vec.x+m[3][1]*vec.y+m[3][2]*vec.z+m[3][3]);


return CVector3D(
(m[0][0]*vec.x+m[0][1]*vec.y+m[0][2]*vec.z+m[0][3])*fInv,
(m[1][0]*vec.x+m[1][1]*vec.y+m[1][2]*vec.z+m[1][3])*fInv,
(m[2][0]*vec.x+m[2][1]*vec.y+m[2][2]*vec.z+m[2][3])*fInv
);
}


CVector4D operator*(CVector4D const& vec)
{
return CVector4D(
m[0][0]*vec.x+m[0][1]*vec.y+m[0][2]*vec.z+m[0][3]*vec.w,
m[1][0]*vec.x+m[1][1]*vec.y+m[1][2]*vec.z+m[1][3]*vec.w,
m[2][0]*vec.x+m[2][1]*vec.y+m[2][2]*vec.z+m[2][3]*vec.w,
m[3][0]*vec.x+m[3][1]*vec.y+m[3][2]*vec.z+m[3][3]*vec.w
);
}


CMatrix44& operator+=(CMatrix44 const& other)
{
m[0][0]+=other.m[0][0]; m[0][1]+=other.m[0][1]; m[0][2]+=other.m[0][2]; m[0][3]+=other.m[0][3];
m[1][0]+=other.m[1][0]; m[1][1]+=other.m[1][1]; m[1][2]+=other.m[1][2]; m[1][3]+=other.m[1][3];
m[2][0]+=other.m[2][0]; m[2][1]+=other.m[2][1]; m[2][2]+=other.m[2][2]; m[2][3]+=other.m[2][3];
m[3][0]+=other.m[3][0]; m[3][1]+=other.m[3][1]; m[3][2]+=other.m[3][2]; m[3][3]+=other.m[3][3];
return *this;
}


CMatrix44& operator-=(CMatrix44 const& other)
{
m[0][0]-=other.m[0][0]; m[0][1]-=other.m[0][1]; m[0][2]-=other.m[0][2]; m[0][3]-=other.m[0][3];
m[1][0]-=other.m[1][0]; m[1][1]-=other.m[1][1]; m[1][2]-=other.m[1][2]; m[1][3]-=other.m[1][3];
m[2][0]-=other.m[2][0]; m[2][1]-=other.m[2][1]; m[2][2]-=other.m[2][2]; m[2][3]-=other.m[2][3];
m[3][0]-=other.m[3][0]; m[3][1]-=other.m[3][1]; m[3][2]-=other.m[3][2]; m[3][3]-=other.m[3][3];
return *this;
}


bool operator==(CMatrix44 const& m2) const
{
if( 
m[0][0] != m2.m[0][0] || m[0][1] != m2.m[0][1] || m[0][2] != m2.m[0][2] || m[0][3] != m2.m[0][3] ||
m[1][0] != m2.m[1][0] || m[1][1] != m2.m[1][1] || m[1][2] != m2.m[1][2] || m[1][3] != m2.m[1][3] ||
m[2][0] != m2.m[2][0] || m[2][1] != m2.m[2][1] || m[2][2] != m2.m[2][2] || m[2][3] != m2.m[2][3] ||
m[3][0] != m2.m[3][0] || m[3][1] != m2.m[3][1] || m[3][2] != m2.m[3][2] || m[3][3] != m2.m[3][3] )
return false;
return true;
}
bool operator!=(CMatrix44 const& m2) const
{
if( 
m[0][0] != m2.m[0][0] || m[0][1] != m2.m[0][1] || m[0][2] != m2.m[0][2] || m[0][3] != m2.m[0][3] ||
m[1][0] != m2.m[1][0] || m[1][1] != m2.m[1][1] || m[1][2] != m2.m[1][2] || m[1][3] != m2.m[1][3] ||
m[2][0] != m2.m[2][0] || m[2][1] != m2.m[2][1] || m[2][2] != m2.m[2][2] || m[2][3] != m2.m[2][3] ||
m[3][0] != m2.m[3][0] || m[3][1] != m2.m[3][1] || m[3][2] != m2.m[3][2] || m[3][3] != m2.m[3][3] )
return true;
return false;
}


CMatrix44& operator=(CMatrix33 const& mat3)
{
m[0][0] = mat3.m[0][0]; m[0][1] = mat3.m[0][1]; m[0][2] = mat3.m[0][2];
m[1][0] = mat3.m[1][0]; m[1][1] = mat3.m[1][1]; m[1][2] = mat3.m[1][2];
m[2][0] = mat3.m[2][0]; m[2][1] = mat3.m[2][1]; m[2][2] = mat3.m[2][2];
return *this;
}


void transpose()
{
std::swap(m[0][1],m[1][0]);
std::swap(m[0][2],m[2][0]);
std::swap(m[0][3],m[3][0]);
std::swap(m[1][2],m[2][1]);
std::swap(m[1][3],m[3][1]);
std::swap(m[2][3],m[3][2]);
}


//translation
void set_tranlate(CVector3D const& offset)
{
m[0][3]=offset.x;
m[1][3]=offset.y;
m[2][3]=offset.z;
}


CVector3D get_translate() const
{
return CVector3D(m[0][3],m[1][3],[2][3]);
}


void make_translate(CVector3D const& offset)
{
m[0][0] = 1.0; m[0][1] = 0.0; m[0][2] = 0.0; m[0][3] = offset.x;
m[1][0] = 0.0; m[1][1] = 1.0; m[1][2] = 0.0; m[1][3] = offset.y;
m[2][0] = 0.0; m[2][1] = 0.0; m[2][2] = 1.0; m[2][3] = offset.z;
m[3][0] = 0.0; m[3][1] = 0.0; m[3][2] = 0.0; m[3][3] = 1.0;
}


void make_translate(float tx,float ty,float tz)
{
m[0][0] = 1.0; m[0][1] = 0.0; m[0][2] = 0.0; m[0][3] = tx;
m[1][0] = 0.0; m[1][1] = 1.0; m[1][2] = 0.0; m[1][3] = ty;
m[2][0] = 0.0; m[2][1] = 0.0; m[2][2] = 1.0; m[2][3] = tz;
m[3][0] = 0.0; m[3][1] = 0.0; m[3][2] = 0.0; m[3][3] = 1.0;
}


//scale
void set_scale(CVector3D const& scale)
{
m[0][0]=scale.x;
m[1][1]=scale.y;
m[2][2]=scale.z;
}


void set_scale(float sx,float sy,float sz)
{
m[0][0]=sx;
m[1][1]=sy;
m[2][2]=sz;
}


CVector3D get_translate() const
{
return CVector3D(m[0][0],m[1][1],m[2][2]);
}


void make_scale(CVector3D const& v)
{
m[0][0] = v.x; m[0][1] = 0.0; m[0][2] = 0.0; m[0][3] = 0.0f;
m[1][0] = 0.0; m[1][1] = v.y; m[1][2] = 0.0; m[1][3] = 0.0f;
m[2][0] = 0.0; m[2][1] = 0.0; m[2][2] = v.z; m[2][3] = 0.0f;
m[3][0] = 0.0; m[3][1] = 0.0; m[3][2] = 0.0; m[3][3] = 1.0f;
}


void make_scale(float sx,float sy,float sz)
{
m[0][0] = sx; m[0][1] = 0.0; m[0][2] = 0.0; m[0][3] = 0.0f;
m[1][0] = 0.0; m[1][1] = sy; m[1][2] = 0.0; m[1][3] = 0.0f;
m[2][0] = 0.0; m[2][1] = 0.0; m[2][2] = xz; m[2][3] = 0.0f;
m[3][0] = 0.0; m[3][1] = 0.0; m[3][2] = 0.0; m[3][3] = 1.0f;
}


//rotate
void set_rotate(CVector3D const& v,float rad)
{
float const c(cosf(rad)),s(sinf(rad));


m[0][0]=c+v.x*v.x*(1.0f-c);
m[0][1]=-v.z*s+v.x*v.y*(1.0f-c);
m[0][2]=y*s+v.x*v.z*(1.0f-c);
// m[0][3]=0.0f;


m[1][0]=v.z*s+v.x*v.y*(1.0f-c);
m[1][1]=c+v.y*v.y*(1.0f-c);
m[1][2]=-v.x*s+v.y*v.z*(1.0f-c);
// m[1][3]=0.0f;


m[2][0]=-v.y*s+v.x*v.z*(1.0f-c);
m[2][1]=v.x*s+v.y*v.z*(1.0f-c);
m[2][2]=c+v.z*v.z*(1.0f-c);
// m[2][3]=0.0f;


// m[3][0]=0.0f;
// m[3][1]=0.0f;
// m[3][2]=0.0f;
// m[3][3]=1.0f;
}


void make_rotate(CVector3D const& v,float rad)
{
float const c(cosf(rad)),s(sinf(rad));


m[0][0]=c+v.x*v.x*(1.0f-c);
m[0][1]=-v.z*s+v.x*v.y*(1.0f-c);
m[0][2]=y*s+v.x*v.z*(1.0f-c);
m[0][3]=0.0f;


m[1][0]=v.z*s+v.x*v.y*(1.0f-c);
m[1][1]=c+v.y*v.y*(1.0f-c);
m[1][2]=-v.x*s+v.y*v.z*(1.0f-c);
m[1][3]=0.0f;


m[2][0]=-v.y*s+v.x*v.z*(1.0f-c);
m[2][1]=v.x*s+v.y*v.z*(1.0f-c);
m[2][2]=c+v.z*v.z*(1.0f-c);
m[2][3]=0.0f;


m[3][0]=0.0f;
m[3][1]=0.0f;
m[3][2]=0.0f;
m[3][3]=1.0f;
}


void set_pitch(float rad)
{
float const c(cosf(rad)),s(sinf(rad));


m[0][0]=1.0f; 
m[0][1]=0.0f;
m[0][2]=0.0f; 
// m[0][3]=0.0f;


m[1][0]=0.0f; 
m[1][1]=c;
m[1][2]=-s;
// m[1][3]=0.0f;


m[2][0]=0.0f;
m[2][1]=s;
m[2][2]=c;
// m[2][3]=0.0f;


// m[3]
}


void make_pitch(float rad)
{
float const c(cosf(rad)),s(sinf(rad));


m[0][0]=1.0f; 
m[0][1]=0.0f;
m[0][2]=0.0f; 
m[0][3]=0.0f;


m[1][0]=0.0f; 
m[1][1]=c;
m[1][2]=-s;
m[1][3]=0.0f;


m[2][0]=0.0f;
m[2][1]=s;
m[2][2]=c;
m[2][3]=0.0f;


m[3][0]=0.0f;
m[3][1]=0.0f;
m[3][2]=0.0f;
m[3][3]=1.0f;
}


void set_yaw(float rad)
{
float const c(cosf(rad)),s(sinf(rad));


m[0][0]=c; 
m[0][1]=0.0f;
m[0][2]=s; 
//m[0][3]=0.0f;


m[1][0]=0.0f;
m[1][1]=1.0f;
m[1][2]=0.0f;
// m[1][3]=0.0f;


m[2][0]=-s;
m[2][1]=0.0f;
m[2][2]=c;
// m[2][3]=0.0f;


// m[3]
}


void make_yaw(float yaw)
{
float const c(cosf(rad)),s(sinf(rad));


m[0][0]=c; 
m[0][1]=0.0f;
m[0][2]=s; 
m[0][3]=0.0f;


m[1][0]=0.0f;
m[1][1]=1.0f;
m[1][2]=0.0f;
m[1][3]=0.0f;


m[2][0]=-s;
m[2][1]=0.0f;
m[2][2]=c;
m[2][3]=0.0f;


m[3][0]=0.0f;
m[3][1]=0.0f;
m[3][2]=0.0f;
m[3][3]=1.0f;
}


void set_roll(float rad)
{
float const c(cosf(rad)),s(sinf(rad));


m[0][0]=c; 
m[0][1]=s;
m[0][2]=0.0f; 
// m[0][3]=0.0f;


m[1][0]=s; 
m[1][1]=c;
m[1][2]=0.0f;
// m[1][3]=0.0f;


m[2][0]=0.0f;
m[2][1]=0.0f;
m[2][2]=1.0f;
// m[2][3]=0.0f;


// m[3]
}


void make_roll(float rad)
{
float const c(cosf(rad)),s(sinf(rad));


m[0][0]=c; 
m[0][1]=s;
m[0][2]=0.0f; 
m[0][3]=0.0f;


m[1][0]=s; 
m[1][1]=c;
m[1][2]=0.0f;
m[1][3]=0.0f;


m[2][0]=0.0f;
m[2][1]=0.0f;
m[2][2]=1.0f;
m[2][3]=0.0f;


m[3][0]=0.0f;
m[3][1]=0.0f;
m[3][2]=0.0f;
m[3][3]=1.0f;
}
};


inline CMatrix44 operator*=(CMatrix44 const& mat,float const& fc)
{
CMatrix44 res(mat);


res*=fc;
return res;
}


inline CMatrix44 operator*=(float const& fc,CMatrix44 const& mat)
{
CMatrix44 res(mat);


res*=fc;
return res;
}


inline CMatrix44 operator/=(CMatrix44 const& mat,float const& fc)
{
assert(fc!=0.0f);


CMatrix44 res(mat);


res/=fc;
return res;
}


inline CMatrix44 operator*(CMatrix44 const& lhs,CMatrix44 const& rhs)
{
CMatrix44 res(lhs);


res*=rhs;
return res;
}


inline CMatrix44 operator-(CMatrix44 const& lhs,CMatrix44 const& rhs)
{
CMatrix44 res(lhs);


res-=rhs;
return res;
}


inline CMatrix44 operator+(CMatrix44 const& lhs,CMatrix44 const& rhs)
{
CMatrix44 res(lhs);


res+=rhs;
return res;
}


inline void translate(CVector3D& v,float tx,float ty,float tz)
{
v.x+=tx;
v.y+=ty;
v.z+=tz;
}


inline void scale(CVector3D& v,float sx,float sy,float sz)
{
v.x*=sx;
v.y*=sy;
v.z*=sz;
}


inline void rotate(CVector3D& v,CVector3D const& dir,float rad)
{
CMatrix44 mat4;


mat4.make_rotate(dir,rad);
v=mat4*v;
}


inline void pitch(CVector3D& v,float rad)
{
CMatrix44 mat4;


mat4.make_pitch(rad);
v=mat4*v;
}


inline void yaw(CVector3D& v,float rad)
{
CMatrix44 mat4;


mat4.make_yaw(rad);
v=mat4*v;
}


inline void roll(CVector3D& v,float rad)
{
CMatrix44 mat4;


mat4.make_roll(rad);
v=mat4*v;
}
}


#endif

基本类实现了，下一节开始3D碰撞检测技术征途！