我的3D引擎数学库头文件

#ifndef MYMATH_H_
#define MYMATH_H_

#define MIN(a, b) ((a) < (b) ? (a) : (b))
#define SWAP(a, b, t) {(t) = (a) ; (a) = (b) ; (b) = (t) ;}

float sinLookUpTable[361] = {
	0, 0, 0} ;

float cosLookUpTable[361] = {
	0, 0, 0} ;

#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 PARM_LINE_NO_INTERSECT          0
#define PARM_LINE_INTERSECT_IN_SEGMENT  1
#define PARM_LINE_INTERSECT_OUT_SEGMENT 2
#define PARM_LINE_INTERSECT_EVERYWHERE  3

int Fast_Distance_2D(int x, int y);
float Fast_Distance_3D(float x, float y, float z);

//		2D极坐标
typedef struct POLAR2D_TYP
{
float r;			//	半径
float theta;		//	同X轴逆时针夹角
} POLAR2D, *POLAR2D_PTR;

//		2D向量
typedef struct VECTOR2D_TYP
{
union
    {
    float M[2];

    struct
         {
         float x,y;
         };
    };
} VECTOR2D, POINT2D, *VECTOR2D_PTR, *POINT2D_PTR;

//		3D向量
typedef struct VECTOR3D_TYP
{
union
    {
    float M[3] ;

    struct
         {
         float x ;
		 float y ;
		 float z ;
         };
    } ;
} VECTOR3D, POINT3D, *VECTOR3D_PTR, *POINT3D_PTR;

// 4D向量
typedef struct VECTOR4D_TYP
{
union
    {
    float M[4];

    struct
         {
         float x,y,z,w;
         };
    };
} VECTOR4D, POINT4D, *VECTOR4D_PTR, *POINT4D_PTR;

//	3D柱面坐标
typedef struct CYLINDRICAL3D_TYP
{
	float r ;			//		半径
	float theta ;		//		与X轴的逆时针夹角
	float z ;			//		Z坐标
} CYLINDRICAL3D, *CYLINDRICAL3D_PTR ;

// 3D球面坐标
typedef struct SPHERICAL3D_TYP
{
float p;			//	距原点的距离
float theta;		// OP与Z轴正向夹角
float phi;			//	OP在XOY平面上的投影与X轴正向夹角
} SPHERICAL3D, *SPHERICAL3D_PTR;

// 1x2矩阵
typedef struct MATRIX1X2_TYP
{
	union
	{
        float M[2];

        struct
		{
			float M00, M01;
		};
	} ;
}MATRIX1X2, *MATRIX1X2_PTR;

// 1x3矩阵
typedef struct MATRIX1X3_TYP
{
	union
	{
		float M[3];

        struct
		{
			float M00, M01, M02;
		} ;
	} ;
}MATRIX1X3, *MATRIX1X3_PTR;

//	1x4矩阵
typedef struct MATRIX1X4_TYP
{
	union
	{
		float M[4];

		struct
		{
			float M00, M01, M02, M03;
		};
	};
} MATRIX1X4, *MATRIX1X4_PTR;

// 2x2矩阵
typedef struct MATRIX2X2_TYP
{
union
    {
    float M[2][2];

    struct
         {
         float M00, M01;
         float M10, M11;
         };

     };
} MATRIX2X2, *MATRIX2X2_PTR;

// 3x2矩阵
typedef struct MATRIX3X2_TYP
{
	union
	{
		float M[3][2];

		struct
		{
			float M00, M01;
			float M10, M11;
			float M20, M21;
		};
	} ;
} MATRIX3X2, *MATRIX3X2_PTR;

//		3X3矩阵
typedef struct MATRIX3X3_TYP
{
	union
	{
		float M[3][3];

        struct
		{
			float M00, M01, M02;
			float M10, M11, M12;
			float M20, M21, M22;
		};
	};
} MATRIX3X3, *MATRIX3X3_PTR;

//		4x3矩阵
typedef struct MATRIX4X3_TYP
{
	union
    {
		float M[4][3];
		
		struct
		{
			 float M00, M01, M02;
			 float M10, M11, M12;
			 float M20, M21, M22;
			 float M30, M31, M32;
		};

    };
}MATRIX4X3, *MATRIX4X3_PTR;

// 4x4矩阵
typedef struct MATRIX4X4_TYP
{
	union
	{
		float M[4][4];

		struct
		{
			float M00, M01, M02, M03;
			float M10, M11, M12, M13;
			float M20, M21, M22, M23;
			float M30, M31, M32, M33;
		};
	};
} MATRIX4X4, *MATRIX4X4_PTR;

#define MAT_COPY_2X2(src_mat, dest_mat) {memcpy((void *)(dest_mat), (void *)(src_mat), sizeof(MATRIX2X2) ); }
#define MAT_COPY_3X3(src_mat, dest_mat) {memcpy((void *)(dest_mat), (void *)(src_mat), sizeof(MATRIX3X3) ); }
#define MAT_COPY_4X4(src_mat, dest_mat) {memcpy((void *)(dest_mat), (void *)(src_mat), sizeof(MATRIX4X4) ); }
#define MAT_COPY_4X3(src_mat, dest_mat) {memcpy((void *)(dest_mat), (void *)(src_mat), sizeof(MATRIX4X3) ); }

inline void MAT_COLUMN_SWAP_2X2(MATRIX2X2_PTR m, int c, MATRIX1X2_PTR v) 
{ m->M[0][c]=v->M[0]; m->M[1][c]=v->M[1]; } 

inline void MAT_COLUMN_SWAP_3X3(MATRIX3X3_PTR m, int c, MATRIX1X3_PTR v) 
{ m->M[0][c]=v->M[0]; m->M[1][c]=v->M[1]; m->M[2][c]=v->M[2]; } 

inline void MAT_COLUMN_SWAP_4X4(MATRIX4X4_PTR m, int c, MATRIX1X4_PTR v) 
{m->M[0][c]=v->M[0]; m->M[1][c]=v->M[1]; m->M[2][c]=v->M[2]; m->M[3][c]=v->M[3]; } 

inline void MAT_COLUMN_SWAP_4X3(MATRIX4X3_PTR m, int c, MATRIX1X4_PTR v) 
{m->M[0][c]=v->M[0]; m->M[1][c]=v->M[1]; m->M[2][c]=v->M[2]; m->M[3][c]=v->M[3]; } 

float Fast_Sin(float theta) ;
float Fast_Cos(float theta) ;

//		极坐标转换到2D坐标
void POLAR2D_To_POINT2D(POLAR2D_PTR polar, POINT2D_PTR rect) ;

//		2D坐标准换到极坐标
void POINT2D_To_POLAR2D(POINT2D_PTR rect , POLAR2D_PTR polar) ;

//		2D极坐标转换为 r 和 theta
void POINT2D_To_PolarRTh(POINT2D_PTR rect, float * pR, float * pTheta) ;

//		3D柱面坐标点转换为3D直角坐标
void CYLINDRICAL3D_To_POINT3D(CYLINDRICAL3D_PTR cyl, POINT3D_PTR rect) ;

//		3D柱面坐标点转换为 x y z 坐标
void CYLINDRICAL3D_To_POINT3D(CYLINDRICAL3D_PTR cyl, float *pX, float *pY, float *pZ) ;

//		3D直角坐标转换为3D柱面坐标
void POINT3D_To_CYLINDRICAL3D(POINT3D_PTR rect, CYLINDRICAL3D_PTR cyl) ;

//		3D球面坐标转换为3D直角坐标
void SPHERICAL3D_To_POINT3D(SPHERICAL3D_PTR sph, POINT3D_PTR rect) ;

//		3D球面坐标转换为 x y z 坐标
void SPHERICAL3D_To_RectXYZ(SPHERICAL3D_PTR sph, float * pX, float * pY, float * pZ) ;

//		3D点转换为球面坐标点
void POINT3D_To_SPHERICAL3D(POINT3D_PTR rect, SPHERICAL3D_PTR sph) ;

//		3D点转换为球面坐标
void POINT3D_To_SphericalPThPh(POINT3D_PTR rect, float * p, float * theta, float * phi) ;


//		2D向量初始化
inline void VECTOR2D_INIT(VECTOR2D_PTR vdst, VECTOR2D_PTR vsrc)
{
	vdst ->x = vsrc ->x ;
	vdst ->y = vsrc ->y ;
}

//		2D向量加法

void VECTOR2D_Add(VECTOR2D_PTR va, VECTOR2D_PTR vb, VECTOR2D_PTR vsum) ;

VECTOR2D VECTOR2D_Add(VECTOR2D_PTR va, VECTOR2D_PTR vb) ;

//		2D向量减法
void VECTOR2D_Sub(VECTOR2D_PTR va, VECTOR2D_PTR vb, VECTOR2D_PTR vsum) ;

VECTOR2D VECTOR2D_Sub(VECTOR2D_PTR va, VECTOR2D_PTR vb) ;

//		2D向量初始化
inline void VECTOR3D_INIT(VECTOR3D_PTR vdst, VECTOR3D_PTR vsrc)
{
	vdst ->x = vsrc ->x ;
	vdst ->y = vsrc ->y ;
	vdst ->z = vsrc ->z ;
}

//		3D向量加法
void VECTOR3D_Add(VECTOR3D_PTR va, VECTOR3D_PTR vb, VECTOR3D_PTR vsum) ;

VECTOR3D VECTOR3D_Add(VECTOR3D_PTR va, VECTOR3D_PTR vb) ;

//		3D向量减法
void VECTOR3D_Sub(VECTOR3D_PTR va, VECTOR3D_PTR vb, VECTOR3D_PTR vdiff) ;

VECTOR3D VECTOR3D_Sub(VECTOR3D_PTR va, VECTOR3D_PTR vb) ;

//		4D向量加法
void VECTOR4D_Add(VECTOR4D_PTR va, VECTOR4D_PTR vb, VECTOR4D_PTR vsum) ;

VECTOR4D VECTOR4D_Add(VECTOR4D_PTR va, VECTOR4D_PTR vb) ;

//		4D向量减法
void VECTOR4D_Sub(VECTOR4D_PTR va, VECTOR4D_PTR vb, VECTOR4D_PTR vdiff) ;

VECTOR4D VECTOR4D_Sub(VECTOR4D_PTR va, VECTOR4D_PTR vb) ;

//		2D向量缩放
void VECTOR2D_Scale(float k, VECTOR2D_PTR va, VECTOR2D_PTR vscaled) ;
void VECTOR2D_Scale(float k, VECTOR2D_PTR va) ;

//		3D向量缩放
void VECTOR3D_Scale(float k, VECTOR3D_PTR va, VECTOR3D_PTR vscaled) ;
void VECTOR3D_Scale(float k, VECTOR3D_PTR va) ;

//		4D向量缩放
void VECTOR4D_Scale(float k, VECTOR4D_PTR va, VECTOR4D_PTR vscaled) ;
void VECTOR4D_Scale(float k, VECTOR4D_PTR va) ;

//		向量点乘
float VECTOR2D_Dot(VECTOR2D_PTR va, VECTOR2D_PTR vb) ;

float VECTOR3D_Dot(VECTOR3D_PTR va, VECTOR3D_PTR vb) ;

float VECTOR4D_Dot(VECTOR4D_PTR va, VECTOR4D_PTR vb) ;

//		向量叉乘
void VECTOR3D_Cross(VECTOR3D_PTR va, VECTOR3D_PTR vb, VECTOR3D_PTR vn) ;

VECTOR3D VECTOR3D_Cross(VECTOR3D_PTR va, VECTOR3D_PTR vb) ;

void VECTOR4D_Cross(VECTOR4D_PTR va, VECTOR4D_PTR vb, VECTOR4D_PTR vn) ;

