【中国西北陕西西安】卫星电话_激光测距仪测高仪_无人机_网络安全_测亩仪_GNSS设备推广维修服务专家--西安北斗星联测绘科技有限公司

WGS84和BJ54坐标转换源程序

时间:2010-11-17 20:47来源:未知 作者:admin 点击:
WGS84和BJ54坐标转换源程序
public class CoordTrans7Param
{
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.Cos(rz), 0 },
{ -Math.Cos(rz), -Math.Sin(rz), 0 },
{ 0, 0, 0 }};

double[,] m=new double[3,3];

MatrixTool.Multi(mt,GetMy(),ref m);
MatrixTool.Multi(m,GetMx(),ref m);
return m;
}
private double[,] specialMulti(double[,] m,double[,] X)
{
int rowNumM=m.GetLength(0);
int colNumM=m.GetLength(1);
int rowNumX=X.GetLength(0);
int colNumX=X.GetLength(1);
int lines=rowNumX/colNumM;
double[,] mt=MatrixTool.Init(rowNumM,colNumX);
double[,] subX=MatrixTool.Init(colNumM,colNumX);
double[,] res=MatrixTool.Init(rowNumM*lines,colNumX);

for(int i=0;i<lines;i++)
{
MatrixTool.CopySub(X,i*colNumM,0,colNumM,colNumX,ref subX,0,0);
MatrixTool.Multi(m,subX,ref mt);
MatrixTool.CopySub(mt,0,0,rowNumM,colNumX,ref res,i*rowNumM,0);
}
return res;
}
private double[,] specialSub(double[,] m,double[,] X)
{
int rowNumM=m.GetLength(0);
int colNumM=m.GetLength(1);
int rowNumX=X.GetLength(0);
int colNumX=X.GetLength(1);
int lines=rowNumX/rowNumM;
double[,] subX=MatrixTool.Init(rowNumM,colNumX);
double[,] res=MatrixTool.Init(rowNumX,colNumX);

for(int i=0;i<rowNumX;i+=rowNumM)
{
MatrixTool.CopySub(X,i,0,rowNumM,colNumX,ref subX,0,0);
MatrixTool.Sub(m,subX,ref subX);
MatrixTool.CopySub(subX,0,0,rowNumM,colNumX,ref res,i,0);
}
return res;
}

private double[,] GetF(double[,] X,double[,] Y)
{
double[,] f0;
double[,] qx=MatrixTool.Init(X.GetLength(0),1);
double[,] K={{-dx},{-dy},{-dz}};
double[,] S={{1+k}};

MatrixTool.Multi(X,S,ref qx);
double [,] M=GetM();
qx=specialMulti(M,qx);
MatrixTool.Sub(qx,Y,ref qx);
f0=specialSub(K,qx);
return f0;
}
private double[,] GetB(double[,] X)
{
int rowNum=X.GetLength(0);
double[,] B=MatrixTool.Init(rowNum,7);
double[,] M=GetM();
double[,] Mdx=GetMdx();
double[,] Mdy=GetMdy();
double[,] Mdz=GetMdz();
double[,] mi=MatrixTool.Ident(3);
double[,] MX,MY,MZ,MK;

MK=specialMulti(M,X);
MX=specialMulti(Mdx,X);
MY=specialMulti(Mdy,X);
MZ=specialMulti(Mdz,X);

for(int i=0;i<rowNum;i+=3)
MatrixTool.CopySub(mi,0,0,3,3,ref B,i,0);
MatrixTool.CopySub(MX,0,0,rowNum,1,ref B,0,3);
MatrixTool.CopySub(MY,0,0,rowNum,1,ref B,0,4);
MatrixTool.CopySub(MZ,0,0,rowNum,1,ref B,0,5);
MatrixTool.CopySub(MK,0,0,rowNum,1,ref B,0,6);
return B;
}
private double[,] GetA()
{
double[,] M=GetM();
double[,] I2=MatrixTool.Ident(3);
double[,] A=MatrixTool.Init(3,6);

MatrixTool.MutliConst(ref I2,-1);
MatrixTool.MutliConst(ref M,(1+k));
MatrixTool.CopySub(M,0,0,3,3,ref A,0,0);
MatrixTool.CopySub(I2,0,0,3,3,ref A,0,3);
return A;
}
private double[,] GetV(double[,] X,double[,] Y,CoordTrans7Param dpp)
{
int rowNum=X.GetLength(0);

double[,] B,F,A,B2,B3,F2,V;
double[,] AT=MatrixTool.Init(6,3);

A=GetA();
MatrixTool.AT(A,ref AT);
MatrixTool.MutliConst(ref AT,1/(1+(1+k)*(1+k)));

F=GetF(X,Y);
B=GetB(X);
B2=MatrixTool.Init(3,7);
B3=MatrixTool.Init(3,1);
F2=MatrixTool.Init(rowNum,1);
for(int i=0;i<rowNum/3;i++)
{
MatrixTool.CopySub(B,i*3,0,3,7,ref B2,0,0);
MatrixTool.Multi(B2,dpp.values,ref B3);
MatrixTool.CopySub(B3,0,0,3,1,ref F2,i*3,0);
}
MatrixTool.Sub(F,F2,ref F2);
V=specialMulti(AT,F2);
return V;
}
public double CalculateTrans7Param(double[,] X,double[,] Y)
{
int PtNum=X.GetLength(0)/3;
double[,] B;
double[,] F;
double[,] BT=MatrixTool.Init(7,3*PtNum);
double[,] BTB=MatrixTool.Init(7,7);
double[,] BTF=MatrixTool.Init(7,1);


//init pararm
CoordTrans7Param dpp=new CoordTrans7Param();
Set4Param(0,0,0,0);
this.SetRotationParamMM(0,0,0);
//debug
//this.TransCoord(X[0,0],X[1,0],X[2,0],out x2,out y2,out z2);
int round=0;
while(round++<20)
{
F=GetF(X,Y);
B=GetB(X);
MatrixTool.AT(B,ref BT);

MatrixTool.Multi(BT,B,ref BTB);
MatrixTool.Inv(BTB);
MatrixTool.Multi(BT,F,ref BTF);
MatrixTool.Multi(BTB,BTF,ref dpp.values);
if (dpp.isSmall())
break;
else
MatrixTool.Add(this.values,dpp.values,ref this.values);
}
//this.TransCoord(X[0,0],X[1,0],X[2,0],out x2,out y2,out z2);
double[,] V=GetV(X,Y,dpp);

double vMax=-1;
for(int i=0;i<V.GetLength(0);i++)
{
if (Math.Abs(V)>vMax)
vMax=Math.Abs(V);
}

return vMax;
}
private bool isSmall()
{
double s=0;
for(int i=0;i<7;i++)
s+=Math.Abs(values);
if (s<0.0000001)
return true;
else
return false;
}
public void TransCoord(double x1,double y1,double z1,out double x2,out double y2,out double z2)
{

double[,] Xi={{x1},{y1},{z1}};
double[,] DX={{dx},{dy},{dz}};
double[,] tY=new double[3,1];
double[,] K={{1+k}};

double [,] M=GetM();
MatrixTool.Multi(Xi,K,ref tY);
MatrixTool.Multi(M,tY,ref tY);
MatrixTool.Add(tY,DX,ref tY);
x2=tY[0,0];
y2=tY[1,0];
z2=tY[2,0];
}

