关于经纬度算法

public Angle se(Angle latA, Angle lonA, TKsoft.Earth.Angle latB, Angle lonB)
{
//知道 两点经纬度，求角度的方法
double cosLatB = Math.Cos(latB.Radians);
Angle tcA = TKsoft.Earth.Angle.FromRadians(Math.Atan2(
Math.Sin(lonA.Radians - lonB.Radians) * cosLatB,
Math.Cos(latA.Radians) * Math.Sin(latB.Radians) -
Math.Sin(latA.Radians) * cosLatB *
Math.Cos(lonA.Radians - lonB.Radians)));
if (tcA.Radians < 0)
tcA.Radians = tcA.Radians + Math.PI * 2;
tcA.Radians = Math.PI * 2 - tcA.Radians;
return tcA;
}

/**
*根据一点、角点(正北0度，顺生针增大)、距离(米)计算另一点
*/
public static System.Drawing.PointF CalPoint(double x, double y, double angel, double distance)
{
//
System.Drawing.PointF endPoint = new System.Drawing.PointF();
//角度换算成正东0度，逆时针增大
angel = -angel - 360+90;
//米换算成经纬度
var dis = distance / 1000 / 111.7;
//角点换成弧度
angel = angel * Math.PI / 180;
//计算y坐标
endPoint.Y = (float)(Math.Sin(angel) * dis + y);
//计算x坐标
endPoint.X = (float)(Math.Cos(angel) * dis + x);
return endPoint;
}

/// <summary>
/// 计算两个经纬度之间的经纬度差
/// </summary>
/// <param name="pt1"></param>
/// <param name="pt2"></param>
/// <returns></returns>
public static double Distance(IPoint pt1, IPoint pt2)
{
return Distance(pt1.X, pt1.Y, pt2.X, pt2.Y);
}
/// <summary>
/// 计算两个坐标点之间的距离
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="tx"></param>
/// <param name="ty"></param>
/// <returns></returns>
public static double Distance(double x, double y, double tx, double ty)
{
double a = ty - y;
double b = tx - x;
return Math.Sqrt(a * a + b * b);
}

/// <summary>
/// 计算起始点和结束点之间的地理方向
/// </summary>
/// <param name="start"></param>
/// <param name="end"></param>
/// <returns></returns>
public static double CalcDirection(IPoint start, IPoint end)
{
return CalcDirection(start.X, start.Y, end.X, end.Y);
}

public static double CalcDirection(double bX, double bY, double eX, double eY)
{
double degree = 0;
double dx = eX - bX;
double dy = eY - bY;
if (dx == 0)
{
if (bY < eY) degree = 0;
else degree = 180;
}
else
{
degree = Math.Atan(dy / dx) * 180 / Math.PI;
if (degree < 0 && bY < eY) degree = 180 + degree;
degree = 90 - degree;
if (bY > eY && bX > eX) degree += 180;
if (degree == 90 && bX > eX) degree += 180;
}
return degree;
}

/// <summary>
/// 获取指定纬度线上纬线圈的半径
/// </summary>
/// <param name="lat"></param>
/// <returns></returns>
public static double CalcLatRadius(double lat)
{
double arc = lat / 180 * Math.PI;
return Math.Cos(arc) * DrawArgs.EquatorialRadius;
}

/// <summary>
/// 求出指定长度的线段在指定纬度圈上的经度跨度
/// </summary>
/// <param name="length">长度(或者距离，单位:米)</param>
/// <param name="lat">纬度</param>
/// <returns></returns>
public static double CalcLonSpan(double length, double lat)
{
double c = 2 * Math.PI * CalcLatRadius(lat);
return length / c * 360;
}

/// <summary>
/// 计算指定长度的线段在纬度跨度
/// </summary>
/// <param name="length"></param>
/// <returns></returns>
public static double CalcLatSpan(double length)
{
double c = 2 * Math.PI * DrawArgs.EquatorialRadius;
return length / c * 360;
}

/// <summary>
/// 计算num的pwo次幂
/// </summary>
/// <param name="num"></param>
/// <param name="pow"></param>
/// <returns></returns>
public static double Pow(double num, int pow)
{
double result = Convert.ToDouble(num);
for (int i = 1; i < pow; i++)
{
result *= num;
}
return result;
}