VECTOR4D VECTOR4D_Cross(VECTOR4D_PTR va, VECTOR4D_PTR vb) ;

//		计算向量的模
float VECTOR2D_Length(VECTOR2D_PTR va) ;
float VECTOR3D_Length(VECTOR3D_PTR va) ;
float VECTOR4D_Length(VECTOR4D_PTR va) ;

float VECTOR2D_Length_Fast(VECTOR2D_PTR va) ;
float VECTOR3D_Length_Fast(VECTOR3D_PTR va) ;
float VECTOR4D_Length_Fast(VECTOR4D_PTR va) ;

//		标准化向量
void VECTOR2D_Normalize(VECTOR2D_PTR va) ;
void VECTOR3D_Normalize(VECTOR3D_PTR va) ;
void VECTOR4D_Normalize(VECTOR4D_PTR va) ;

void VECTOR2D_Normalize(VECTOR2D_PTR va, VECTOR2D_PTR vn) ;
void VECTOR3D_Normalize(VECTOR3D_PTR va, VECTOR3D_PTR vn) ;
void VECTOR4D_Normalize(VECTOR4D_PTR va, VECTOR4D_PTR vn) ;

//		构建一条从 init 到 term 的向量
void VECTOR2D_Build(VECTOR2D_PTR init, VECTOR2D_PTR term, VECTOR2D_PTR result) ;
void VECTOR3D_Build(VECTOR3D_PTR init, VECTOR3D_PTR term, VECTOR3D_PTR result) ;
void VECTOR4D_Build(VECTOR4D_PTR init, VECTOR4D_PTR term, VECTOR4D_PTR result) ;

//		计算两向量的夹角
float VECTOR2D_CosTh(VECTOR2D_PTR va, VECTOR2D_PTR vb) ;
float VECTOR3D_CosTh(VECTOR3D_PTR va, VECTOR3D_PTR vb) ;
float VECTOR4D_CosTh(VECTOR4D_PTR va, VECTOR4D_PTR vb) ;

//		2X2矩阵运算
//		初始化
void Mat_Init_2X2(MATRIX2X2_PTR ma, float m00, float m01, float m10, float m11) ;
//		加法
void Mat_Add_2X2(MATRIX2X2_PTR ma, MATRIX2X2_PTR mb, MATRIX2X2_PTR msum) ;
//		乘法
void Mat_Mul_2X2(MATRIX2X2_PTR ma, MATRIX2X2_PTR mb, MATRIX2X2_PTR mprod) ;
//		乘法
void Mat_Mul_1X2_3X2(MATRIX1X2_PTR ma, MATRIX3X2_PTR mb, MATRIX1X2_PTR mprod) ;
//		计算行列式
float Mat_Det_2X2(MATRIX2X2_PTR m) ;
//		计算逆矩阵
bool Mat_Inverse_2X2(MATRIX2X2_PTR m, MATRIX2X2_PTR mi) ;

//		3X3矩阵运算
//		初始化
void Mat_Init_3X3(MATRIX3X3_PTR ma,
	float m00, float m01, float m02,
	float m10, float m11, float m12,
	float m20, float m21, float m22) ;
//		加法
void Mat_Add_3X3(MATRIX3X3_PTR ma, MATRIX3X3_PTR mb, MATRIX3X3_PTR msum) ;
//		乘法
void Mat_Mul_3X3_3X3(MATRIX3X3_PTR ma, MATRIX3X3_PTR mb, MATRIX3X3_PTR mprod) ;
//		计算行列式
float Mat_Det_3X3(MATRIX3X3_PTR m) ;
//		计算逆矩阵
bool Mat_Inverse_3X3(MATRIX3X3_PTR m, MATRIX3X3_PTR mi) ;
//		3D向量乘3X3矩阵
void Mat_Mul_VECTOT3D_3X3(VECTOR3D_PTR va, MATRIX3X3_PTR mb, VECTOR3D_PTR vprod) ;
//		1X3矩阵乘3X3矩阵
void Mat_Mul_1X3_3X3(MATRIX1X3_PTR ma, MATRIX3X3_PTR mb, MATRIX1X3_PTR mprod) ;


