public class CoordTrans7Param
{ public double[,] values=new double[7,1]; //{{dx},{dy},{dz},{rx},{ry},{rz},{k}}; //public double 两个坐标系转换一般需要平移,旋转,缩放共七参数。
Y=(1+k)*M(x,y,z)*X+dX;
public double[,] values=new double[7,1];
//{{dx},{dy},{dz},{rx},{ry},{rz},{k}};
//public double dx,dy,dz,rx,ry,rz,k;
public void Set4Param(double dx,double dy,double dz,double k)
{
this.dx=dx;
this.dy=dy;
this.dz=dz;
this.k=k;
this.rx=this.ry=this.rz=0;
}
public void SetRotationParamRad(double rx,double ry,double rz)
{
this.rx=rx;
this.ry=ry;
this.rz=rz;
}
public void SetRotationParamMM(double rx,double ry,double rz)
{
SetRotationParamRad(rx*Math.PI/648000,ry*Math.PI/648000,rz*Math.PI/648000);
}
private double[,] GetMx()
{
double [,] Mx=new double[,]
{{1,0,0},
{0,Math.Cos(rx),Math.Sin(rx)},
{0,-Math.Sin(rx),Math.Cos(rx)}};
return Mx;
}
private double[,] GetMy()
{
double [,] My=new double[,]
{{Math.Cos(ry),0,-Math.Sin(ry)},
{0,1,0},
{Math.Sin(ry),0,Math.Cos(ry)}};
return My;
}
private double[,] GetMz()
{
double [,] Mz=new double[,]
{{Math.Cos(rz),Math.Sin(rz),0},
{-Math.Sin(rz),Math.Cos(rz),0},
{0,0,1}};
return Mz;
}
private double[,] GetM() //M=Mx*My*Mz? or M=Mz*My*Mx?
{
double [,] M=new double[3,3];
MatrixTool.Multi(GetMz(),GetMy(),ref M);
MatrixTool.Multi(M,GetMx(),ref M);
return M;
}
private double[,] GetMdx()
{
double[,] mt = {{ 0, 0, 0 },
{ 0, -Math.Sin(rx), Math.Cos(rx) },
{ 0, -Math.Cos(rx), -Math.Sin(rx) }};
double[,] m=new double[3,3];
MatrixTool.Multi(GetMz(),GetMy(),ref m);
MatrixTool.Multi(m,mt,ref m);
return m;
}
private double[,] GetMdy()
{
double[,] mt = {{ -Math.Sin(ry), 0, -Math.Cos(ry) },
{ 0, 0, 0 },
{ Math.Cos(ry), 0, -Math.Sin(ry) }};
double[,] m=new double[3,3];
MatrixTool.Multi(GetMz(),mt,ref m);
MatrixTool.Multi(m,GetMx(),ref m);
return m;
}
private double[,] GetMdz()
{
double[,] mt = {{ -Math.Sin(rz), Math.Co