Blade Math Lib数学库
发布时间:2021-06-20 12:00:00    阅读次数:436
Blade Math Lib(Bmath)是我用C++开发的矢量数学库,主要用于二、三维图形计算。从2012年8月至今已经发展到2.0版本。囊括了向量、矩阵、四元数和简单物理碰撞检测各种运算。我用BMATH库开发过多款3D游戏和应用。在2016年把BMATH部分功能用JAVASCRIPT改写,让其适合在浏览器上开发,我的《机器人程序训练平台》就是用它和WEBGL一起制作出来的。

Blade Math Lib组成
Bmath2包括BMath2.h,BBase.cpp,BVector.cpp,BMatrix.cpp,BQuater.cpp,BPhy.cpp六个文件。

编译方式
BMath是用C++编写的,可以在Windows、Linux和Mac上直接编译使用。这里我只给出Linux下的编译方式。最后编译为:lib_bmath2.a静态库文件。
		g++ -c bbase.cpp bvector.cpp bmatrix.cpp bquater.cpp bphy.cpp
		ar cr lib_bmath2.a bbase.o bvector.o bmatrix.o bquater.o bphy.o
		rm bbase.o
		rm bvector.o
		rm bmatrix.o
		rm bquater.o
		rm bphy.o
		

bmath2.h头文件
声明所有变量和函数。
		#ifndef   BMATH_H
		#define   BMATH_H

		#include<stdio.h>
		#include<math.h>
		#include<stdlib.h>
		#include<time.h>

		typedef float B_FLOAT;

		const B_FLOAT B_PI = 3.1415926f;
		const B_FLOAT B_DELAY = 0.00000001f;

		const int CRASH_OUT = 0;
		const int CRASH_IN  = 1;
		const int CRASH_TOUCH = 2;

		B_FLOAT BSqrt(B_FLOAT a);
		B_FLOAT BPower(B_FLOAT a,int n);
		B_FLOAT BRound(B_FLOAT val);

		B_FLOAT* BAdd4(B_FLOAT* d,B_FLOAT* a,B_FLOAT* b);
		B_FLOAT* BSub4(B_FLOAT* d,B_FLOAT* a,B_FLOAT* b);
		B_FLOAT* BMul4(B_FLOAT* d,B_FLOAT* a,B_FLOAT b);
		B_FLOAT* BDiv4(B_FLOAT* d,B_FLOAT* a,B_FLOAT b);
		B_FLOAT* BSqrt4(B_FLOAT* d,B_FLOAT* a);


		struct Vector2
		{
			union
			{
				struct
				{
					B_FLOAT _x;
					B_FLOAT _y;
				};
				B_FLOAT _v[2];
			};
		};

		struct Vector3
		{
			union
			{
				struct
				{
					B_FLOAT _x;
					B_FLOAT _y;
					B_FLOAT _z;
				};
				B_FLOAT _v[3];
			};
		};

		struct Vector4
		{
			union
			{
				struct
				{
					B_FLOAT _x;
					B_FLOAT _y;
					B_FLOAT _z;
					B_FLOAT _w;
				};
				B_FLOAT _v[4];
			};
		};

		struct Matrix
		{
			union
			{
				struct
				{
					B_FLOAT _11,_12,_13,_14;
					B_FLOAT _21,_22,_23,_24;
					B_FLOAT _31,_32,_33,_34;
					B_FLOAT _41,_42,_43,_44;
				};
				B_FLOAT bm[16];
			};
		};

		class BVector2:public Vector2
		{
		public:
			BVector2();
			BVector2(B_FLOAT x,B_FLOAT y);
			BVector2(B_FLOAT *v);

			BVector2  operator =( BVector2 v ) ;
			BVector2  operator =( B_FLOAT* v ) ;

			BVector2  operator +=( BVector2 v ) ;
			BVector2  operator -=( BVector2 v ) ;
			BVector2  operator *=( B_FLOAT v) ;
			BVector2  operator /=( B_FLOAT v) ;

			BVector2  operator +(BVector2 V);
			BVector2  operator -(BVector2 V);
			BVector2  operator *(B_FLOAT V);
			BVector2  operator /(B_FLOAT V);

			BVector2 operator +() ;
			BVector2 operator -() ;

			bool operator ==( BVector2 v) ;
			bool operator !=( BVector2 v) ;

		};

		class BVector3:public Vector3
		{
		public:
			BVector3();
			BVector3(B_FLOAT x,B_FLOAT y,B_FLOAT z);
			BVector3(B_FLOAT *v);

			BVector3  operator =( BVector3 v ) ;
			BVector3  operator =( B_FLOAT* v ) ;

			BVector3  operator +=( BVector3 v ) ;
			BVector3  operator -=( BVector3 v ) ;
			BVector3  operator *=( B_FLOAT v) ;
			BVector3  operator /=( B_FLOAT v) ;

			BVector3  operator +(BVector3 V);
			BVector3  operator -(BVector3 V);
			BVector3  operator *(B_FLOAT V);
			BVector3  operator /(B_FLOAT V);

			BVector3 operator +() ;
			BVector3 operator -() ;

			bool operator ==( BVector3 v) ;
			bool operator !=( BVector3 v) ;

		};

		class BVector4:public Vector4
		{
		public:
			BVector4();
			BVector4(B_FLOAT x,B_FLOAT y,B_FLOAT z,B_FLOAT w);
			BVector4(B_FLOAT *v);

			BVector4 operator =( BVector4 v ) ;
			BVector4 operator =( B_FLOAT* v ) ;

			BVector4  operator +=( BVector4 v ) ;
			BVector4  operator -=( BVector4 v ) ;
			BVector4  operator *=( B_FLOAT v) ;
			BVector4  operator /=( B_FLOAT v) ;

			BVector4  operator +(BVector4 V);
			BVector4  operator -(BVector4 V);
			BVector4  operator *(B_FLOAT V);
			BVector4  operator /(B_FLOAT V);

			BVector4 operator +() ;
			BVector4 operator -() ;

			bool operator ==( BVector4 v) ;
			bool operator !=( BVector4 v) ;

		};

		//Matrix 16*16

		class BMatrix:public Matrix
		{
		public:
			BMatrix();
			BMatrix(B_FLOAT* val);

			BMatrix  operator +=( BMatrix m ) ;
			BMatrix  operator -=( BMatrix m ) ;
			BMatrix  operator *=( B_FLOAT v) ;
			BMatrix  operator /=( B_FLOAT v) ;

			BMatrix  operator +( BMatrix m ) ;
			BMatrix  operator -( BMatrix m ) ;
			BMatrix  operator *( B_FLOAT v) ;
			BMatrix  operator /( B_FLOAT v) ;

			bool operator == ( BMatrix m) ;
			bool operator != ( BMatrix m) ;

			B_FLOAT operator ()(int n);
			B_FLOAT operator ()(int row,int col);

			void    Identity();
			void    Inverse();

		};

		//QUATERNION
		class BQuaternion:public Vector4
		{
		public:
			BQuaternion();
			BQuaternion(B_FLOAT* val);
			BQuaternion(B_FLOAT x,B_FLOAT y,B_FLOAT z,B_FLOAT w);

			BQuaternion operator +=(BQuaternion q);
			BQuaternion operator -=(BQuaternion q);
			BQuaternion operator *=(BQuaternion q);
			BQuaternion operator *=(B_FLOAT v);
			BQuaternion operator /=(B_FLOAT v);

			BQuaternion operator +();
			BQuaternion operator -();

			BQuaternion operator +(BQuaternion q);
			BQuaternion operator -(BQuaternion q);
			BQuaternion operator *(BQuaternion q);
			BQuaternion operator *(B_FLOAT v);
			BQuaternion operator /(B_FLOAT v);

			bool operator == ( BQuaternion q) ;
				bool operator != ( BQuaternion q) ;


		};

		//common
		const char* GetVerID();
		B_FLOAT     GetRandom();
		long GetTime();
		B_FLOAT BSin(B_FLOAT val);
		B_FLOAT BCos(B_FLOAT val);

		BVector2*   BVec2Lerp(BVector2* dest,BVector2* src1,BVector2* src2,B_FLOAT s);
		B_FLOAT BVec2LengthSq(BVector2* src);
		B_FLOAT     BVec2DistanceSq(BVector2* src,BVector2* det);
		BVector2*   BVec2Maximize(BVector2* dest,BVector2* src1,BVector2* src2);
		BVector2*   BVec2Minimize(BVector2* dest,BVector2* src1,BVector2* src2);
		BVector2*   BVec2Normalize(BVector2* dest,BVector2* src);
		B_FLOAT BVec2Cross(BVector2* src1,BVector2* src2);
		B_FLOAT BVec2Dot(BVector2* src1,BVector2* src2);
		B_FLOAT     BVec2ComputAngle(BVector2* v1,BVector2* v2,BVector2* v3);

		BVector3*   BVec3Lerp(BVector3* dest,BVector3* src1,BVector3* src2,B_FLOAT s);
		B_FLOAT BVec3LengthSq(BVector3* src);
		B_FLOAT     BVec3DistanceSq(BVector3* src,BVector3* det);
		BVector3*   BVec3Maximize(BVector3* dest,BVector3* src1,BVector3* src2);
		BVector3*   BVec3Minimize(BVector3* dest,BVector3* src1,BVector3* src2);
		BVector3*   BVec3Normalize(BVector3* dest,BVector3* src);
		BVector3*   BVec3Cross(BVector3* dest,BVector3* src1,BVector3* src2);
		B_FLOAT BVec3Dot(BVector3* src1,BVector3* src2);
		B_FLOAT     BVec3ComputAngle(BVector3* v1,BVector3* v2,BVector3* v3);

		BVector4*   BVec4Lerp(BVector4* dest,BVector4* src1,BVector4* src2,B_FLOAT s);
		B_FLOAT BVec4LengthSq(BVector4* src);
		B_FLOAT     BVec4DistanceSq(BVector4* src,BVector4* det);
		BVector4*   BVec4Maximize(BVector4* dest,BVector4* src1,BVector4* src2);
		BVector4*   BVec4Minimize(BVector4* dest,BVector4* src1,BVector4* src2);
		BVector4*   BVec4Normalize(BVector4* dest,BVector4* src);
		BVector4*   BVec4Cross(BVector4* dest,BVector4* src1,BVector4* src2,BVector4* src3);
		B_FLOAT BVec4Dot(BVector4* src1,BVector4* src2);

		BVector4*   BVec2Transform(BVector4* dest,BVector2* src,BMatrix*  m);
		BVector4*   BVec3Transform(BVector4* dest,BVector3* src,BMatrix*  m);
		BVector4*   BVec4Transform(BVector4* dest,BVector4* src,BMatrix*  m);

		B_FLOAT BQuatLengthSq(BQuaternion* src);
		BQuaternion*    BQuatNormalize(BQuaternion* dest,BQuaternion* src);
		BQuaternion*    BQuatSlerp(BQuaternion* dest,BQuaternion* src1, BQuaternion* src2, B_FLOAT s,bool line=true);
		BQuaternion*    BQuatRotationAxis(BQuaternion* dest,BVector3* pv,B_FLOAT angle);
		void MatToQuat(BQuaternion* dest,BMatrix* src);
		void QuatToMat(BMatrix* dest,BQuaternion* src);

		BVector2*    BVec2BaryCentric(BVector2* dest,BVector2* point1,BVector2* point2,BVector2* point3,B_FLOAT f,B_FLOAT g);
		BVector3*    BVec3BaryCentric(BVector3* dest,BVector3* point1,BVector3* point2,BVector3* point3,B_FLOAT f,B_FLOAT g);

		BVector2*       BVec2CatmullRom(BVector2* dest,BVector2* src1,BVector2* src2,BVector2* src3,BVector2* src4,B_FLOAT s);
		BVector3*       BVec3CatmullRom(BVector3* dest,BVector3* src1,BVector3* src2,BVector3* src3,BVector3* src4,B_FLOAT s);
		BVector4*       BVec4CatmullRom(BVector4* dest,BVector4* src1,BVector4* src2,BVector4* src3,BVector4* src4,B_FLOAT s);

		BVector2*       BVec2Hermite(BVector2* dest,BVector2* src1,BVector2* tan1,BVector2* src2,BVector2* tan2,B_FLOAT s);
		BVector3*       BVec3Hermite(BVector3* desc,BVector3* src1,BVector3* tan1,BVector3* src2,BVector3* tan2,B_FLOAT s);
		BVector4*       BVec4Hermite(BVector4* dest,BVector4* src1,BVector4* tan1,BVector4* src2,BVector4* tan2,B_FLOAT s);

		BMatrix*    BMatrixLookAtRH(BMatrix* dest,BVector3* pEye,BVector3* pAt,BVector3* pUp);
		BMatrix*    BMatrixLookAtLH(BMatrix* dest,BVector3* pEye,BVector3* pAt,BVector3* pUp);

		BMatrix*    BMatrixOrthoLH(BMatrix* dest,B_FLOAT w,B_FLOAT h,B_FLOAT zn,B_FLOAT zf);
		BMatrix*    BMatrixOrthoRH(BMatrix* dest,B_FLOAT w,B_FLOAT h,B_FLOAT zn,B_FLOAT zf);

		BMatrix*    BMatrixPerspectiveLH(BMatrix* dest,B_FLOAT w,B_FLOAT h,B_FLOAT zn,B_FLOAT zf);
		BMatrix*    BMatrixPerspectiveRH(BMatrix* dest,B_FLOAT w,B_FLOAT h,B_FLOAT zn,B_FLOAT zf);

		BMatrix*    BMatrixPerspectiveFovLH(BMatrix* dest,B_FLOAT fovy,B_FLOAT aspect,B_FLOAT zn,B_FLOAT zf);
		BMatrix*    BMatrixPerspectiveFovRH(BMatrix* dest,B_FLOAT fovy,B_FLOAT aspect,B_FLOAT zn,B_FLOAT zf);

		BVector3*       BMatrixUnProj(BVector3 pt,BMatrix* matView,BMatrix* matProj,int* viewport,BVector3* dest);

		BMatrix*    BMatrixTranslation(BMatrix* dest,B_FLOAT x,B_FLOAT y,B_FLOAT z);
		BMatrix* BMatrixScaling(BMatrix* dest,B_FLOAT x,B_FLOAT y,B_FLOAT z);
		BMatrix*    BMatrixRotateX(BMatrix* dest,B_FLOAT angle);
		BMatrix*    BMatrixRotateY(BMatrix* dest,B_FLOAT angle);
		BMatrix*    BMatrixRotateZ(BMatrix* dest,B_FLOAT angle);
		BMatrix* BMatrixMultiply(BMatrix* dest,BMatrix* src1,BMatrix* src2);

		BMatrix*    BMatrixTranspose(BMatrix* dest,BMatrix* src);
		BMatrix*    BMatrixInverse(BMatrix* dest,BMatrix* src);

		bool BRay2Triangle(BVector3 pStart,BVector3 pEnd,BVector3 p1,BVector3 p2,BVector3 p3,B_FLOAT* len);
		bool     BRay2Quad(BVector3 pStart,BVector3 pEnd,BVector3 p1,BVector3 p2,BVector3 p3,BVector3 p4,B_FLOAT* len);
		bool     BRay2Box(BVector3 pStart,BVector3 pEnd,BVector3 MinVector,BVector3 MaxVector,B_FLOAT* len,int* style);

		int     CrashQuad2Quad(BVector2 v1,BVector2 v2,BVector2 m1,BVector2 m2);
		int      CrashBox2Box(BVector3 Min1,BVector3 Max1,BVector3 Min2,BVector3 Max2);
		bool     CrashTriangle(BVector3 a1,BVector3 a2,BVector3 a3,BVector3 b1,BVector3 b2,BVector3 b3);

		BVector3 GetRayPos(BVector3 pStart,BVector3 pEnd,B_FLOAT v);

		#endif
		