//		4X4矩阵运算
//		初始化
void Mat_Init_4X4(MATRIX4X4_PTR ma,
	float m00, float m01, float m02, float m03,
	float m10, float m11, float m12, float m13,
	float m20, float m21, float m22, float m23,
	float m30, float m31, float m32, float m33) ;
//		加法
void Mat_Add_4X4(MATRIX4X4_PTR ma, MATRIX4X4_PTR mb, MATRIX4X4_PTR mprod) ;
//		乘法
void Mat_Mul_4X4(MATRIX4X4_PTR ma, MATRIX4X4_PTR mb, MATRIX4X4_PTR mprod) ;
//		计算逆矩阵
bool Mat_Inverse_4X4(MATRIX4X4_PTR m, MATRIX4X4_PTR mi) ;

//		3D向量乘4X4矩阵
void Mat_Mul_VECTOR3D_4X4(VECTOR3D_PTR va, MATRIX4X4_PTR mb, VECTOR3D_PTR vprod) ;
//		3D向量乘4X3矩阵
void Mat_Mul_VECTOR3D_4X3(VECTOR3D_PTR va, MATRIX4X3_PTR mb, VECTOR3D_PTR vprod) ;
//		4D向量乘4X4矩阵
void Mat_Mul_VECTOR4D_4X4(VECTOR4D_PTR va, MATRIX4X4_PTR mb, VECTOR4D_PTR vprod) ;
//		4D向量乘4X4矩阵
void Mat_Mul_VECTOR4D_4X3(VECTOR4D_PTR va, MATRIX4X3_PTR mb, VECTOR4D_PTR vprod) ;
//		1X4矩阵乘4X4矩阵
void Mat_Mul_1X4_4X4(MATRIX1X4_PTR ma, MATRIX4X4_PTR mb, MATRIX1X4_PTR mprod) ;

typedef struct PARMLINE2D_TYP
{
	POINT2D p0 ;	//	起点
	POINT2D p1 ;	//	终点
	VECTOR2D v ;	//	方向向量
} PARMLINE2D, *PARMLINE2D_PTR ;

typedef struct PARMLINE3D_TYP
{
	POINT3D p0 ;	//	起点
	POINT3D p1 ;	//	终点
	VECTOR3D V ;	//	方向向量
} PARMLINE3D, *PARMLINE3D_PTR ;

//	初始化参数化2D直线
void Init_Parm_Line2D(POINT2D_PTR p_init, POINT2D_PTR p_term, PARMLINE2D_PTR p) ;
//	计算指定t值在2D直线上的位置
void Compute_Parm_Line2D(PARMLINE2D_PTR p, float t, POINT2D_PTR pt) ;
//	判断两条参数化2D线段相互位置关系,并求出各自相交的t值
int Intersect_Parm_Line2D(PARMLINE2D_PTR p1, PARMLINE2D_PTR p2, float * t1, float * t2) ;
//	判断两条参数化2D线段相互位置关系,并求出相交处的坐标
int Intersect_Parm_Lines2D(PARMLINE2D_PTR p1, PARMLINE2D_PTR p2, POINT2D_PTR pt) ;


//	初始化参数化3D直线
void Init_Parm_Line3D(POINT3D_PTR p_init, POINT3D_PTR p_term, PARMLINE3D_PTR p) ;
//	计算指定t值在3D直线上的位置
void Compute_Parm_Line3D(PARMLINE3D_PTR p, float t, POINT3D_PTR pt) ;

typedef struct PLANE3D_TYP
{
	POINT3D p0 ;	//	平面上的一个点
	VECTOR3D n ;	//	平面的法线(不必是单位向量)
} PLANE3D, *PLANE3D_PTR ;