public double dx
{
get
{
return values[0,0];
}
set
{
values[0,0]=value;
}
}
public double dy
{
get
{
return values[1,0];
}
set
{
values[1,0]=value;
}
}
public double dz
{
get
{
return values[2,0];
}
set
{
values[2,0]=value;
}
}
public double rx
{
get
{
return values[3,0];
}
set
{
values[3,0]=value;
}
}
public double ry
{
get
{
return values[4,0];
}
set
{
values[4,0]=value;
}
}
public double rz
{
get
{
return values[5,0];
}
set
{
values[5,0]=value;
}
}
public double k
{
get
{
return values[6,0];
}
set
{
values[6,0]=value;
}
}
}

矩阵计算程序
public class MatrixTool
{
public static double[,] Init(int m,int n)
{
double[,] M=new double[m,n];
for(int i=0;i<m;i++)
for(int j=0;j<n;j++)
M=0;
return M;
}

public static double[,] Ident(int rank)
{
double[,] m=new double[rank,rank];
for(int i=0;i<rank;i++)
for(int j=0;j<rank;j++)
{
if (i==j)
m=1.0;
else
m=0;
}
return m;
}
public static void MutliConst(ref double[,] m1,double c)
{
for(int i=0;i<m1.GetLength(0);i++)
for(int j=0;j<m1.GetLength(1);j++)
{
m1*=c;
}
}
public static void Copy(double[,] m1,ref double[,] m2)
{
for(int i=0;i<m1.GetLength(0);i++)
for(int j=0;j<m1.GetLength(1);j++)
{
m2=m1;
}
}
public static void CopySub(double[,] m1,int rowStart,int colStart,int rowNum,int colNum,ref double[,] m2,int rowStart2,int colStart2)
{
for(int i1=rowStart,i2=rowStart2;i1<rowStart+rowNum;i1++,i2++)
for(int j1=colStart,j2=colStart2;j1<colStart+colNum;j1++,j2++)
{
m2[i2,j2]=m1[i1,j1];
}
}
public static void Multi(double[,] m1,double [,] m2,ref double [,] mout)
{
int m1x,m1y,m2x,m2y,moutx,mouty;

if (m1.Rank!=2 || m2.Rank!=2||mout.Rank!=2)
throw new Exception("Multi 输入错误!");
m1x=m1.GetLength(0);
m1y=m1.GetLength(1);

m2x=m2.GetLength(0);
m2y=m2.GetLength(1);

moutx=mout.GetLength(0);
mouty=mout.GetLength(1);


if (m1y!=m2x||m1x!=moutx||m2y!=mouty)
throw new Exception("Multi 输入错误!");

double[,] mtemp=new double[moutx,mouty];
for(int i=0;i<m1x;i++)
for(int j=0;j<m2y;j++)
{
mtemp=0;
for (int k=0;k<m1y;k++)
mtemp+=m1*m2[k,j];
}
Copy(mtemp,ref mout);
}
public static void Add(double[,] m1,double [,] m2,ref double [,] mout)
{
int m1x,m1y,m2x,m2y;

if (m1.Rank!=2 || m2.Rank!=2||mout.Rank!=2)
throw new Exception("Matrix.Add 输入错误!");
m1x=m1.GetLength(0);
m1y=m1.GetLength(1);

m2x=m2.GetLength(0);
m2y=m2.GetLength(1);

if (m1x!=m2x||m1y!=m2y)
throw new Exception("Matrix.Add 输入错误!");
if (mout.GetLength(0)!=m1x||mout.GetLength(1)!=m2y)
throw new Exception("Matrix.Add 输入错误!");
//mout=new double[m1x,m2y];
for(int i=0;i<m1x;i++)
for(int j=0;j<m2y;j++)
mout=m1+m2;
}
public static void Sub(double[,] m1,double [,] m2,ref double [,] mout)
{
int m1x,m1y,m2x,m2y;

if (m1.Rank!=2 || m2.Rank!=2||mout.Rank!=2)
throw new Exception("Matrix.Sub 输入错误!");
m1x=m1.GetLength(0);
m1y=m1.GetLength(1);

m2x=m2.GetLength(0);
m2y=m2.GetLength(1);

if (m1x!=m2x||m1y!=m2y)
throw new Exception("Matrix.Sub 输入错误!");
if (mout.GetLength(0)!=m1x||mout.GetLength(1)!=m2y)
throw new Exception("Matrix.Sub 输入错误!");
//mout=new double[m1x,m2y];
for(int i=0;i<m1x;i++)
for(int j=0;j<m2y;j++)
mout=m1-m2;
}
public static void AT(double[,] m1,ref double [,] mout)
{
for(int i=0;i<m1.GetLength(0);i++)
for(int j=0;j<m1.GetLength(1);j++)
{
mout[j,i]=m1;
}
}
public static void ATBA(double[,] m1,double [,] m2,ref double [,] mout)
{
int M,N;
M=m1.GetLength(0);
N=m1.GetLength(1);
if (mout.GetLength(0)!=mout.GetLength(1)||mout.GetLength(0)!=M)
throw new Exception("ATBA 输入错误!");
for(int i=0;i<N;i++)
for(int j=0;j<N;j++)
{
mout=0;
for (int r=0;r<M;r++)
for(int k=0;k<M;k++)
mout=mout+m1[k,i]*m2[k,r]*m1[r,j];

}
}
public static void ABAT(double[,] m1,double [,] m2,ref double [,] mout)
{
int M,N;
M=m1.GetLength(0);
N=m1.GetLength(1);
if (mout.GetLength(0)!=mout.GetLength(1)||mout.GetLength(0)!=M)
throw new Exception("ATBA 输入错误!");
for(int i=0;i<M;i++)
for(int j=0;j<M;j++)
{
mout=0;
for (int r=0;r<N;r++)
for(int k=0;k<N;k++)
mout=mout+m1*m2[k,r]*m1[j,r];
}
}
public static void Inv(double[,] c)
{


double temp=0;
int i,j,k,N=c.GetLength(0);

//debug
for(i=1;i<N;i++)
for(j=0;j<i;j++)
c=0;
for(i=0;i<N;i++)
{
for(j=i;j<N;j++)
{
temp=c;
for(k=0;k<i;k++)
temp=temp-c[k,i]*c[k,j]/c[k,k];
if (j==i)
c=1/temp;
else
c=temp*c;
}
}

for(i=0;i<N-1;i++)
{
for(j=i+1;j<N;j++)
{
temp=-c;
for(k=i+1;k<j;k++)
{
temp=temp-c*c[k,j];
}
c=temp;
}
}

for(i=0;i<N-1;i++)
{
for(j=i;j<N;j++)
{
if(j==i)
temp=c;
else
temp=c*c[j,j];
for(k=j+1;k<N;k++)
temp=temp+c*c[j,k]*c[k,k];
c=temp;
}
}
for(i=1;i<N;i++)
for(j=0;j<i;j++)
c=c[j,i];
}
public static void DEBUG_DUMP(double[,] m1)
{
int M,N;
string buf;

M=m1.GetLength(0);
N=m1.GetLength(1);
Debug.WriteLine("****************debug matrix****************");
for(int i=0;i<M;i++)
{
buf="";
for(int j=0;j<N;j++)
{
buf+=m1.ToString("0.000000");
buf+=",";
}
Debug.WriteLine(buf);
}
Debug.WriteLine("**************** end ****************");
}
}

 

(责任编辑:admin)
顶一下
(4)
100%
踩一下
(0)
0%
------分隔线----------------------------
栏目列表
推荐内容
分享到: