xmath.h

一个简单的数学库.但是很实用..大家可以下载过去研究一下.

//////////////////////////////////////////////////////////////
//
// 作者：Blackbird
// 版本：0.01 beta
// 描述：X3D的数学引擎，提供3D图形学的基本数学支持 
//
//////////////////////////////////////////////////////////////

//
// 常量和宏定义
//

#define PI			(3.141592654f)
#define PI2			(6.283185307f)
#define PI_DIV_2	(1.570796327f)
#define PI_DIV_4	(0.785398163f)
#define PI_INV		(0.318309886f)

// 极小值
#define EPSILON_E3	(float)(1E-3) 
#define EPSILON_E4	(float)(1E-4) 
#define EPSILON_E5	(float)(1E-5)
#define EPSILON_E6	(float)(1E-6)

#define MIN(a,b)	((a<b)?(a):(b))
#define MAX(a,b)	((a>b)?(a):(b))

#define SWAP(a,b,t) {t=a;a=b;b=t;}

// 角度和弧度之间的转换
#define DEG_TO_RAD(ang)		((ang)*PI/180.0f)
#define RAD_TO_DEG(rad)		((rad)*180.0f/PI)

///////////////////////////////////////////////////////////

//
// 全局变量
//

// 三角函数查找表
extern float sin_table[361];
extern float cos_table[361];

////////////////////////////////////////////////////////////

//
// 常用数学函数
//

// 构建正/余弦函数值查找表
void BuildSinCosTable();

// 快速的正/余弦函数，使用线性插值法实现
float FastSin(float ang);
float FastCos(float ang);

// 快速计算平方根和平方根的倒数，基于NR
float InvSqrt (float x);
float FastSqrt(float x);

/////////////////////////////////////////////////////////////

//
// 向量
//

//
// 通用向量模板
//
template<class T, int D>
class CVector
{
//
// 数据
//
public:
	T	M[D];

//
// 构造函数
//
public:
	CVector() {}

	~CVector() {}

	CVector(const CVector &v)
	{
		*this=v;
	}
	inline CVector& operator = (const CVector &v)
	{
		memcpy(M,v.M,sizeof(T)*D);
		return *this;
	}
//
// 外部接口
//
public:
	// 返回维数
	inline int Dimension () const
	{
		return D;
	}
	// 元素置零
	inline void SetZero ()
	{
		memset(M,0,sizeof(T)*D);
	}
	// 安全的元素存取
	inline void SetVal (int i, T val)
	{
		if(i>=0 && i<D)
			M[i]=val;
	}
	inline void GetVal (int i, T &val) const
	{
		if(i>=0 && i<D)
			val=M[i];
	}
	// 快速元素存取
	inline T operator () (int i) const
	{
		return M[i];
	}
	inline T& operator () (int i)
	{
		return M[i];
	}
	//
	// 向量基本运算
	//
	// 向量加
	CVector& Add (const CVector &v)
	{
		for(int i=0; i<D; ++i)
			M[i]+=v.M[i];
		return *this;
	}
	CVector& Add (const CVector &v1, const CVector &v2)
	{
		for(int i=0; i<D; ++i)
			M[i]=v1.M[i]+v2.M[i];
		return *this;
	}
	// 向量减
	CVector& Sub (const CVector &v)
	{
		for(int i=0; i<D; ++i)
			M[i]-=v.M[i];
		return *this;
	}
	CVector& Sub (const CVector &v1, const CVector &v2)
	{
		for(int i=0; i<D; ++i)
			M[i]=v1.M[i]-v2.M[i];
		return *this;
	}
	// 向量取反
	CVector& Reverse()
	{
		for(int i=0; i<D; ++i)
			M[i]=-M[i];
		return *this;
	}
	// 标量积
	CVector& Scale (T val)
	{
		for(int i=0; i<D; ++i)
			M[i]*=val;
		return *this;
	}
	// 点积
	T Dot (const CVector &v) const
	{
		T sum=0;
		for(int i=0; i<D; ++i)
			sum+=M[i]*v.M[i];
		return sum;
	}
	// 向量长度
	inline T Length () const
	{
		return (sqrt(Length2()));
	}
	// 向量长度的平方
	T Length2 () const
	{
		T sum=0;
		for(int i=0; i<D; ++i)
			sum+=M[i]*M[i];
		return sum;
	}
	// 归一化为单位向量
	void Normalize ()
	{
		T len=Length();
		if(len == 0)
			return;
		len=1/len;
		for(int i=0; i<D; ++i)
			M[i]*=len;
	}
};

/////////////////////////////////////////////////////////////

//
// 2D向量
//
template<class T>
class CVector<T,2>
{
//
// 数据
//
public:
	union
	{
		T	M[2];
		struct
		{
			T	x,y;
		};
	};
//
// 构造函数
//
public:
	CVector() {}