BBase.cpp源文件
定义常用数学运算函数和数据类型。
			#include "BMath2.h"
			const char*    GetVerID()
			{
				return "2.0";
			}

			B_FLOAT       GetRandom()
			{
				// 0-1
				B_FLOAT v = sin(B_FLOAT(clock()));

				if(v<0.000000f)
					v+=1.0f;

				return v;

			}

			long GetTime()
			{
				return clock();
			}

			B_FLOAT BSqrt(B_FLOAT a)
			{
				return sqrt(a);
			}

			B_FLOAT BPower(B_FLOAT a,int n)
			{
				B_FLOAT t = 1.000000f;

				while(n>0)
				{
					t = t*a;
					n--;
				}

				return t;
			}

			B_FLOAT* BAdd4(B_FLOAT* d,B_FLOAT* a,B_FLOAT* b)
			{
				d[0] = a[0] + b[0] ;
				d[1] = a[1] + b[1] ;
				d[2] = a[2] + b[2] ;
				d[3] = a[3] + b[3] ;
				return d;
			}

			B_FLOAT* BSub4(B_FLOAT* d,B_FLOAT* a,B_FLOAT* b)
			{
				d[0] = a[0] - b[0] ;
				d[1] = a[1] - b[1] ;
				d[2] = a[2] - b[2] ;
				d[3] = a[3] - b[3] ;
				return d;
			}

			B_FLOAT* BMul4(B_FLOAT* d,B_FLOAT* a,B_FLOAT b)
			{
				d[0] = a[0] * b ;
				d[1] = a[1] * b ;
				d[2] = a[2] * b ;
				d[3] = a[3] * b ;
				return d;
			}

			B_FLOAT* BDiv4(B_FLOAT* d,B_FLOAT* a,B_FLOAT b)
			{
				d[0] = a[0] / b ;
				d[1] = a[1] / b ;
				d[2] = a[2] / b ;
				d[3] = a[3] / b ;
				return d;
			}

			B_FLOAT* BSqrt4(B_FLOAT* d,B_FLOAT* a)
			{
				d[0] = sqrt(a[0]);
				d[1] = sqrt(a[1]);
				d[2] = sqrt(a[2]);
				d[3] = sqrt(a[3]);

				return d;
			}

			B_FLOAT BSin(B_FLOAT val)
			{
				return sin(val);
			}

			B_FLOAT BCos(B_FLOAT val)
			{
				return cos(val);
			}

			B_FLOAT BRound(B_FLOAT v)
			{
				double i=0;

				B_FLOAT c = modf(v*10,&i);

				if(c>=0.5f)
					v = floor(v*10.0f)+1.0f;
				else
					v = floor(v*10.0f);

				return v/10.0f;
			}
		

BVector.cpp源文件
定义二维、三维和四维的向量相关运算。
		#include "BMath2.h"

		BVector2::BVector2()
		{
			_x = 0.0f;
			_y = 0.0f;
		}

		BVector2::BVector2(B_FLOAT x,B_FLOAT y)
		{
			_x = x;
			_y = y;
		}

		BVector2::BVector2(B_FLOAT *v)
		{
			_x = v[0];
			_y = v[1];
		}


		BVector2 BVector2::operator =( BVector2 v )
		{
			_x = v._x;
			_y = v._y;

			return *this;
		}

		BVector2 BVector2::operator =( B_FLOAT* v )
		{
			_x = v[0];
			_y = v[1];

			return *this;
		}


		BVector2 BVector2::operator +=( BVector2 v )
		{
			_x = _x+v._x;
			_y = _y+v._y;

			return *this;
		}

		BVector2 BVector2::operator -=( BVector2 v )
		{
			_x = _x-v._x;
			_y = _y-v._y;

			return *this;
		}

		BVector2 BVector2::operator *=( B_FLOAT v)
		{
			_x = _x*v;
			_y = _y*v;

			return *this;
		}

		BVector2 BVector2::operator /=( B_FLOAT v)
		{
			_x = _x/v;
			_y = _y/v;

			return *this;
		}

		BVector2  BVector2::operator +(BVector2 V)
		{
			return BVector2(_x+V._x,_y+V._y);
		}

		BVector2  BVector2::operator -(BVector2 V)
		{
			return BVector2(_x-V._x,_y-V._y);
		}

		BVector2  BVector2::operator *(B_FLOAT V)
		{
			return BVector2(_x*V,_y*V);
		}

		BVector2  BVector2::operator /(B_FLOAT V)
		{
			return BVector2(_x/V,_y/V);
		}

		BVector2 BVector2::operator +()
		{
			return *this;
		}

		BVector2 BVector2::operator -()
		{
			return BVector2(-_x,-_y);
		}

		bool BVector2::operator ==( BVector2 v)
		{
			if(abs(_x- v._x)<0.000001f &&
			   abs(_y- v._y)<0.000001f )
				return true;

			return false;
		}

		bool BVector2::operator !=( BVector2 v )
		{
			if(abs(_x- v._x)>=0.000001f ||
			   abs(_y- v._y)>=0.000001f)
			   return true;

			return false;
		}

		//vector3
		BVector3::BVector3()
		{
			_x = 0.0f;
			_y = 0.0f;
			_z = 0.0f;
		}

		BVector3::BVector3(B_FLOAT x,B_FLOAT y,B_FLOAT z)
		{
			_x = x;
			_y = y;
			_z = z;
		}

		BVector3::BVector3(B_FLOAT *v)
		{
			_x = v[0];
			_y = v[1];
			_z = v[2];
		}


		BVector3 BVector3::operator =( BVector3 v )
		{
			_x = v._x;
			_y = v._y;
			_z = v._z;

			return *this;
		}

		BVector3 BVector3::operator =( B_FLOAT* v )
		{
			_x = v[0];
			_y = v[1];
			_z = v[2];

			return *this;
		}


		BVector3 BVector3::operator +=( BVector3 v )
		{
			_x = _x+v._x;
			_y = _y+v._y;
			_z = _z+v._z;

			return *this;
		}

		BVector3 BVector3::operator -=( BVector3 v )
		{
			_x = _x-v._x;
			_y = _y-v._y;
			_z = _z-v._z;

			return *this;
		}

		BVector3 BVector3::operator *=( B_FLOAT v)
		{
			_x = _x*v;
			_y = _y*v;
			_z = _z*v;

			return *this;
		}

		BVector3 BVector3::operator /=( B_FLOAT v)
		{
			_x = _x/v;
			_y = _y/v;
			_z = _z/v;

			return *this;
		}

		BVector3  BVector3::operator +(BVector3 V)
		{
			return BVector3(_x+V._x,_y+V._y,_z+V._z);
		}

		BVector3  BVector3::operator -(BVector3 V)
		{
			return BVector3(_x-V._x,_y-V._y,_z-V._z);
		}

		BVector3  BVector3::operator *(B_FLOAT V)
		{
			return BVector3(_x*V,_y*V,_z*V);
		}

		BVector3  BVector3::operator /(B_FLOAT V)
		{
			return BVector3(_x/V,_y/V,_z/V);
		}

		BVector3 BVector3::operator +()
		{
			return *this;
		}

		BVector3 BVector3::operator -()
		{
			return BVector3(-_x,-_y,-_z);
		}

		bool BVector3::operator ==( BVector3 v)
		{
			if(abs(_x- v._x)<0.000001f &&
			   abs(_y- v._y)<0.000001f &&
			   abs(_z- v._z)<0.000001f)
				return true;

			return false;
		}

		bool BVector3::operator !=( BVector3 v )
		{
			if(abs(_x- v._x)>=0.000001f ||
			   abs(_y- v._y)>=0.000001f ||
			   abs(_z- v._z)>=0.000001f)
			   return true;

			return false;
		}

		// BVector4
		BVector4::BVector4()
		{
			_x = 0.0f;
			_y = 0.0f;
			_z = 0.0f;
			_w = 0.0f;
		}

		BVector4::BVector4(B_FLOAT x,B_FLOAT y,B_FLOAT z,B_FLOAT w)
		{
			_x = x;
			_y = y;
			_z = z;
			_w = w;
		}

		BVector4::BVector4(B_FLOAT *v)
		{
			_x = v[0];
			_y = v[1];
			_z = v[2];
			_w = v[3];
		}

		BVector4 BVector4::operator =( BVector4 v )
		{
			_x = v._x;
			_y = v._y;
			_z = v._z;
			_w = v._w;

			return *this;
		}

		BVector4 BVector4::operator =( B_FLOAT* v )
		{
			_x = v[0];
			_y = v[1];
			_z = v[2];
			_w = v[3];

			return *this;
		}


		BVector4 BVector4::operator +=( BVector4 v )
		{
			BAdd4(_v,_v,v._v);

			return *this;
		}

		BVector4 BVector4::operator -=( BVector4 v )
		{
			BSub4(_v,_v,v._v);
			return *this;
		}

		BVector4 BVector4::operator *=( B_FLOAT v)
		{
			BMul4(_v,_v,v);
			return *this;
		}

		BVector4 BVector4::operator /=( B_FLOAT v)
		{
			BDiv4(_v,_v,v);

			return *this;
		}

		BVector4  BVector4::operator +(BVector4 V)
		{
			return BVector4(_x+V._x,_y+V._y,_z+V._z,_w+V._w);
		}

		BVector4  BVector4::operator -(BVector4 V)
		{
			return BVector4(_x-V._x,_y-V._y,_z-V._z,_w-V._w);
		}

		BVector4  BVector4::operator *(B_FLOAT V)
		{
			return BVector4(_x*V,_y*V,_z*V,_w*V);
		}

		BVector4  BVector4::operator /(B_FLOAT V)
		{
			return BVector4(_x/V,_y/V,_z/V,_w/V);
		}

		BVector4 BVector4::operator +()
		{
			return *this;
		}

		BVector4 BVector4::operator -()
		{
			return BVector4(-_x,-_y,-_z,_w);
		}

		bool BVector4::operator ==( BVector4 v)
		{
			if(abs(_x- v._x)<0.000001f &&
			   abs(_y- v._y)<0.000001f &&
			   abs(_z- v._z)<0.000001f &&
			   abs(_w- v._w)<0.000001f)
				return true;

			return false;
		}

		bool BVector4::operator !=( BVector4 v )
		{
			if(abs(_x- v._x)>=0.000001f ||
			   abs(_y- v._y)>=0.000001f ||
			   abs(_z- v._z)>=0.000001f ||
			   abs(_w- v._w)>=0.000001f)
			   return true;

			return false;
		}

		BVector2* BVec2Lerp(BVector2* dest,BVector2* src1,BVector2* src2,B_FLOAT s)
		{
			dest->_x = src1->_x + ( src2->_x - src1->_x ) * s ;
			dest->_y = src1->_y + ( src2->_y - src1->_y ) * s ;

			return dest;
		}

		B_FLOAT BVec2LengthSq(BVector2* src)
		{
			return (src->_x*src->_x) + (src->_y * src->_y);
		}

		B_FLOAT       BVec2DistanceSq(BVector2* src,BVector2* det)
		{
				return (src->_x-det->_x)*(src->_x-det->_x)+(src->_y-det->_y)*(src->_y-det->_y);
		}

		BVector2*    BVec2Maximize(BVector2* dest,BVector2* src1,BVector2* src2)
		{
			if( src1->_x > src2->_x )
				dest->_x = src1->_x;
			else
				dest->_x = src2->_x;

			if( src1->_y > src2->_y )
				dest->_y = src1->_y;
			else
				dest->_y = src2->_y;

			return dest;
		}

		BVector2*   BVec2Minimize(BVector2* dest,BVector2* src1,BVector2* src2)
		{
			if( src1->_x < src2->_x )
				dest->_x = src1->_x;
			else
				dest->_x = src2->_x;

			if( src1->_y < src2->_y )
				dest->_y = src1->_y;
			else
				dest->_y = src2->_y;

			return dest;
		}

		BVector2*   BVec2Normalize(BVector2* dest,BVector2* src)
		{
			B_FLOAT s = BSqrt( src->_x*src->_x + src->_y*src->_y);

			*dest = *src / s;

			return dest;
		}

		B_FLOAT BVec2Cross(BVector2* src1,BVector2* src2)
		{
			return src1->_x * src2->_y - src1->_y * src2->_x;
		}

		B_FLOAT BVec2Dot(BVector2* src1,BVector2* src2)
		{
			return src1->_x*src2->_x + src1->_y*src2->_y;
		}

		B_FLOAT     BVec2ComputAngle(BVector2* v1,BVector2* v2,BVector2* v3)
		{
			float val = (v2->_x-v1->_x) * (v2->_x-v3->_x) + (v2->_y-v1->_y) * (v2->_y-v3->_y) ;

			float t1 = sqrt( (v2->_x-v1->_x) * (v2->_x-v1->_x) + (v2->_y-v1->_y) * (v2->_y-v1->_y) );
			float t2 = sqrt( (v2->_x-v3->_x) * (v2->_x-v3->_x) + (v2->_y-v3->_y) * (v2->_y-v3->_y) );

			return acos(val/(t1*t2));
		}


		BVector3*    BVec3Lerp(BVector3* dest,BVector3* src1,BVector3* src2,B_FLOAT s)
		{
			dest->_x =  src1->_x +  ( src2->_x - src1->_x ) *s ;
			dest->_y =  src1->_y +  ( src2->_y - src1->_y ) *s ;
			dest->_z =  src1->_z +  ( src2->_z - src1->_z ) *s ;

			return dest;
		}


		B_FLOAT BVec3LengthSq(BVector3* src)
		{
			return src->_x*src->_x+src->_y * src->_y + src->_z*src->_z;
		}

		B_FLOAT       BVec3DistanceSq(BVector3* src,BVector3* det)
		{
			return (src->_x-det->_x)*(src->_x-det->_x)+(src->_y-det->_y)*(src->_y-det->_y)+(src->_z-det->_z)*(src->_z-det->_z);

		}

		BVector3*    BVec3Maximize(BVector3* dest,BVector3* src1,BVector3* src2)
		{
			if( src1->_x > src2->_x )
				dest->_x = src1->_x;
			else
				dest->_x = src2->_x;

			if( src1->_y > src2->_y )
				dest->_y = src1->_y;
			else
				dest->_y = src2->_y;

			if( src1->_z > src2->_z )
				dest->_z = src1->_z;
			else
				dest->_z = src2->_z;

			return dest;
		}

		BVector3*   BVec3Minimize(BVector3* dest,BVector3* src1,BVector3* src2)
		{
			if( src1->_x < src2->_x )
				dest->_x = src1->_x;
			else
				dest->_x = src2->_x;

			if( src1->_y < src2->_y )
				dest->_y = src1->_y;
			else
				dest->_y = src2->_y;

			if( src1->_z < src2->_z )
				dest->_z = src1->_z;
			else
				dest->_z = src2->_z;

			return dest;
		}

		BVector3*   BVec3Normalize(BVector3* dest,BVector3* src)
		{
			B_FLOAT s = sqrt(BVec3LengthSq(src));

			*dest = *src / s;

			return dest;
		}

		BVector3* BVec3Cross(BVector3* dest,BVector3* src1,BVector3* src2)
		{
			dest->_x = src1->_y * src2->_z - src1->_z * src2->_y;
			dest->_y = src1->_z * src2->_x - src1->_x * src2->_z;
			dest->_z = src1->_x * src2->_y - src1->_y * src2->_x;

			return dest;
		}

		B_FLOAT BVec3Dot(BVector3* src1,BVector3* src2)
		{
			return src1->_x*src2->_x + src1->_y*src2->_y + src1->_z*src2->_z;
		}


		B_FLOAT BVec3ComputAngle(BVector3* v1,BVector3* v2,BVector3* v3)
		{
			float val = (v2->_x-v1->_x) * (v2->_x-v3->_x) + (v2->_y-v1->_y) * (v2->_y-v3->_y) + 
						(v2->_z-v1->_z) * (v2->_z-v3->_z);

			float t1 = sqrt( (v2->_x-v1->_x) * (v2->_x-v1->_x) + (v2->_y-v1->_y) * (v2->_y-v1->_y) + 
							 (v2->_z-v1->_z) * (v2->_z-v1->_z));
			float t2 = sqrt( (v2->_x-v3->_x) * (v2->_x-v3->_x) + (v2->_y-v3->_y) * (v2->_y-v3->_y) + 
							 (v2->_z-v3->_z) * (v2->_z-v3->_z));

			return acos(val/(t1*t2));
		}

		BVector4* BVec4Lerp(BVector4* dest,BVector4* src1,BVector4* src2,B_FLOAT s)
		{
			dest->_x = src1->_x + (src2->_x - src1->_x)  *s ;
			dest->_y = src1->_y + (src2->_y - src1->_y)  *s ;
			dest->_z = src1->_z + (src2->_z - src1->_z)  *s ;
			dest->_w = src1->_w + (src2->_w - src1->_w)  *s ;

			return dest;
		}

		B_FLOAT BVec4LengthSq(BVector4* src)
		{
			return src->_x*src->_x+src->_y * src->_y + src->_z*src->_z + src->_w *src->_w;
		}

		B_FLOAT BVec4DistanceSq(BVector4* src,BVector4* det)
		{
			return (src->_x-det->_x)*(src->_x-det->_x)+(src->_y-det->_y)*(src->_y-det->_y)+
			       (src->_z-det->_z)*(src->_z-det->_z)+(src->_w-det->_w)*(src->_w-det->_w);

		}

		BVector4* BVec4Maximize(BVector4* dest,BVector4* src1,BVector4* src2)
		{
			if( src1->_x > src2->_x )
				dest->_x = src1->_x;
			else
				dest->_x = src2->_x;

			if( src1->_y > src2->_y )
				dest->_y = src1->_y;
			else
				dest->_y = src2->_y;

			if( src1->_z > src2->_z )
				dest->_z = src1->_z;
			else
				dest->_z = src2->_z;

			if( src1->_w > src2->_w )
				dest->_w = src1->_w;
			else
				dest->_w = src2->_w;

			return dest;
		}

		BVector4* BVec4Minimize(BVector4* dest,BVector4* src1,BVector4* src2)
		{
			if( src1->_x < src2->_x )
				dest->_x = src1->_x;
			else
				dest->_x = src2->_x;

			if( src1->_y < src2->_y )
				dest->_y = src1->_y;
			else
				dest->_y = src2->_y;

			if( src1->_z < src2->_z )
				dest->_z = src1->_z;
			else
				dest->_z = src2->_z;

			if( src1->_w < src2->_w )
				dest->_w = src1->_w;
			else
				dest->_w = src2->_w;

			return dest;
		}

		BVector4* BVec4Normalize(BVector4* dest,BVector4* src)
		{
			B_FLOAT s = sqrt(BVec4LengthSq(src));

			*dest = *src / s;

			return dest;
		}

		BVector4* BVec4Cross(BVector4* dest,BVector4* src1,BVector4* src2,BVector4* src3)
		{
			B_FLOAT a1 = (src2->_z*src3->_w)-(src2->_w*src3->_z);
			B_FLOAT a2 = (src2->_y*src3->_w)-(src2->_w*src3->_y);
			B_FLOAT a3 = (src2->_y*src3->_z)-(src2->_z*src3->_y);
			B_FLOAT a4 = (src2->_x*src3->_z)-(src2->_z*src3->_x);
			B_FLOAT a5 = (src2->_x*src3->_w)-(src2->_w*src3->_x);
			B_FLOAT a6 = (src2->_x*src3->_y)-(src2->_y*src3->_x);

			dest->_x =  (src1->_y*a1) - (src1->_z*a2)  + (src1->_w*a3);
			dest->_y = -((src1->_x*a1) - (src1->_z*a5)  + (src1->_w*a4));
			dest->_z =  (src1->_x*a2) - (src1->_y*a5)  + (src1->_w*a6);
			dest->_w = -((src1->_x*a3) - (src1->_y*a4)  + (src1->_z*a6));

			return dest;
		}

		B_FLOAT BVec4Dot(BVector4* src1,BVector4* src2)
		{
			return src1->_x*src2->_x + src1->_y*src2->_y + src1->_z*src2->_z + src1->_w*src2->_w;
		}


		BVector4* BVec2Transform(BVector4* dest,BVector2* src,BMatrix*  m)
		{
			BVector4 T(src->_x,src->_y,0,1.0f);

			BVec4Transform(dest,&T,m);

			return dest;
		}

		BVector4* BVec3Transform(BVector4* dest,BVector3* src,BMatrix*  m)
		{
			BVector4 T(src->_x,src->_y,src->_z,1.0f);

			BVec4Transform(dest,&T,m);

			return dest;
		}

		BVector4* BVec4Transform(BVector4* dest,BVector4* src,BMatrix*  m)
		{
			dest->_x =  src->_x*m->bm[0]+src->_y*m->bm[4]+src->_z*m->bm[8]+src->_w*m->bm[12];
			dest->_y =  src->_x*m->bm[1]+src->_y*m->bm[5]+src->_z*m->bm[9]+src->_w*m->bm[13];
			dest->_z =  src->_x*m->bm[2]+src->_y*m->bm[6]+src->_z*m->bm[10]+src->_w*m->bm[14];
			dest->_w =  src->_x*m->bm[3]+src->_y*m->bm[7]+src->_z*m->bm[11]+src->_w*m->bm[15];

			return dest;
		}

		BVector2* BVec2BaryCentric(BVector2* dest,BVector2* point1,BVector2* point2,BVector2* point3,B_FLOAT f,B_FLOAT g)
		{
			*dest = *point1 + (*point2- *point1)*f + (*point3 - *point1)*g;
			return dest;
		}

		BVector3* BVec3BaryCentric(BVector3* dest,BVector3* point1,BVector3* point2,BVector3* point3,B_FLOAT f,B_FLOAT g)
		{
			*dest = *point1 + (*point2- *point1)*f + (*point3 - *point1)*g;
			return dest;
		}

		BVector2* BVec2CatmullRom(BVector2* dest,BVector2* src1,BVector2* src2,BVector2* src3,BVector2* src4,B_FLOAT s)
		{
			BVector2 t1 = (*src3 - *src1)/2;
			BVector2 t2 = (*src4 - *src2)/2;

			B_FLOAT s2 = BPower(s,2);
			B_FLOAT s3 = BPower(s,3);

			*dest = *src2 * (2*s3-3*s2+1) + *src3 * (-2*s3+3*s2) + t1*(s3-2*s2+s) + t2*(s3-s2);

			return dest;
		}

		BVector3* BVec3CatmullRom(BVector3* dest,BVector3* src1,BVector3* src2,BVector3* src3,BVector3* src4,B_FLOAT s)
		{
			BVector3 t1 = (*src3 - *src1)/2;
			BVector3 t2 = (*src4 - *src2)/2;

			B_FLOAT s2 = BPower(s,2);
			B_FLOAT s3 = BPower(s,3);

			*dest = *src2 * (2*s3-3*s2+1) + *src3 * (-2*s3+3*s2) + t1*(s3-2*s2+s) + t2*(s3-s2);
			return dest;
		}

		BVector4* BVec4CatmullRom(BVector4* dest,BVector4* src1,BVector4* src2,BVector4* src3,BVector4* src4,B_FLOAT s)
		{
			BVector4 t1 = (*src3 - *src1)/2;
			BVector4 t2 = (*src4 - *src2)/2;

			B_FLOAT s2 = BPower(s,2);
			B_FLOAT s3 = BPower(s,3);

			*dest = *src2 * (2*s3-3*s2+1) + *src3 * (-2*s3+3*s2) + t1*(s3-2*s2+s) + t2*(s3-s2);

			return dest;
		}

		BVector2* BVec2Hermite(BVector2* dest,BVector2* src1,BVector2* tan1,BVector2* src2,BVector2* tan2,B_FLOAT s)
		{
			BVector2 A = *src1 * 2 - *src2 * 2 + *tan2 + *tan1;
			BVector2 B = *src2 * 3 - *src1 * 3 - *tan1*2 - *tan2;

			*dest = A*BPower(s,3) + B*BPower(s,2) + *tan1*s + *src1;

			return dest;
		}

		BVector3* BVec3Hermite(BVector3* dest,BVector3* src1,BVector3* tan1,BVector3* src2,BVector3* tan2,B_FLOAT s)
		{
			BVector3 A = *src1 * 2 - *src2 * 2 + *tan2 + *tan1;
			BVector3 B = *src2 * 3 - *src1 * 3 - *tan1*2 - *tan2;

			*dest = A*BPower(s,3) + B*BPower(s,2) + *tan1*s + *src1;

			return dest;
		}


		BVector4* BVec4Hermite(BVector4* dest,BVector4* src1,BVector4* tan1,BVector4* src2,BVector4* tan2,B_FLOAT s)
		{
			BVector4 A = *src1 * 2 - *src2 * 2 + *tan2 + *tan1;
			BVector4 B = *src2 * 3 - *src1 * 3 - *tan1*2 - *tan2;

			*dest = A*BPower(s,3) + B*BPower(s,2) + *tan1*s + *src1;

			return dest;
		}
		