//定义角度转化为弧度的函数
public double angle_radian(double[] angle)                                       //角度转化为弧度函数
{
double radian;
radian = (angle[0] + angle[1] / 60 + angle[2] / 3600) / 180 * PI;
return radian;
}

//定义弧度转化为角度的函数
public double[] radian_angle(double radian)                                      //弧度转化为角度函数
{
double degree, minute, second;
double[] angle = { 0, 0, 0 };
degree = System.Math.Truncate(radian /PI * 180);
minute = System.Math.Truncate((radian / PI * 180 - degree) * 60);
second = System.Math.Abs(((radian / PI * 180 - degree) * 60 - minute) * 60);
minute = System.Math.Abs(minute);
angle[0] = degree;
angle[1] = minute;
angle[2] = second;
return angle;
}

public static double E = 0.081813334,PI = Math.PI;              //定义常变量
public static double B1, L1, A1, B2, L2, A2, S;                 //定义主变量
public static double A,B,C,α,β;                              //定义正算变量
public static double a1, a2, b1, b2;                           //定义反算变量

//大地主题正算
//大地主题正算
private void button1_Click(object sender, EventArgs e)
{
double[] Bre1={0,0,0},Lat1={0,0,0},Ang1={0,0,0}, Bre2={0,0,0},Lat2={0,0,0},Ang2={0,0,0};
double W1, sinu1, sinu2, cosu1, sinA0, cotσ1, sin2σ1, cos2σ1,couAo, sin2σ1plusσ0, cos2σ1plusσ0, σ, σ0, λ, δ;

Bre1[0] = Convert.ToDouble(textBox1.Text); Bre1[1] = Convert.ToDouble(textBox2.Text);Bre1[2] = Convert.ToDouble(textBox3.Text);
Lat1[0] = Convert.ToDouble(textBox4.Text); Lat1[1] = Convert.ToDouble(textBox5.Text);Lat1[2] = Convert.ToDouble(textBox6.Text);
Ang1[0] = Convert.ToDouble(textBox7.Text); Ang1[1] = Convert.ToDouble(textBox8.Text); Ang1[2] = Convert.ToDouble(textBox9.Text);
S= Convert.ToDouble(textBox10.Text);
B1= angle_radian(Bre1);L1= angle_radian(Lat1);A1= angle_radian(Ang1);

//计算起点规划纬度
W1 = Math.Sqrt(1.0 - Math.Pow(E *Math.Sin(B1), 2));
sinu1 = (Math.Sin(B1) * Math.Sqrt(1 - E * E)) / W1;
cosu1 = Math.Cos(B1) / W1;

//计算辅助函数值
sinA0 = cosu1 * Math.Sin(A1);
cotσ1 = cosu1 * Math.Cos(A1) / sinu1;
sin2σ1 = cotσ1 * 2 / (cotσ1 * cotσ1 + 1);
cos2σ1 = (cotσ1 * cotσ1 - 1) / (cotσ1 * cotσ1 + 1);

//计算必要系数值
couAo = Math.Sqrt(1 - sinA0 * sinA0);
A = 6356863.020 + (10708.949 - 13.474 * couAo * couAo) * couAo * couAo;
B = (5354.469 - 8.978 * couAo * couAo) * couAo * couAo;
C = (2.238 * couAo * couAo) * couAo * couAo + 0.006;
α = (33523299 - (28189 - 70 * couAo * couAo) * couAo * couAo) * Math.Pow(10, -10);
β= (14093.5 - 48.5 * couAo * couAo) * couAo * couAo * System.Math.Pow(10, -10);

//计算球面长度
σ0 = (S - (B + C * cos2σ1) * sin2σ1) / A;
sin2σ1plusσ0 = sin2σ1 * Math.Cos(2 * σ0) + cos2σ1 * Math.Sin(2 * σ0);
cos2σ1plusσ0 = cos2σ1 * Math.Cos(2 * σ0) - sin2σ1 * Math.Sin(2 * σ0);
σ = σ0 + (B + 5 * C * cos2σ1plusσ0) * sin2σ1plusσ0 / A;

//计算经度差改正数
δ = (α * σ + β * (sin2σ1plusσ0 - sin2σ1)) * sinA0;

//计算终点大地坐标及坐标方位角
sinu2 = sinu1 * Math.Cos(σ) + cosu1 * Math.Cos(A1) * Math.Sin(σ);
B2 = Math.Atan(sinu2 / (Math.Sqrt(1 - E * E) * Math.Sqrt(1 - sinu2 * sinu2)));
λ = Math.Atan(Math.Sin(A1) * Math.Sin(σ) / (cosu1 * Math.Cos(σ) - sinu1 * Math.Sin(σ) * Math.Cos(A1)));

if (Math.Sin(A1) > 0)
{
if (Math.Tan(λ) > 0)
{
λ = Math.Abs(λ);
}
else
{
λ = PI - Math.Abs(λ);
}
}
else
{
if (Math.Tan(λ) < 0)
{
λ = -Math.Abs(λ);
}
else
{
λ = Math.Abs(λ) - PI;
}
}
L2 = L1 + λ - δ;
A2 =Math.Atan(cosu1 * Math.Sin(A1) / (cosu1 * Math.Cos(σ) *Math.Cos(A1) - sinu1 *Math.Sin(σ)));
if (Math.Sin(A1) < 0)
{
if (Math.Tan(A2) > 0)
{
A2 = Math.Abs(A2);
}
else
{
A2 = PI - Math.Abs(A2);
}
}
else
{
if (Math.Tan(A2) > 0)
{
A2 = PI + Math.Abs(A2);
}
else
{
A2 = 2 * PI - Math.Abs(A2);
}
}
//将弧度制角化为角度制角
Bre2 = radian_angle(B2);
Lat2 = radian_angle(L2);
Ang2 = radian_angle(A2);

//通过语句在程序界面显示最后结果
textBox11.Text = Convert.ToString(Bre2[0]); textBox12.Text = Convert.ToString(Bre2[1]); textBox13.Text = Convert.ToString(Bre2[2]);
textBox14.Text = Convert.ToString(Lat2[0]); textBox15.Text = Convert.ToString(Lat2[1]); textBox16.Text = Convert.ToString(Lat2[2]);
textBox17.Text = Convert.ToString(Ang2[0]); textBox18.Text = Convert.ToString(Ang2[1]); textBox19.Text = Convert.ToString(Ang2[2]);

}

//大地主题 反算

private void button2_Click(object sender, EventArgs e)
{
double B1, L1, A1, B2, L2, A2, S;
double[] Bre1 = { 0, 0, 0 }, Lat1 = { 0, 0, 0 }, Ang1 = { 0, 0, 0 }, Bre2 = { 0, 0, 0 }, Lat2 = { 0, 0, 0 }, Ang2 = { 0, 0, 0 };
double W1, W2, sinu1, sinu2, cosu1, cosu2, sinA0,cosA0,  sinσ, cosσ, σ,  λ, L, δ, δ1,p, q, x, y, β1,B11,C11;

Bre1[0] = Convert.ToDouble(textBox25.Text); Bre1[1] = Convert.ToDouble(textBox24.Text); Bre1[2] = Convert.ToDouble(textBox23.Text);
Lat1[0] = Convert.ToDouble(textBox20.Text); Lat1[1] = Convert.ToDouble(textBox21.Text); Lat1[2] = Convert.ToDouble(textBox22.Text);
Bre2[0] = Convert.ToDouble(textBox26.Text); Bre2[1] = Convert.ToDouble(textBox27.Text); Bre2[2] = Convert.ToDouble(textBox28.Text);
Lat2[0] = Convert.ToDouble(textBox29.Text); Lat2[1] = Convert.ToDouble(textBox30.Text); Lat2[2] = Convert.ToDouble(textBox31.Text);
B1 = angle_radian(Bre1); L1 = angle_radian(Lat1);
B2 = angle_radian(Bre2); L2 = angle_radian(Lat2);

W1 = Math.Sqrt(1 - E * E * Math.Sin(B1) * Math.Sin(B1));
W2 = Math.Sqrt(1 - E * E * Math.Sin(B2) * Math.Sin(B2));
sinu1 = (Math.Sin(B1) * Math.Sqrt(1 - E * E)) / W1;
sinu2 = (Math.Sin(B2) * Math.Sqrt(1 - E * E)) / W2;
cosu1 = Math.Cos(B1) / W1;
cosu2 = Math.Cos(B2) / W2;
L = L2 - L1;
a1 = sinu1 * sinu2;
a2 = cosu1 * cosu2;
b1 = cosu1 * sinu2;
b2 = sinu1 * cosu2;

//用逐次趋近法计算大地起点大地方位角、球面长度及经差λ=L+δ：
δ = 0; λ = 1.0; δ1 = 0;
do
{
δ = δ1;
p = cosu2 * Math.Sin(λ);
q = b1 - b2 * Math.Cos(λ);
A1 = Math.Atan(p / q);
if (p > 0)
{
if (q > 0)
{
A1 = Math.Abs(A1);
}
else
{
A1 = PI - Math.Abs(A1);
}
}
else
{
if (p < 0)
{
A1 =PI + Math.Abs(A1);
}
else
{
A1 = 2 * PI - Math.Abs(A1);
}
}
sinσ = p * Math.Sin(A1) + q * Math.Cos(A1);
cosσ = a1 + a2 * Math.Cos(λ);
σ = Math.Atan(sinσ / cosσ);

if (cosσ > 0)
{ σ = Math.Abs(σ); }
else
{ σ = PI - Math.Abs(σ); }

sinA0 = cosu1 * Math.Sin(A1);
cosA0 = Math.Sqrt(1 - sinA0 * sinA0);
x = 2 * a1 - Math.Pow(cosA0, 2) * cosσ;
α = (33523299 - (28189 - 70 * cosA0 * cosA0) * cosA0 * cosA0) * Math.Pow(10, -10);
β1 = (28189 - 94 * cosA0 * cosA0) * Math.Pow(10, -10);
δ1 = (α * σ - β1 * x *Math.Sin(σ)) * sinA0;
λ = L + δ1;
} while (Math.Abs(δ - δ1) >= Math.Pow(10, -9));
δ = δ1;

//计算某些系数
A = 6356863.020 + (10708.949 - 13.474 * cosA0 * cosA0) * cosA0 * cosA0;
B11 = 10708.938 - 17.956 * cosA0 * cosA0;
C11 = 4.487;

//计算大地线长度S
y = (Math.Pow(cosA0, 4) - 2 * x * x) * cosσ;
S = A * σ + (B11 * x + C11 * y) * sinσ;

//计算反方位角A2
p = cosu1 * System.Math.Sin(λ); q = b1 * System.Math.Cos(λ) - b2;
A2 = System.Math.Atan(p / q);

if (p < 0)
{
if (q < 0)
{
A2 = Math.Abs(A2);
}
else
{
A2 =PI - Math.Abs(A2);
}
}
else
{
if (p > 0)
{
A2 = PI + Math.Abs(A2);
}
else
{
A2 = 2 * PI -Math.Abs(A2);
}
}

//将弧度制角转化为角度制角
Ang1 = radian_angle(A1);
Ang2 = radian_angle(A2);

textBox33.Text = Convert.ToString(Ang1[0]); textBox34.Text = Convert.ToString(Ang1[1]); textBox35.Text = Convert.ToString(Ang1[2]);
textBox36.Text = Convert.ToString(Ang2[0]); textBox37.Text = Convert.ToString(Ang2[1]); textBox38.Text = Convert.ToString(Ang2[2]);
textBox32.Text = Convert.ToString(S);

}

/// <summary>
/// 计算两个经纬度之间的球面距离
/// </summary>
/// <param Name="p1">p1的经纬度</param>
/// <param Name="p2">p2的经纬度</param>
/// <returns>距离(单位是米)</returns>
public static double Distance(PointF p1, PointF p2)
{
double _nRslt;
double x1 = p1.X * Math.PI / 180.0;
double y1 = p1.Y * Math.PI / 180.0;
double x2 = p2.X * Math.PI / 180.0;
double y2 = p2.Y * Math.PI / 180.0;
_nRslt = Math.Sin(y1) * Math.Sin(y2) + Math.Cos(y1) * Math.Cos(y2) * Math.Cos(Math.Abs(x2 - x1));
if (_nRslt < 1)
{
_nRslt = Math.Atan(-_nRslt / Math.Sqrt(-_nRslt * _nRslt + 1)) + 2.0 * Math.Atan(1);
}
else
{
_nRslt = 0.0;
}

_nRslt = _nRslt * DrawArgs.EquatorialRadius;
return _nRslt;
}