public static BLHCoordinate XYZToBLH(XYZCoordinate XYZ, Datum datum)
{
double f, f1, f2;
double p, zw, nnq;
double b, l, h;
double a = datum.r_major;
double eq = datum.E2;
f = Pi * 50 / 180;
double x, y, z;
x = XYZ.X;
y = XYZ.Y;
z = XYZ.Z;
p = z / Math.Sqrt(x * x + y * y);
do
{
zw = a / Math.Sqrt(1 - eq * Math.Sin(f) * Math.Sin(f));
nnq = 1 - eq * zw / (Math.Sqrt(x * x + y * y) / Math.Cos(f));
f1 = Math.Atan(p / nnq);
f2 = f;
f = f1;
} while (!(Math.Abs(f2 - f1) < 10E-10));
b = f * 180 / Pi;
l = Math.Atan(y / x) * 180 / Pi;
if (l < 0)
{
l += 180;
}
h = Math.Sqrt(x * x + y * y) / Math.Cos(f1) - a / Math.Sqrt(1 - eq * Math.Sin(f1) * Math.Sin(f1));
return(new BLHCoordinate(b, l, h));
}
/// <summary>
/// 利用4参数求新坐标系的坐标
/// </summary>
/// <param name="aPtSource">点的源坐标系的坐标</param>
/// <param name="sep">已经知道的4参数</param>
/// <param name="aPtTo">输出: 点的新坐标系的坐标</param>
public static XYZCoordinate Para4Convert(XYZCoordinate aPtSource, FourPara forPara)
{
double k = forPara.scale;
double a1 = k * Math.Cos(forPara.Ax);
double a2 = k * Math.Sin(forPara.Ax);
XYZCoordinate ToCoordinate = new XYZCoordinate();
ToCoordinate.X = forPara.deltaX + a1 * aPtSource.X + a2 * aPtSource.Y;
ToCoordinate.Y = forPara.deltaY + a1 * aPtSource.Y - a2 * aPtSource.X;
ToCoordinate.Z = aPtSource.Z;
return(ToCoordinate);
}
/// <summary>
/// 利用7参数求新坐标系的坐标
/// </summary>
/// <param name="aPtSource">点的源坐标系的坐标</param>
/// <param name="sep">已经知道的7参数</param>
/// <param name="aPtTo">输出: 点的新坐标系的坐标</param>
public static XYZCoordinate Para7Convert(XYZCoordinate aPtSource, SevenPara sep)
{
double k = sep.scale;
double a2 = k * sep.Ax;
double a3 = k * sep.Ay;
double a4 = k * sep.Az;
XYZCoordinate ToCoordinate = new XYZCoordinate();
ToCoordinate.X = sep.deltaX + k * aPtSource.X - a3 * aPtSource.Z + a4 * aPtSource.Y;
ToCoordinate.Y = sep.deltaY + k * aPtSource.Y + a2 * aPtSource.Z - a4 * aPtSource.X;
ToCoordinate.Z = sep.deltaZ + k * aPtSource.Z - a2 * aPtSource.Y + a3 * aPtSource.X;
return(ToCoordinate);
}
//高斯投影由大地平面坐标(Unit:Metres)反算经纬度(Unit:DD)
public static BLHCoordinate GaussProjInvCal(XYZCoordinate XYZ, Datum datum, double lon)
{
int ProjNo, ZoneWide; ////带宽
double l, b, longitude0, X0, xval, yval;
double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai;
a = datum.r_major; //54年北京坐标系参数
ZoneWide = 3; //3度带宽
ProjNo = (int)(XYZ.X / 1000000L); //查找带号
longitude0 = lon;
if (Math.Abs(lon - 0) < 0.000001)
{
longitude0 = ProjNo * ZoneWide; //中央经线
}
longitude0 = longitude0 * Math.PI / 180; //中央经线
X0 = ProjNo * 1000000L + 500000L;
xval = XYZ.X - X0; //带内大地坐标
yval = XYZ.Y;
e2 = datum.E2;
e1 = (1.0 - Math.Sqrt(1 - e2)) / (1.0 + Math.Sqrt(1 - e2));
ee = e2 / (1 - e2);
M = yval;
u = M / (a * (1 - e2 / 4 - 3 * e2 * e2 / 64 - 5 * e2 * e2 * e2 / 256));
fai = u + (3 * e1 / 2 - 27 * e1 * e1 * e1 / 32) * Math.Sin(2 * u) + (21 * e1 * e1 / 16 - 55 * e1 * e1 * e1 * e1 / 32) * Math.Sin(4 * u) + (151 * e1 * e1 * e1 / 96) * Math.Sin(6 * u) + (1097 * e1 * e1 * e1 * e1 / 512) * Math.Sin(8 * u);
C = ee * Math.Cos(fai) * Math.Cos(fai);
T = Math.Tan(fai) * Math.Tan(fai);
NN = a / Math.Sqrt(1.0 - e2 * Math.Sin(fai) * Math.Sin(fai));
R = a * (1 - e2) / Math.Sqrt((1 - e2 * Math.Sin(fai) * Math.Sin(fai)) * (1 - e2 * Math.Sin(fai) * Math.Sin(fai)) * (1 - e2 * Math.Sin(fai) * Math.Sin(fai)));
D = xval / NN;
//计算经度(Longitude) 纬度(Latitude)
l = longitude0 + (D - (1 + 2 * T + C) * D * D * D / 6 + (5 - 2 * C + 28 * T - 3 * C * C + 8 * ee + 24 * T * T) * D
* D * D * D * D / 120) / Math.Cos(fai);
b = fai - (NN * Math.Tan(fai) / R) * (D * D / 2 - (5 + 3 * T + 10 * C - 4 * C * C - 9 * ee) * D * D * D * D / 24
+ (61 + 90 * T + 298 * C + 45 * T * T - 256 * ee - 3 * C * C) * D * D * D * D * D * D / 720);
//转换为度 DD
l = l * 180 / Math.PI;
b = b * 180 / Math.PI;
return(new BLHCoordinate(b, l, XYZ.Z));
}