BMatirx.cpp源文件
定义矩阵转置、相乘和转换等相关运算。
		#include "BMath2.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;
			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;
		}
		

BQuater.cpp源文件
定义四元数转换和旋转的相关运算。
		#include"BMath2.h"

		BQuaternion::BQuaternion()
		{
			_x = 0;
			_y = 0;
			_z = 0;
			_w = 0;
		}

		BQuaternion::BQuaternion(B_FLOAT* val)
		{
			_x = val[0];
			_y = val[1];
			_z = val[2];
			_w = val[3];
		}

		BQuaternion::BQuaternion(B_FLOAT x,B_FLOAT y,B_FLOAT z,B_FLOAT w)
		{
			_x = x;
			_y = y;
			_z = z;
			_w = w;
		}

		BQuaternion BQuaternion::operator +=(BQuaternion q)
		{
			_x = _x + q._x;
			_y = _y + q._y;
			_z = _z + q._z;
			_w = _w + q._w;

			return *this;
		}

		BQuaternion BQuaternion::operator -=(BQuaternion q)
		{
			_x = _x + q._x;
			_y = _y + q._y;
			_z = _z + q._z;
			_w = _w + q._w;

			return *this;
		}

		BQuaternion BQuaternion::operator *=(BQuaternion q)
		{
			/*B_FLOAT w = BSub(BSub(BSub(BMul(_w,q._w),BMul(_x,q._x)),BMul(_y,q._y)),BMul(_z,q._z));
			B_FLOAT x = BSub(BAdd(BAdd(BMul(_w,q._x),BMul(_x,q._w)),BMul(_y,q._z)),BMul(_z,q._y));
			B_FLOAT y = BSub(BAdd(BAdd(BMul(_w,q._y),BMul(_y,q._w)),BMul(_z,q._x)),BMul(_x,q._z));
			B_FLOAT z = BSub(BAdd(BAdd(BMul(_w,q._z),BMul(_z,q._w)),BMul(_x,q._y)),BMul(_y,q._x));*/

			B_FLOAT w = _w*q._w - _x*q._x - _y*q._y - _z*q._z;
			B_FLOAT x = _w*q._x + _x*q._w + _y*q._z - _z*q._y;
			B_FLOAT y = _w*q._y + _y*q._w + _z*q._x - _x*q._z;
			B_FLOAT z = _w*q._z + _z*q._w + _x*q._y - _y*q._x;

			_x = x;
			_y = y;
			_z = z;
			_w = w;

			return *this;
		}

		BQuaternion BQuaternion::operator *=(B_FLOAT v)
		{
			_x = _x * v;
			_y = _y * v;
			_z = _z * v;
			_w = _w * v;

			return *this;
		}

		BQuaternion BQuaternion::operator /=(B_FLOAT v)
		{
			_x = _x / v;
			_y = _y / v;
			_z = _z / v;
			_w = _w / v;

			return *this;
		}

		BQuaternion BQuaternion::operator +()
		{
			return *this;
		}

		BQuaternion BQuaternion::operator -()
		{
			return BQuaternion(-_x,-_y,-_z,-_w);
		}

		BQuaternion BQuaternion::operator +(BQuaternion q)
		{
			return BQuaternion(_x + q._x,_y + q._y ,_z + q._z,_w + q._w);
		}

		BQuaternion BQuaternion::operator -(BQuaternion q)
		{
			return BQuaternion(_x - q._x,_y - q._y ,_z - q._z,_w - q._w);
		}

		BQuaternion BQuaternion::operator *(BQuaternion q)
		{
			B_FLOAT w = _w*q._w - _x*q._x - _y*q._y - _z*q._z;
			B_FLOAT x = _w*q._x + _x*q._w + _y*q._z - _z*q._y;
			B_FLOAT y = _w*q._y + _y*q._w + _z*q._x - _x*q._z;
			B_FLOAT z = _w*q._z + _z*q._w + _x*q._y - _y*q._x;

			return BQuaternion(x,y,z,w);
		}

		BQuaternion BQuaternion::operator *(B_FLOAT v)
		{
			return BQuaternion(_x * v,_y * v ,_z * v,_w * v);
		}

		BQuaternion BQuaternion::operator /(B_FLOAT v)
		{
			return BQuaternion(_x / v,_y / v ,_z / v,_w / v);
		}

		bool BQuaternion::operator == ( BQuaternion q)
		{
			if(_x == q._x)
				if( _y == q._y)
					if(_z == q._z)
						if(_w == q._w)
							return true;
			return false;
		}

		bool BQuaternion::operator != ( BQuaternion q)
		{
			if(_x == q._x)
				if( _y == q._y)
					if(_z == q._z)
						if(_w == q._w)
							return false;
			return true;
		}

		B_FLOAT BQuatLengthSq(BQuaternion* src)
		{
			B_FLOAT v = src->_x*src->_x + src->_y*src->_y + src->_z*src->_z + src->_w*src->_w;

			return v;
		}

		BQuaternion* BQuatNormalize(BQuaternion* dest,BQuaternion* src)
		{
			B_FLOAT s = sqrt(BQuatLengthSq(src));

			*dest = *src /s;

			return dest;
		}

		BQuaternion*    BQuatSlerp(BQuaternion* dest,BQuaternion* src1, BQuaternion* src2, B_FLOAT s,bool line)
		{
			if(line)
			{
				dest->_x = (1.0f-s)*src1->_x + s*src2->_x;
				dest->_y = (1.0f-s)*src1->_y + s*src2->_y;
				dest->_z = (1.0f-s)*src1->_z + s*src2->_z;
				dest->_w = (1.0f-s)*src1->_w + s*src2->_w;
				return dest;
			}

			B_FLOAT tol[4];
			B_FLOAT omega, cosom, sinom, scale0, scale1;

			cosom = (src1->_x * src2->_x) + (src1->_y * src2->_y) + (src1->_z + src2->_z) * (src1->_w * src2->_w);

			if(cosom<0.0)
			{
				cosom = -cosom;
				tol[0] = -src2->_x;
				tol[1] = -src2->_y;
				tol[2] = -src2->_z;
				tol[3] = -src2->_w;
			}
			else
			{
				tol[0] = -src2->_x;
				tol[1] = -src2->_y;
				tol[2] = -src2->_z;
				tol[3] = -src2->_w;
			}

			if((1.0-cosom)>0.01f)
			{
				omega = acos(cosom);
				sinom = sin(omega);
				scale0 = sin((1.0f - s) * omega) / sinom;
				scale1 = sin(s * omega) / sinom;
			}
			else
			{
				scale0 = 1.0f - s;
				scale1 = s;
			}
			dest->_x = scale0 * src1->_x + scale1 * tol[0];
			dest->_y = scale0 * src1->_y + scale1 * tol[1];
			dest->_z = scale0 * src1->_z + scale1 * tol[2];
			dest->_w = scale0 * src1->_w + scale1 * tol[3];

			return dest;
		}

		BQuaternion*    BQuatRotationAxis(BQuaternion* dest,BVector3* pv,B_FLOAT angle)
		{
			BVector3 c;
			BVec3Normalize(&c,pv);
			BVector3 t = c * sin(angle*0.5f);

			dest->_x = t._x;
			dest->_y = t._y;
			dest->_z = t._z;
			dest->_w = cos(angle*0.5f);

			return dest;
		}

		void MatToQuat(BQuaternion* dest,BMatrix* src)
		{
			B_FLOAT s;
			B_FLOAT tq[4];
			int    i, j;

			// Use tq to store the largest trace
			tq[0] = 1.0f + src->bm[0]+src->bm[5]+src->bm[10];
			tq[1] = 1.0f + src->bm[0]-src->bm[5]-src->bm[10];
			tq[2] = 1.0f - src->bm[0]+src->bm[5]-src->bm[10];
			tq[3] = 1.0f - src->bm[0]-src->bm[5]+src->bm[10];

			// Find the maximum (could also use stacked if's later)
			j = 0;
			for(i=1;i<4;i++)
				j = (tq[i]>tq[j])? i : j;

			// check the diagonal
			if (j==0)
			{
				/* perform instant calculation */
				dest->_w = tq[0];
				dest->_x = src->bm[6]-src->bm[9];
				dest->_y = src->bm[8]-src->bm[2];
				dest->_z = src->bm[1]-src->bm[4];
			}
			else if (j==1)
			{
				dest->_w = src->bm[6]-src->bm[9];
				dest->_x = tq[1];
				dest->_y = src->bm[1]+src->bm[4];
				dest->_z = src->bm[8]+src->bm[2];
			}
			else if (j==2)
			{
				dest->_w = src->bm[8]-src->bm[2];
				dest->_x = src->bm[1]+src->bm[4];
				dest->_y = tq[2];
				dest->_z = src->bm[6]+src->bm[9];
			}
			else /* if (j==3) */
			{
				dest->_w = src->bm[1]-src->bm[4];
				dest->_x = src->bm[8]+src->bm[2];
				dest->_y = src->bm[6]+src->bm[9];
				dest->_z = tq[3];
			}

			s = BSqrt(0.25f / tq[j]);

			*dest *=s;


		}

		void QuatToMat(BMatrix* dest,BQuaternion* src)
		{
			B_FLOAT wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

			 //计算相关的系数
			 x2 = src->_x + src->_x;
			 y2 = src->_y + src->_y;
			 z2 = src->_z + src->_z;

			 xx = src->_x * x2;
			 xy = src->_x * y2;
			 xz = src->_x * z2;
			 yy = src->_y * y2;
			 yz = src->_y * z2;
			 zz = src->_z * z2;
			 wx = src->_w * x2;
			 wy = src->_w * y2;
			 wz = src->_w * z2;

			 //将其填入矩阵位置
			 dest->bm[0] = 1.000000f - (yy +zz);
			 dest->bm[4] = xy - wz;
			 dest->bm[8] = xz + wy;
			 dest->bm[12] = 0.0f;
			 dest->bm[1] = xy + wz;
			 dest->bm[5] = 1.000000f-(xx+zz);
			 dest->bm[9] = yz - wx;
			 dest->bm[13] = 0.0f;
			 dest->bm[2] = xz - wy;
			 dest->bm[6] = yz + wx;
			 dest->bm[10] = 1.000000f - (xx + yy);
			 dest->bm[14] = 0.0f;
			 dest->bm[3] = 0.0f;
			 dest->bm[7] = 0.0f;
			 dest->bm[11] = 0.0f;
			 dest->bm[15] = 1.000000f;
		}
		

