转自:http://www.4oa.com/Article/html/6/31/446/2005/15505.html
/*分型图形库数学函数头文件*/
/*Math.Inc*/#define Ln10 2.30258509299405E+000 /*数学常数定义*/
#define Pi 3.1415927
#define PiOver180 1.74532925199433E-002
#define PiUnder180 5.72957795130823E+001
typedef enum{false,true}Boolean; /*数据存储格式定义*/
typedef unsigned char Byte;
typedef unsigned int Word;
int Round(float x) /*取近似值函数*/
{
return(int)(x+0.5));
}
int Trunc(float x) /*取整函数*/
{
return(int)(x));
}
float SqrFP(float x) /*求平方值函数*/
{
return(x*x);
}
int Sqr(int x) /*求平方根函数*/
{
return(X*x);
}
float adians(float Angle) /*弧度换算函数*/
{
return(Angle*PiOver180);
}
float Degress(float Angle) /*角度转换函数*/
{
return(Angle*PiUnder180);
}
float CosD(float Angle) /*求余弦函数*/
{
return(cos(Radians(Angle)));
}
float SinD(float Angle) /*求正弦函数*/
{
return(sin(Radians(Angle)));
}
float Power(float Base,int Exponent) /*求取以e为底的幂函数*/
{
float BPower;
int t;
if(Exponent==0)
return 1;
else{
BPower=1.0;
for(t=1;t<=Exponent;t++)
BPower*=Base;
return(BPower);
}
}
float Log(float x) /*求取标准 对数值函数*/
{
return(log(x)/Ln10);
}
float Exp10(float x) /*求取反对数值函数*/
{
return(exp(x*Ln10));
}
float Sign(float x) /*浮点型符号函数*/
{
if(x<0)
return -1;
else{
if(x>0)
return 1;
else return 0;
}
}
int IntSign(int x) /*整型符号函数*/
{
if(x<0)
return -1;
else{
if(x>0)
return 1;
else
return 0;
}
}
int IntSqrt(int x) /*求整方根函数*/
{
int OddInt,OldArg,FirstSqrt;
OddInt=1;
OldArg=x;
while(x>=0){
x=OddInt;
OddInt+=2;
}
FirstSqrt=OddInt>>1;
if(Sqrt(FirstSqrt)-FirstSqrt+1>OldArg)
return(FirstSqrt-1);
else
return(FirstSqrt);
}
int IntPower(int Base,int Exponent) /*求以e为底的整幂函数*/
{
if(Exponent==0)
return 1;
else
return(Base*IntPower(Base,Exponent-1));
}
/*与向量和矩阵相关的函数库*/
/*Vector and Matrix Routines*/
typedef float TDA[3]; /*常向量定义*/
typedef int TDIA[3];
typedef float FDA[4];
typedef float Matx4x4[4][4];
void vec(float r,float s,float t,TDA A) /*浮点数向量赋值函数*/
{
A[0]=r;
A[1]=s;
A[2]=t;
}
void VecInt(int r,int s,int t,TDIA A) /*整数向量赋值函数*/
{
A[0]=r;
A[1]=s;
A[2]=t;
}
void UnVec(TDA A,float *r,float *s,float *t) /*浮点数向量取值函数*/
{
*r=A[0];
*s=A[1];
*t=A[2];
}
void UnVecInt(TDIA A,float *r,float *s,float *t) /*整数向量取值函数*/
{
*r=A[0];
*s=A[1];
*t=A[2];
}
float VecDot(TDA A,TDA B) /*向量求积函数*/
{
return(A[0]*B[0]+A[1]*B[1]+A[2]*B[2]);
}
void VecCross(TDA A,TDA B,TDA c) /*向量叉乘函数*/
{
C[0]=A[1]*B[2]-A[2]*B[1];
C[1]=A[2]*B[0]-A[0]*B[2];
C[2]=A[0]*B[1]-A[1]*B[0];
}
float VecLen(TDA A) /*向量长度函数*/
{
return(sqrt(SqrFP(A[0])+SqrFP(A[1])+SqrFP(A[2])));
}
void VecNormalize(TDA A) /*向量初始化函数*/
{
float dist,invdist;
dist=VecLen(A);
if(!(dist==0.0)){
invdist=1/dist;
A[0]*=invdist;
A[1]*=invdist;
A[2]=invdist;
}
else{
puts("Zero_length Vectors Cannot be Normalized");
exit(1);
}
}
void VexMatxult(FDA A,Matx4x4 Matix,FDA B) /*向量乘矩阵函数*/
{
int mRow,mCol;
for(mCol=0;mCol<4;mCol++){
B[mCol]=0;
for(mRow=0;mRow<4;mRow++)
B[mCol]+=A[mRow]*Matrix[mRow][mCol];
}
}
void VecSub(TDA A,TDA B,TDA C) /*浮点数向量相减函数*/
{
C[0]=A[0]-B[0];
C[1]=A[1]-B[1];
C[2]=A[2]-B[2];
}
void VecSubInt(TDIA A,TDIA B,TDA C) /*整数向量相减函数*/
{
C[0]=A[0]-B[0];
C[1]=A[1]-B[1];
C[2]=A[2]-B[2];
}
void VecAdd(TDA A,TDA B,TDA C) /*向量求和函数*/
{
C[0]=A[0]+B[0];
C[1]=A[1]+B[1];
C[2]=A[2]+B[2];
}
void VecSubAdd3(TDA A,TDA B,TDA C,TDA D) /*三向理求和函数*/
{
D[0]=A[0]+B[0]+C[0];
D[1]=A[1]+B[1]+C[1];
D[2]=A[2]+B[2]+C[2];
}
void VecCopy(TDA A,TDA B) /*向量复制函数*/
{
int i;
for(i=0;i<3;i++)
B[i]=A[i];
}
void VecLinComb(float r,TDA A,float s,TDA B,TDA C) /*向量转换函数*/
{
C[0]=r*A[0]+s*B[0];
C[1]=r*A[1]+r*B[1];
C[2]=r*A[2]+r*B[2];
}
void VecScalMult(float r,TDA A,TDA B) /*向量与标量相乘函数*/
{
B[0]=r*A[0];
B[1]=r*A[1];
B[2]=r*A[2];
}
void VecScalMultI(float r,TDIA A,TDA B) /*向量与标量相乘函数2*/
{
B[0]=r*A[0];
B[1]=r*A[1];
B[2]=r*A[2];
}
void VCCScalMultInt(float r,TDA A,TDIA B) /*向量与标量相乘并取整函数*/
{
B[0]=Round(r*A[0]);
B[1]=Round(r*A[1]);
B[2]=Round(r*A[2]);
}
void VccAddScalMult(float r,TDA A,TDA B,TDA C) /*向量转换函数*/
{
C[0]=r*A[0]+B[0];
C[1]=r*A[1]+B[1];
C[2]=r*A[2]+B[2];
}
void VecNull(TDA A) /*零向量初始化函数*/
{
A[0]=0.0;
A[1]=0.0;
A[2]=0.0;
}
void VecElemMult(float r,TDA A,TDA B,TDA C) /*向量乘积倍度函数*/
{
C[0]=r*A[0]*B[0];
C[1]=r*A[1]*B[1];
C[2]=r*A[2]*B[2];
}
/*Affine Transformation Routines*/
void ZeroMatrix(Matx4x4 A) /*四维零方阵初始化*/
{
int i,j;
for(i=0;i<4;i++){
for(j=0;j<4;j++)
A[i][j]=0.0;
}
}
void Translate3D(float tx,float ty,float tz,Matx4x4 A) /*3D变换阵*/
{
int i;
ZeroMatrix(A);
for(i=0;i<4;i++){
A[i][i]=1.0;
A[0][3]=-tx;
A[1][3]=-ty;
A[2][3]=-tz;
}
}
void Scale3D(float sx,float sy,float sz,Matx4x4 A) /*3D标准阵*/
{
ZeroMatrix(A);
A[0][0]=sx;
A[1][1]=sy;
A[2][2]=sz;
A[3][3]=1.0;
}
void Rotate3D(int m,float Theta,Matx4x4 A) /*3D旋转阵*/
{
int m1,m2;
float c,s;
ZeroMatrix(A);
A[m-1][m-1]=1.0;
A[3][3]=1.0;
m1=(m%3)+1;
m2=(m1%3);
c=CosD(Theta);
s=SinD(Theta);
A[m1][m1]=c;
A[m1[m2]=s;
A[m2][m2]=c;
A[m2][m1]=s;
}
void Multiply3DMatrixies(Matx4x4 A,Matx4x4 B,Matx4x4 C) /*矩阵相乘变换阵*/
{
int i,j,k;
float ab;
for(i=0;i<4;i++){
for(j=0;j<4;j++){
ab=0;
for(k=0;k<4;k++)
ab+=A[i][k]*B[k][j];
C[i][j]=ab;
}
}
}
void MatCopy(Matx4x4 a,Matx4x4 b) /*矩阵复制函数*/
{
byte i,j;
for(i=0;i<4;i++){
for(j=0;j<4;j++)
b[i][j]=a[i][j];
}
}
void PrepareMatrix(float Tx,float Ty,float Tz,float Sx,float Sy,float Sz
float Rx,float Ry,float Rz,Matx4x4 XForm) /*矩阵初始化函数*/
{
Matx4x4 M1,M2,M3,M4,M5,M6,M7,M8,M9;
Scal3D(Sx,Sy,Sz,M1);
Rotate3D(1,Rx.M2);
Rotate3D(2,Ry,M3);
Rotate3D(3,Rz,M4);
translate3D(Tx,Ty,Tz,M5);
Multiply3DMatricies(M2,M1,M6);
Multiply3DMatricies(M3,M6,M7);
Multiply3DMatricies(M4,M7,M8);
Multiply3DMatricies(M5,M8,M9);
MatCopy(M9,XForm);
}
void PreparcInvMatrix(float Tx,float Ty,float Tz,float Sx,float Sy,float Sz,
float Rx,float Ry,float Rz,Matx4x4 XForm) /*矩阵的逆变换函数*/
{
Matx4x4 M1,M2,M3,M4,M5,M6,M7,M8,M9;
Scale3D(Sx,Sy,Sz,M1);
Rotate3D(1,Rx,M2);
Rotate3D(2,Ry,M3);
Rotate3D(3,Rz,M4);
Translate3D(Tx,Ty,Tz,M5);
Multiply3DMatricies(M4,M5,M6);
Multiply3DMatricies(M3,M6,M7);
Multiply3DMatricies(M2,M7,M8);
Multiply3DMatricies(M1,M8,M9);
MatCopy(M9,XForm);
}
void Transform(TDA A,Matx4x4 M,TDA B) /*3D变换函数*/
{
B[0]=M[0][0]*A[0]+M[0][1]*A[1]+M[0][2]*A[2]+M[0][3];
B[1]=M[1][0]*A[0]+M[1][1]*A[1]+M[1][2]*A[2]+M[0][3];
B[2]=M[2][0]*A[0]+M[2][1]*A[1]+M[2][2]*A[2]+M[0][3];
}