Math.Inc 分型图形库数学函数头文件 数字函数+向量和矩阵函数+仿射变换

转自：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];
}