BPhy.cpp源文件
定义几何图形碰撞、相交的基础物理算法。
		#include "BMath2.h"
		bool BRay2Triangle(BVector3 pStart,BVector3 pEnd,BVector3 p1,BVector3 p2,BVector3 p3,B_FLOAT* len)
		{
			BVector3 e1 = p2 - p1;
			BVector3 e2 = p3 - p1;

			BVector3 pvec;
			BVector3 pDir = pEnd - pStart;
			BVec3Cross(&pvec,&pDir,&e2);

			B_FLOAT det = BVec3Dot(&e1,&pvec);

			BVector3 tvec,qvec;

			if(det>B_DELAY)
			{
				tvec  = pStart - p1;
			}
			else
			{
				tvec = p1 - pStart;
				det = -det;
			}

			if(det<B_DELAY)
				return false;

			B_FLOAT tu,tv;

			tu = BVec3Dot(&tvec,&pvec);
			if(tu<B_DELAY || tu>det)
				return false;

			BVec3Cross(&qvec,&tvec,&e1);
			tv = BVec3Dot(&pDir,&qvec);
			if(tv<B_DELAY || (tu+tv)>det)
				return false;

			if(len!=NULL)
			{
				*len = BVec3Dot(&e2,&qvec);
				*len *= 1.000000f/det;
			}
			return true;
		}

		bool BRay2Quad(BVector3 pStart,BVector3 pEnd,BVector3 p1,BVector3 p2,BVector3 p3,BVector3 p4,B_FLOAT* len)
		{		
			if(BRay2Triangle(pStart,pEnd,p1,p2,p4,len))
				return true;

			if(BRay2Triangle(pStart,pEnd,p4,p2,p3,len))
				return true;

			return false;
		}

		bool BRay2Box(BVector3 pStart,BVector3 pEnd,BVector3 MinVector,BVector3 MaxVector,B_FLOAT* len,int* style)
		{
			BVector3 p1,p2,p3,p4,p5,p6,p7,p8;

			p1 = BVector3(MinVector._x,MaxVector._y,MinVector._z);
			p2 = BVector3(MaxVector._x,MaxVector._y,MinVector._z);
			p3 = BVector3(MinVector._x,MaxVector._y,MaxVector._z);
			p4 = BVector3(MaxVector._x,MaxVector._y,MaxVector._z);

			p5 = BVector3(MinVector._x,MinVector._y,MinVector._z);
			p6 = BVector3(MaxVector._x,MinVector._y,MinVector._z);
			p7 = BVector3(MinVector._x,MinVector._y,MaxVector._z);
			p8 = BVector3(MaxVector._x,MinVector._y,MaxVector._z);


			//top
			if(BRay2Quad(pStart,pEnd,p1,p2,p3,p4,len))
			{
				if(style!=NULL)
					*style = 0;
				return true;
			}

			//bottom
			if(BRay2Quad(pStart,pEnd,p5,p6,p7,p8,len))
			{
				if(style!=NULL)
					*style = 1;
				return true;
			}

			//front
			if(BRay2Quad(pStart,pEnd,p3,p4,p7,p8,len))
			{
				if(style!=NULL)
					*style = 2;
				return true;
			}

			//back
			if(BRay2Quad(pStart,pEnd,p1,p2,p5,p6,len))
			{
				if(style!=NULL)
					*style = 3;
				return true;
			}


			//left
			if(BRay2Quad(pStart,pEnd,p1,p3,p5,p7,len))
			{
				if(style!=NULL)
					*style = 4;
				return true;
			}

			//right
			if(BRay2Quad(pStart,pEnd,p4,p2,p8,p6,len))
			{
				if(style!=NULL)
					*style = 5;
				return true;
			}

			return false;
		}


		int    CrashQuad2Quad(BVector2 v1,BVector2 v2,BVector2 m1,BVector2 m2)
		{
			B_FLOAT lx = abs(v2._x-v1._x)/2 + abs(m2._x - m1._x)/2;//两中心点间理论距离
			B_FLOAT ly = abs(v2._y-v1._y)/2 + abs(m2._y - m1._y)/2;

			B_FLOAT cx = abs((v2._x+v1._x)/2 - (m2._x+m1._x)/2);
			B_FLOAT cy = abs((v2._y+v1._y)/2 - (m2._y+m1._y)/2);

			if((lx-cx)>B_DELAY && (ly-cy)>B_DELAY)
			{
				return CRASH_IN;

			}

			if(( (lx-cx)>B_DELAY && ly==cy) || (lx==cx && (ly-cy)>B_DELAY))
				return CRASH_TOUCH;

			return CRASH_OUT;
		}


		int CrashBox2Box(BVector3 Min1,BVector3 Max1,BVector3 Min2,BVector3 Max2)
		{
			//two center point real length
			BVector3 len = (Min1+Max1)*0.5f - (Min2+Max2)*0.5f;
			len._x = fabs(len._x);
			len._y = fabs(len._y);
			len._z = fabs(len._z);

			int isCrash = 0;
			int isTouch = 0;

			//two center poing crash length
			B_FLOAT cx = (Max1._x - Min1._x)/2 + (Max2._x - Min2._x)/2 ;
			B_FLOAT cy = (Max1._y - Min1._y)/2 + (Max2._y - Min2._y)/2 ;
			B_FLOAT cz = (Max1._z - Min1._z)/2 + (Max2._z - Min2._z)/2 ;

			if(len._x < cx)
				isCrash++;

			if(len._y < cy)
				isCrash++;

			if(len._z < cz)
				isCrash++;

			if(len._x==cx)
				isTouch++;

			if(len._y==cy)
				isTouch++;

			if(len._z==cz)
				isTouch++;

			if(isCrash>2)
			{
				return CRASH_IN;
			}
			else if( isCrash==2 && isTouch==1)
			{
				return CRASH_TOUCH;
			}

			return CRASH_OUT;

		}


		bool    CrashTriangle(BVector3 p1,BVector3 p2,BVector3 p3,BVector3 b1,BVector3 b2,BVector3 b3)//三角形碰撞
		{
			B_FLOAT len=0;

			if(BRay2Triangle((p1+p2)*0.5f,p3,b1,b2,b3,&len) == true)
			{
				if(len > B_DELAY)
				{
					if(BRay2Triangle(p3,(p1+p2)*0.5f,b1,b2,b3,&len) == true)
					{
						if(len > B_DELAY)
							return true;
					}
				}
			}

			if(BRay2Triangle((p1+p3)*0.5f,p2,b1,b2,b3,&len) == true)
			{
				if(len > B_DELAY)
				{
					if(BRay2Triangle(p2,(p1+p3)*0.5f,b1,b2,b3,&len) == true)
					{
						if(len > B_DELAY)
							return true;
					}
				}
			}

			if(BRay2Triangle((p3+p2)*0.5f,p1,b1,b2,b3,&len) == true)
			{
				if(len > B_DELAY)
				{
					if(BRay2Triangle(p1,(p3+p2)*0.5f,b1,b2,b3,&len) == true)
					{
						if(len > B_DELAY)
							return true;
					}
				}
			}

			//-----------------------------------------------------------


			if(BRay2Triangle(p1,p2,b1,b2,b3,&len) == true)
			{
				if(len > B_DELAY)
				{
					if(BRay2Triangle(p2,p1,b1,b2,b3,&len) == true)
					{
						if(len > B_DELAY)
							return true;
					}
				}
			}

			if(BRay2Triangle(p2,p3,b1,b2,b3,&len) == true)
			{
				if(len > B_DELAY)
				{
					if(BRay2Triangle(p3,p2,b1,b2,b3,&len) == true)
					{
						if(len > B_DELAY)
							return true;
					}
				}
			}

			if(BRay2Triangle(p1,p3,b1,b2,b3,&len) == true)
			{
				if(len > B_DELAY)
				{
					if(BRay2Triangle(p3,p1,b1,b2,b3,&len) == true)
					{
						if(len > B_DELAY)
							return true;
					}
				}
			}
		//----------------------
			if(BRay2Triangle(b1,b2,p1,p2,p3,&len) == true)
			{
				if(len > B_DELAY)
				{
					if(BRay2Triangle(b2,b1,p1,p2,p3,&len) == true)
					{
						if(len > B_DELAY)
							return true;
					}
				}
			}

			if(BRay2Triangle(b2,b3,p1,p2,p3,&len) == true)
			{
				if(len > B_DELAY)
				{
					if(BRay2Triangle(b3,b2,p1,p2,p3,&len) == true)
					{
						if(len > B_DELAY)
							return true;
					}
				}
			}

			if(BRay2Triangle(b1,b3,p1,p2,p3,&len) == true)
			{
				if(len > B_DELAY)
				{
					if(BRay2Triangle(b3,b1,p1,p2,p3,&len) == true)
					{
						if(len > B_DELAY)
							return true;
					}
				}
			}

			//------------------------------------

			if(BRay2Triangle((b1+b2)*0.5f,b3,p1,p2,p3,&len) == true)
			{
				if(len > B_DELAY)
				{
					if(BRay2Triangle(b3,(b1+b2)*0.5f,p1,p2,p3,&len) == true)
					{
						if(len > B_DELAY)
							return true;
					}
				}
			}

			if(BRay2Triangle((b1+b3)*0.5f,b2,p1,p2,p3,&len) == true)
			{
				if(len > B_DELAY)
				{
					if(BRay2Triangle(b2,(b1+b3)*0.5f,p1,p2,p3,&len) == true)
					{
						if(len > B_DELAY)
							return true;
					}
				}
			}

			if(BRay2Triangle((b3+b2)*0.5f,b1,p1,p2,p3,&len) == true)
			{
				if(len > B_DELAY)
				{
					if(BRay2Triangle(b1,(b3+b2)*0.5f,p1,p2,p3,&len) == true)
					{
						if(len > B_DELAY)
							return true;
					}
				}
			}

			return false;
		}

		BVector3 GetRayPos(BVector3 pStart,BVector3 pEnd,B_FLOAT v)
		{
		/*
			BVector3 tt;

			tt = pStart+(pEnd-pStart)*v;
			return tt;
		*/
		//2013-01-16
			BVector3 p = pEnd-pStart;
			BVector3 pNormal;
			BVec3Normalize(&pNormal,&p);

			if(pNormal._y == v)
			{
				return pStart;
			}

			float n = (v-pStart._y)/pNormal._y;

			return  (pStart+pNormal*n);
		}
		
文章导航