📄 myclass.h
字号:
{
temp=l[k*rows+t];
l[k*rows+t]=l[ik*rows+t];
l[ik*rows+t]=temp;
}
for(t=k;t<rows;++t)
{
temp=a[k*rows+t];
a[k*rows+t]=a[ik*rows+t];
a[ik*rows+t]=temp;
}
temp=s[k];
s[k]=s[ik];
s[ik]=temp;
}
u[k*rows+k]=s[k];
if(k<rows-1)
{
for(j=k+1;j<rows;++j)
{
u[k*rows+j]=a[k*rows+j];
for(t=0;t<k;++t)
u[k*rows+j]-=l[k*rows+t]*u[t*rows+j];
}
for(i=k+1;i<rows;++i)
l[i*rows+k]=s[i]/u[k*rows+k];
}
}
//Qb = Ly AND Ux = y
for(j=0;j<rows;++j)
{
for(i=0;i<rows;++i)
b[i]=bb[i*rows+j];
for(k=0;k<rows-1;++k)
{
t=M[k];
temp=b[k];
b[k]=b[t];
b[t]=temp;
}
y[0]=b[0];
for(i=1;i<rows;++i)
{
y[i]=b[i];
for(t=0;t<i;++t)
y[i]-=l[i*rows+t]*y[t];
}
x[rows-1]=y[rows-1]/u[rows*rows-1];
for(i=rows-2;i>-1;--i)
{
x[i]=y[i];
for(t=i+1;t<rows;++t)
x[i]-=u[i*rows+t]*x[t];
x[i]/=u[i*rows+i];
}
for(i=0;i<rows;++i)
{
xx[i*rows+j]=x[i];
}
}
delete[]M;
delete[]s;
delete[]l;
delete[]u;
delete[]a;
delete[]b;
delete[]y;
delete[]x;
M=NULL;
s=NULL;
l=NULL;
u=NULL;
a=NULL;
b=NULL;
y=NULL;
x=NULL;
}
//////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////
// invertible matrice 矩阵求逆
template<class TT1,class TT2>//,class TT3>
void __declspec(dllexport) MatrixAnti(TT1* Matrix,TT2* MatrixA,int rows)
{// Matrix * MatrixA = I I = E
double* E=new double[rows*rows];
MatrixIdentity(E,rows);
/* memset(E,0,rows*rows*sizeof(double));
int i;
for(i=0;i<rows;++i)
E[i*rows+i]=1.0;*/
/*//Gauss solution
TT2* b=new TT2[rows];
TT2* x=new TT2[rows];
memset(b,0.0,rows*sizeof(TT2));
memset(x,0.0,rows*sizeof(TT2));
for(int j=0;j<rows;j++)
{
for(i=0;i<rows;i++)
b[i]=E[i*rows+j];
Gauss(Matrix,b,x,rows);
for(i=0;i<rows;i++)
MatrixA[i*rows+j]=x[i];
}
delete[]b;
delete[]x;
b=NULL;
x=NULL;*/
//Doolittle solution
Doolittle(Matrix,E,MatrixA,rows);
delete[]E;
E=NULL;
}
//////////////////////////////////////////////////////////////////////////
///Jacobi Solve Eigen of Matrix 求解矩阵特征值及特征向量
template<class TT1,class TT2>
void __declspec(dllexport) EigenJacobi(TT1 *M1,TT2 *M2,int rows)
{//M1 - rows * rows M2 - rows * rows
int i,j,p,q;
double t,z,cos2w,sin2w,cosw,sinw;
double *Upq,*D,maxa,*UTpq;
Upq=new double[rows*rows];
UTpq=new double[rows*rows];
D=new double[rows*rows];
MatrixIdentity(M2,rows);
/* memset(M2,0,rows*rows*sizeof(double));
for(i=0;i<rows;++i)
M2[i*rows+i]=1.0; */
maxa=M1[0*rows+1];
p=0;
q=1;
for(i=0;i<rows;++i)
for(j=i+1;j<rows;++j)
if(fabs(M1[i*rows+j])>fabs(maxa))
{
maxa=M1[i*rows+j];
p=i;
q=j;
}
//
do
{
MatrixIdentity(Upq,rows);
/* memset(Upq,0,rows*rows*sizeof(double));
for(i=0;i<rows;++i)
Upq[i*rows+i]=1.0; */
t=2.0*M1[p*rows+q]/(M1[p*rows+p]-M1[q*rows+q]);
z=(M1[p*rows+p]-M1[q*rows+q])/(2.0*M1[p*rows+q]);
if(fabs(t)<1.0)
{
cos2w=1.0/sqrt(1.0+t*t);
sin2w=t/sqrt(1.0+t*t);
}
else
{
cos2w=fabs(z)/sqrt(1.0+z*z);
sin2w=z/(fabs(z)*sqrt(1.0+z*z));
}
cosw=sqrt(0.5*(1+cos2w));
sinw=0.5*sin2w/cosw;
Upq[p*rows+p]=cosw;
Upq[q*rows+q]=cosw;
Upq[p*rows+q]=-sinw;
Upq[q*rows+p]=sinw;
//
MatrixTranspose(Upq,UTpq,rows,rows);
MultMatrix(UTpq,M1,D,rows,rows);
MultMatrix(D,Upq,M1,rows,rows);
MultMatrix(M2,Upq,D,rows,rows);
MatrixEqual(D,M2,rows,rows);
//
maxa=M1[0*rows+1];
p=0;
q=1;
for(i=0;i<rows;++i)
for(j=i+1;j<rows;++j)
if(fabs(M1[i*rows+j])>fabs(maxa))
{
maxa=M1[i*rows+j];
p=i;
q=j;
}
} while(fabs(maxa)>1.0e-8);
delete []Upq;
Upq=NULL;
delete []UTpq;
UTpq=NULL;
delete []D;
D=NULL;
}
//EigenVector of max EigenValue matrix 求解最小特征值 特征向量
template<class TT1,class TT2>
void __declspec(dllexport) EigenVector(TT1 *M,TT2 *M2,int rows)
{// M - rows * rows M2 - rows
TT1 max;
int i,p;
TT2 *M1=new TT2[rows*rows];
EigenJacobi(M,M1,rows);
max=M[0*rows+0];
p=0;
for(i=1;i<rows;++i)
if(max>M[i*rows+i])
{
p=i;
max=M[i*rows+i];
}
for(i=0;i<rows;++i)
M2[i]=M1[i*rows+p];
delete []M1;
M1=NULL;
}
//////////////////////////////////////////////////////////////////////////
//EigenVector Transform matrix Best align points pairs numbers more than four
//四元素描述 旋转矩阵
template<class TT1,class TT2>
void __declspec(dllexport) EigenMatrix(const tagCVector V[],TT1 *ME, TT2 *Matrix, int rows)
{//V - 2 ME - 4 Matrix - 16
MatrixIdentity(Matrix,rows);
Matrix[0]=ME[0]*ME[0]+ME[1]*ME[1]-ME[2]*ME[2]-ME[3]*ME[3];
Matrix[5]=ME[0]*ME[0]-ME[1]*ME[1]+ME[2]*ME[2]-ME[3]*ME[3];
Matrix[10]=ME[0]*ME[0]-ME[1]*ME[1]-ME[2]*ME[2]+ME[3]*ME[3];
Matrix[1]=2.0*(ME[1]*ME[2]+ME[0]*ME[3]);//by rows lefthand
Matrix[4]=2.0*(ME[1]*ME[2]-ME[0]*ME[3]);
Matrix[2]=2.0*(ME[1]*ME[3]-ME[0]*ME[2]);
Matrix[8]=2.0*(ME[1]*ME[3]+ME[0]*ME[2]);
Matrix[6]=2.0*(ME[2]*ME[3]+ME[0]*ME[1]);
Matrix[9]=2.0*(ME[2]*ME[3]-ME[0]*ME[1]);
/*
Matrix[4]=2.0*(ME[1]*ME[2]+ME[0]*ME[3]);
Matrix[1]=2.0*(ME[1]*ME[2]-ME[0]*ME[3]);
Matrix[8]=2.0*(ME[1]*ME[3]-ME[0]*ME[2]);
Matrix[2]=2.0*(ME[1]*ME[3]+ME[0]*ME[2]);
Matrix[9]=2.0*(ME[2]*ME[3]+ME[0]*ME[1]);
Matrix[6]=2.0*(ME[2]*ME[3]-ME[0]*ME[1]);*/
double M1[4][4],M2[4][4],M3[4][4],M4[4][4];
GetTranslateMatrix(tagCVector(0,0,0)-V[0],&M1[0][0]);
GetTranslateMatrix(V[0]-V[1],&M2[0][0]);
GetTranslateMatrix(V[0],&M3[0][0]);
MultMatrix(&M2[0][0],Matrix,&M4[0][0],4,4);
MultMatrix(&M1[0][0],&M4[0][0],&M2[0][0],4,4);
MultMatrix(&M2[0][0],&M3[0][0],Matrix,4,4);
}
template<class T,class T0>//旋转矩阵 绕X\Y\Z轴转
void __declspec(dllexport) GetRotateMatrix(const char& c,const T& angle,T0* Matrix)
{ //Rotate around axis X Y Z
MatrixIdentity(Matrix,4);
/* memset(Matrix,0,16*sizeof(T0));
for(int i=0;i<4;++i)
Matrix[i*4+i]=1.0;*/
float cangle=angle*PI/180.0;
switch(c)
{
case 'X':
case 'x':
Matrix[5]=cos(cangle);
Matrix[10]=Matrix[5];
Matrix[6]=sin(cangle);
Matrix[9]=-Matrix[6];
break;
case 'Y':
case 'y':
Matrix[0]=cos(cangle);
Matrix[10]=Matrix[0];
Matrix[2]=-sin(cangle);
Matrix[8]=-Matrix[2];
break;
case 'Z':
case 'z':
Matrix[5]=cos(cangle);
Matrix[0]=Matrix[5];
Matrix[1]=sin(cangle);
Matrix[4]=-Matrix[1];
break;
default:
break;
}
}
template<class T>//平移矩阵
void __declspec(dllexport) GetTranslateMatrix(const tagCVector& dxyz,T* Matrix)
{// dxyz offset translate
MatrixIdentity(Matrix,4);
/* memset(Matrix,0,16*sizeof(T));
for(int i=0;i<4;++i)
Matrix[i*4+i]=1.0;*/
Matrix[12]=dxyz.x;
Matrix[13]=dxyz.y;
Matrix[14]=dxyz.z;
}
template<class T,class T0>//绕任意轴旋转矩阵
void __declspec(dllexport) GetRotateMatrix(const T& Angle,const tagCVector& Origin,const tagCVector& Axis,T0* Matrix)
{//rotate anyaxis Origin - origin point Axis - axis direction
double AngleX,AngleY,AngleZ;
tagCVector OriginXYZ,axis=Axis;
OriginXYZ.x=-Origin.x;
OriginXYZ.y=-Origin.y;
OriginXYZ.z=-Origin.z;
axis.normalize();
AngleZ=Angle;//*PI/180.0
AngleY=-asin(axis.x)*180.0/PI;
if(fabs(axis.z)>0.0)
{
AngleX=atan2(axis.y,axis.z)*180.0/PI;
// if(Axis.z<0.0)
// AngleX=AngleX+PI;
}
else
{
if(axis.y>0.0)
AngleX=90.0;
else if(axis.y<0.0)
AngleX=-90.0;
else
AngleX=0.0;
}
double RX[4][4],RY[4][4],RZ[4][4],TT[4][4],mat[4][4],RXA[4][4],RYA[4][4];
GetTranslateMatrix(OriginXYZ,&TT[0][0]);
GetRotateMatrix('X',AngleX,&RX[0][0]);
GetRotateMatrix('Y',AngleY,&RY[0][0]);
GetRotateMatrix('Z',AngleZ,&RZ[0][0]);
MultMatrix(&TT[0][0],&RX[0][0],&mat[0][0],4,4);
MultMatrix(&mat[0][0],&RY[0][0],Matrix,4,4);
MultMatrix(Matrix,&RZ[0][0],&mat[0][0],4,4);
GetTranslateMatrix(Origin,&TT[0][0]);
// GetRotateMatrix('X',-AngleX,&RX[0][0]);
// GetRotateMatrix('Y',-AngleY,&RY[0][0]);
MatrixTranspose(&RX[0][0],&RXA[0][0],4,4);
MatrixTranspose(&RY[0][0],&RYA[0][0],4,4);
MultMatrix(&mat[0][0],&RYA[0][0],Matrix,4,4);
MultMatrix(Matrix,&RXA[0][0],&mat[0][0],4,4);
MultMatrix(&mat[0][0],&TT[0][0],Matrix,4,4);
}
template<class T>//向量旋转到Z轴的旋转矩阵
void __declspec(dllexport) GetRotateToZAxisMatrix(const char& c,const tagCVector& Origin,const tagCVector& Axis,T* Matrix)
{// c='A' 'a' antimatrix c='P' 'p' positive matrix
double AngleX,AngleY;
tagCVector OriginXYZ,axis=Axis;
OriginXYZ.x=-Origin.x;
OriginXYZ.y=-Origin.y;
OriginXYZ.z=-Origin.z;
axis.normalize();
AngleY=-asin(axis.x)*180.0/PI;
if(fabs(axis.z)>0.0)
{
AngleX=atan2(axis.y,axis.z)*180.0/PI;// if(Axis.z<0.0)// AngleX=AngleX+PI;
}
else
{
if(axis.y>0.0)
AngleX=90.0;
else if(axis.y<0.0)
AngleX=-90.0;
else
AngleX=0.0;
}
double RX[4][4],RY[4][4],TT[4][4],mat[4][4];
switch(c)
{
case 'A':
case 'a':
GetTranslateMatrix(Origin,&TT[0][0]);
GetRotateMatrix('X',-AngleX,&RX[0][0]);
GetRotateMatrix('Y',-AngleY,&RY[0][0]);
MultMatrix(&RY[0][0],&RX[0][0],&mat[0][0],4,4);
MultMatrix(&mat[0][0],&TT[0][0],Matrix,4,4);
break;
case 'P':
case 'p':
default:
GetTranslateMatrix(OriginXYZ,&TT[0][0]);
GetRotateMatrix('X',AngleX,&RX[0][0]);
GetRotateMatrix('Y',AngleY,&RY[0][0]);
MultMatrix(&TT[0][0],&RX[0][0],&mat[0][0],4,4);
MultMatrix(&mat[0][0],&RY[0][0],Matrix,4,4);
break;
}
}
/////////////////////////////////////////////////////////////////////////////////////////////
#endif⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -