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=A;
}
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[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=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[k]*B[k][j];
        C[j]=ab;
    }
  }
}
void MatCopy(Matx4x4 a,Matx4x4 b)     /*矩阵复制函数*/
{
  byte i,j;
  for(i=0;i<4;i++){
    for(j=0;j<4;j++)
        b[j]=a[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];
}