	~CVector() {}

	CVector(const CVector &v)
	{
		*this=v;
	}
	inline CVector& operator = (const CVector &v)
	{
		x=v.x;
		y=v.y;
		return *this;
	}
//
// 外部接口
//
public:
	inline void SetZero()
	{
		x=y=0;
	}
	inline void SetVal(T vx, T vy)
	{
		x=vx;
		y=vy;
	}
	// 快速元素存取
	inline T operator () (int i) const
	{
		return M[i];
	}
	inline T& operator () (int i)
	{
		return M[i];
	}
	// 向量运算
	inline CVector& operator += (const CVector &v)
	{
		return Add(v);
	}
	inline CVector& operator -= (const CVector &v)
	{
		return Sub(v);
	}
	inline CVector& operator *= (T val)
	{
		return Scale(val);
	}
	inline CVector& operator *= (const CVector &v)
	{
		return Dot(v);
	}
	inline CVector& Reverse()
	{
		x=-x;
		y=-y;
		return *this;
	}
	inline CVector& Add (const CVector &v)
	{
		x+=v.x;
		y+=v.y;
		return *this;
	}
	inline CVector& Add (const CVector &v1, const CVector &v2)
	{
		x=v1.x+v2.x;
		y=v1.y+v2.y;
		return *this;
	}
	inline CVector& Sub (const CVector &v)
	{
		x-=v.x;
		y-=v.y;
		return *this;
	}
	inline CVector& Sub (const CVector &v1, const CVector &v2)
	{
		x=v1.x-v2.x;
		y=v1.y-v2.y;
		return *this;
	}
	inline CVector& Scale (T val)
	{
		x*=val;
		y*=val;
		return *this;
	}
	inline T Dot (const CVector &v) const
	{
		return (x*v.x+y*v.y);
	}
	inline T Length() const
	{
		return (sqrt(x*x+y*y));
	}
	inline T Length2() const
	{
		return (x*x+y*y);
	}
	// this逆时针到v的夹角的余弦
	inline T CosOfAngle(const CVector &v) const
	{
		return (this->Dot(v)/(Length()*v.Length()));
	}
	void Normalize()
	{
		T len=Length();
		if(len == 0)
			return;
		len=1/len;
		x*=len;
		y*=len;
	}
};

typedef CVector<int,2> POINT2Di;
typedef CVector<int,2> VECTOR2Di;

typedef CVector<float,2> POINT2Df;
typedef CVector<float,2> VECTOR2Df;

typedef CVector<double,2> POINT2Dd;
typedef CVector<double,2> VECTOR2Dd;

//////////////////////////////////////////////////////////////////////////

//
// 3D向量
//
template<class T>
class CVector<T,3>
{
//
// 数据
//
public:
	union
	{
		T	M[3];
		struct
		{
			T	x,y,z;
		};
	};
//
// 构造函数
//
public:
	CVector() {}

	~CVector() {}

	CVector(const CVector &v)
	{
		*this=v;
	}
	inline CVector& operator = (const CVector &v)
	{
		x=v.x;
		y=v.y;
		z=v.z;
		return *this;
	}
//
// 外部接口
//
public:
	inline void SetZero()
	{
		x=y=z=0;
	}
	inline void SetVal(T vx, T vy, T vz)
	{
		x=vx;
		y=vy;
		z=vz;
	}
	// 快速元素存取
	inline T operator () (int i) const
	{
		return M[i];
	}
	inline T& operator () (int i)
	{
		return M[i];
	}
	// 向量运算
	inline CVector& operator += (const CVector &v)
	{
		return Add(v);
	}
	inline CVector& operator -= (const CVector &v)
	{
		return Sub(v);
	}
	inline CVector& operator *= (T val)
	{
		return Scale(val);
	}
	inline CVector& operator *= (const CVector &v)
	{
		return Dot(v);
	}
	inline CVector& Reverse()
	{
		x=-x;
		y=-y;
		z=-z;
		return *this;
	}
	inline CVector& Add (const CVector &v)
	{
		x+=v.x;
		y+=v.y;
		z+=v.z;
		return *this;
	}
	inline CVector& Add (const CVector &v1, const CVector &v2)
	{
		x=v1.x+v2.x;
		y=v1.y+v2.y;
		z=v1.z+v2.z;
		return *this;
	}
	inline CVector& Sub (const CVector &v)
	{
		x-=v.x;
		y-=v.y;
		z-=v.z;
		return *this;
	}
	inline CVector& Sub (const CVector &v1, const CVector &v2)
	{
		x=v1.x-v2.x;
		y=v1.y-v2.y;
		z=v1.z-v2.z;
		return *this;
	}
	inline CVector& Scale (T val)
	{
		x*=val;
		y*=val;
		z*=val;
		return *this;
	}
	inline T Dot (const CVector &v) const
	{
		return (x*v.x+y*v.y+z*v.z);
	}
	// 叉积，v1叉乘v2，结果保存在this中
	CVector& Cross (const CVector &v1, const CVector &v2)
	{
		x=v1.y*v2.z-v1.z*v2.y;
		y=v1.z*v2.x-v1.x*v2.z;
		z=v1.x*v2.y-v1.y*v2.x;
		return *this;
	}
	inline T Length() const
	{
		return (sqrt(x*x+y*y+z*z));
	}
	inline T Length2() const
	{
		return (x*x+y*y+z*z);
	}
	inline T CosOfAngle(const CVector &v) const
	{
		return (this->Dot(v)/(Length()*v.Length()));
	}
	void Normalize()
	{
		T len=Length();
		if(len == 0)
			return;
		len=1/len;
		x*=len;
		y*=len;
		z*=len;
	}
};