//	初始化3D点
void POINT3D_COPY(POINT3D_PTR vdst, POINT3D_PTR vsrc) ;
//	初始化3D向量
void VECTOR3D_COPY(VECTOR3D_PTR vdst, VECTOR3D_PTR vsrc) ;
//	初始化参数化平面
void PLANE3D_Init(PLANE3D_PTR plane, POINT3D_PTR p0, VECTOR3D_PTR normal, bool normalize) ;
//	检测点是在平面上、正半空间还是负半空间中
float Compute_Point_In_Plane3D(POINT3D_PTR pt, PLANE3D_PTR plane) ;
//	计算参数化直线与平面的交点
int Intersect_Parm_Line3D_Plane3D(PARMLINE3D_PTR pline, PLANE3D_PTR plane, float *t, POINT3D_PTR pt) ;

typedef struct QUAT_TYP
{
	union
	{
		float M[4] ;

		struct
		{
			float q0 ;
			VECTOR3D qv ;
		} ;

		struct
		{
			float w, x, y, z ;
		} ;
	} ;
} QUAT, *QUAT_PTR ;

//	使用一个3D方向向量和一个角度来初始化一个四元数
void VECTOR3D_Theta_To_QUAT(QUAT_PTR q, VECTOR3D_PTR v, float theta) ;

//	使用一个4D方向向量和一个角度来初始化一个四元数
void VECTOR4D_Theta_To_QUAT(QUAT_PTR q, VECTOR4D_PTR v, float theta) ;

//	根据绕X Y Z旋转的角度,创建一个zyx顺序进行旋转对应的四元数
void EulerZYX_To_QUAT(QUAT_PTR q, float theta_z, float theta_y, float theta_x) ;

//	这个函数将一个单位四元数转换为一个单位方向向量和一个绕该向量旋转的四元数
void QUAT_To_VECTOR3D_Theta(QUAT_PTR q, VECTOR3D_PTR v, float * theta) ;

//	将两个四元数q1和q2相加
void QUAT_Add(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qsum) ;

//	将两个四元数q1和q2相减
void QUAT_Sub(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qdiff) ;

//	计算四元数q的共轭
void QUAT_Conjugate(QUAT_PTR q, QUAT_PTR qconj) ;

//	根据缩放因子scale对四元数qs进行缩放
void QUAT_Scale(QUAT_PTR q, float scale, QUAT_PTR qs) ;

//	根据缩放因子scale对四元数q进行缩放
void QUAT_Scale(QUAT_PTR q, float scale) ;

//	返回四元数q的范数
float QUAT_Norm(QUAT_PTR q) ;

//	返回四元数q范数的平方
float QUAT_Norm2(QUAT_PTR q) ;

//	将四元数q归一化
void QUAT_Normalize(QUAT_PTR q, QUAT_PTR qn) ;

//	将四元数q归一化
void QUAT_Normalize(QUAT_PTR q) ;

//	计算四元数q的逆,q必须是单位四元数
void QUAT_Unit_Inverse(QUAT_PTR q, QUAT_PTR qi) ;

//	计算四元数q的逆,q必须是单位四元数
void QUAT_Unit_Inverse(QUAT_PTR q) ;

//	将两个四元数相乘
void QUAT_Mul(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qprod) ;

//	将三个四元数相乘
void QUAT_Triple_Product(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR q3, QUAT_PTR qprod) ;

//	暂时不需要,就先注释掉 ^_^
//typedef int FIXP16 ;
//typedef int *FIXP16_PTR ;
//
//#define FIXP16_SHIFT 16
//#define FIXP16_MAG 65536
//#define FIXP16_DP_MASK 0X0000FFFF ;
//#define FIXP16_WP_MASK 0XFFFF0000 ;
//#define FIXP16_ROUND_UP 0X00008000 ;

bool Solve_2X2_System(MATRIX2X2_PTR A, MATRIX1X2_PTR X, MATRIX1X2_PTR B) ;

bool Solve_3X3_System(MATRIX3X3_PTR A, MATRIX1X3_PTR X, MATRIX1X3_PTR B) ;

#endif