typedef CVector<int,3> POINT3Di;
typedef CVector<int,3> VECTOR3Di;

typedef CVector<float,3> POINT3Df;
typedef CVector<float,3> VECTOR3Df;

typedef CVector<double,3> POINT3Dd;
typedef CVector<double,3> VECTOR3Dd;

//////////////////////////////////////////////////////////////////////////

//
// 4D齐次向量
//

template<class T>
class CHomoVector4D: public CVector<T,3>
{
//
// 数据
//
public:
	T	w;
//
// 构造/析构函数
//
public:
	CHomoVector4D() {};

	~CHomoVector4D() {};

	CHomoVector4D(const CVector<T,3> &v): CVector(v), w(1)
	{}

	CHomoVector4D(const CHomoVector4D &v)
	{
		*this=v;
	}

	inline CHomoVector4D& operator = (const CVector<T,3> &v)
	{
		x=v.x;
		y=v.y;
		z=v.z;
		w=1;
		return *this;
	}

	inline CHomoVector4D& operator = (const CHomoVector4D &v)
	{
		x=v.x;
		y=v.y;
		z=v.z;
		w=v.w;
		return *this;
	}
//
// 外部接口
//
public:
	inline void SetZero()
	{
		CVector<T,3>::SetZero();
		w=1;
	}
	inline void SetVal(T vx, T vy, T vz, T vw=1)
	{
		CVector<T,3>::SetZero(vx,vy,vz);
		w=vw;
	}
	void Transform()
	{
		if(w==0 || w==1)
			return;
		w=1/w;
		x*=w;
		y*=w;
		z*=w;
		w=1;
	}
};

typedef CHomoVector4D<int> POINT4Di;
typedef CHomoVector4D<int> VECTOR4Di;

typedef CHomoVector4D<float> POINT4Df;
typedef CHomoVector4D<float> VECTOR4Df;

typedef CHomoVector4D<double> POINT4Dd;
typedef CHomoVector4D<double> VECTOR4Dd;

///////////////////////////////////////////////////////////////////////////////

//
// 向量运算符重载
// 

//
// 一元向量减法（取反）
//
template<class T>
CVector<T,2> operator - (const CVector<T,2> &v1)
{
	CVector<T,2> r=v1;
	r.Reverse();
	return r;
}

template<class T>
CVector<T,3> operator - (const CVector<T,3> &v1)
{
	CVector<T,3> r=v1;
	r.Reverse();
	return r;
}

template<class T>
CHomoVector4D<T> operator - (const CHomoVector4D<T> &v1)
{
	CHomoVector4D<T> r=v1;
	r.Reverse();
	return r;
}

//
// 二元向量加法
//
template<class T>
CVector<T,2> operator + (const CVector<T,2> &v1, const CVector<T,2> &v2)
{
	CVector<T,2> r=v1;
	r+=v2;
	return r;
}

template<class T>
CVector<T,3> operator + (const CVector<T,3> &v1, const CVector<T,3> &v2)
{
	CVector<T,3> r=v1;
	r+=v2;
	return r;
}

template<class T>
CHomoVector4D<T> operator + (const CHomoVector4D<T> &v1, const CHomoVector4D<T> &v2)
{
	CHomoVector4D<T> r=v1;
	r+=v2;
	return r;
}

//
// 二元向量减法
//
template<class T>
CVector<T,2> operator - (const CVector<T,2> &v1, const CVector<T,2> &v2)
{
	CVector<T,2> r=v1;
	r-=v2;
	return r;
}

template<class T>
CVector<T,3> operator - (const CVector<T,3> &v1, const CVector<T,3> &v2)
{
	CVector<T,3> r=v1;
	r-=v2;
	return r;
}

template<class T>
CHomoVector4D<T> operator - (const CHomoVector4D<T> &v1, const CHomoVector4D<T> &v2)
{
	CHomoVector4D<T> r=v1;
	r-=v2;
	return r;
}

//