372 lines
12 KiB
C#
372 lines
12 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using System.Text;
|
||
using System.Threading.Tasks;
|
||
|
||
namespace Plane.FormationCreator
|
||
{
|
||
/**
|
||
*
|
||
* 计算经纬度、距离、方位角
|
||
*
|
||
* */
|
||
public class CalculationLogLatDistance
|
||
{
|
||
/**
|
||
* 地球赤道半径(km)
|
||
* */
|
||
static public double EARTH_RADIUS = 6378.137;
|
||
/**
|
||
* 地球每度的弧长(km)
|
||
* */
|
||
static public double EARTH_ARC = 111.199;
|
||
|
||
/**
|
||
* 转化为弧度(rad)
|
||
* */
|
||
public static double rad(double d)
|
||
{
|
||
return d * Math.PI / 180.0;
|
||
}
|
||
|
||
/**
|
||
* 求两经纬度距离
|
||
*
|
||
* @param lon1
|
||
* 第一点的经度
|
||
* @param lat1
|
||
* 第一点的纬度
|
||
* @param lon2
|
||
* 第二点的经度
|
||
* @param lat2
|
||
* 第二点的纬度
|
||
* @return 两点距离，单位km
|
||
* */
|
||
public static double GetDistanceOne(double lon1, double lat1, double lon2,
|
||
double lat2)
|
||
{
|
||
double r1 = rad(lat1);
|
||
double r2 = rad(lon1);
|
||
double a = rad(lat2);
|
||
double b = rad(lon2);
|
||
double s = Math.Acos(Math.Cos(r1) * Math.Cos(a) * Math.Cos(r2 - b)
|
||
+ Math.Sin(r1) * Math.Sin(a))
|
||
* EARTH_RADIUS;
|
||
return s;
|
||
}
|
||
|
||
public static void GetRotatePos(double Centlon, double Centlat,double Angle, double lon_in,
|
||
double lat_in, out double lon_out, out double lat_out)
|
||
{
|
||
|
||
double lpDistance = GetDistanceOne(Centlon, Centlat,
|
||
lon_in,
|
||
lat_in);
|
||
|
||
MyLatLng mypos1, mypos2;
|
||
mypos1 = new CalculationLogLatDistance.MyLatLng(Centlon, Centlat);
|
||
mypos2 = new CalculationLogLatDistance.MyLatLng(lon_in , lat_in);
|
||
double lpAzimuth = CalculationLogLatDistance.getAngle(mypos1, mypos2);
|
||
ConvertDistanceToLogLat(Centlon, Centlat, lpDistance,
|
||
lpAzimuth + Angle, out lon_out, out lat_out);
|
||
return ;
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
/**
|
||
* 求两经纬度距离(google maps源码中)
|
||
*
|
||
* @param lon1
|
||
* 第一点的经度
|
||
* @param lat1
|
||
* 第一点的纬度
|
||
* @param lon2
|
||
* 第二点的经度
|
||
* @param lat2
|
||
* 第二点的纬度
|
||
* @return 两点距离，单位km
|
||
* */
|
||
public static double GetDistanceTwo(double lon1, double lat1, double lon2,
|
||
double lat2)
|
||
{
|
||
double radLat1 = rad(lat1);
|
||
double radLat2 = rad(lat2);
|
||
double a = radLat1 - radLat2;
|
||
double b = rad(lon1) - rad(lon2);
|
||
double s = 2 * Math.Asin(Math.Sqrt(Math.Pow(Math.Sin(a / 2), 2)
|
||
+ Math.Cos(radLat1) * Math.Cos(radLat2)
|
||
* Math.Pow(Math.Sin(b / 2), 2)));
|
||
s = s * EARTH_RADIUS;
|
||
return s;
|
||
}
|
||
|
||
/**
|
||
* 求两经纬度距离
|
||
*
|
||
* @param lon1
|
||
* 第一点的经度
|
||
* @param lat1
|
||
* 第一点的纬度
|
||
* @param lon2
|
||
* 第二点的经度
|
||
* @param lat2
|
||
* 第二点的纬度
|
||
* @return 两点距离，单位km
|
||
* */
|
||
public static double GetDistanceThree(double lon1, double lat1,
|
||
double lon2, double lat2)
|
||
{
|
||
double radLat1 = rad(lat1);
|
||
double radLat2 = rad(lat2);
|
||
double radLon1 = rad(lon1);
|
||
double radLon2 = rad(lon2);
|
||
if (radLat1 < 0)
|
||
radLat1 = Math.PI / 2 + Math.Abs(radLat1);// south
|
||
if (radLat1 > 0)
|
||
radLat1 = Math.PI / 2 - Math.Abs(radLat1);// north
|
||
if (radLon1 < 0)
|
||
radLon1 = Math.PI * 2 - Math.Abs(radLon1);// west
|
||
if (radLat2 < 0)
|
||
radLat2 = Math.PI / 2 + Math.Abs(radLat2);// south
|
||
if (radLat2 > 0)
|
||
radLat2 = Math.PI / 2 - Math.Abs(radLat2);// north
|
||
if (radLon2 < 0)
|
||
radLon2 = Math.PI * 2 - Math.Abs(radLon2);// west
|
||
double x1 = Math.Cos(radLon1) * Math.Sin(radLat1);
|
||
double y1 = Math.Sin(radLon1) * Math.Sin(radLat1);
|
||
double z1 = Math.Cos(radLat1);
|
||
|
||
double x2 = Math.Cos(radLon2) * Math.Sin(radLat2);
|
||
double y2 = Math.Sin(radLon2) * Math.Sin(radLat2);
|
||
double z2 = Math.Cos(radLat2);
|
||
|
||
double d = Math.Pow((x1 - x2), 2) + Math.Pow((y1 - y2), 2)
|
||
+ Math.Pow((z1 - z2), 2);
|
||
// // 余弦定理求夹角
|
||
// double theta = Math.Acos((2 - d) / 2);
|
||
|
||
d = Math.Pow(EARTH_RADIUS, 2) * d;
|
||
// //余弦定理求夹角
|
||
double theta = Math.Acos((2 * Math.Pow(EARTH_RADIUS, 2) - d)
|
||
/ (2 * Math.Pow(EARTH_RADIUS, 2)));
|
||
|
||
double dist = theta * EARTH_RADIUS;
|
||
return dist;
|
||
}
|
||
|
||
/**
|
||
* 求两经纬度方向角
|
||
*
|
||
* @param lon1
|
||
* 第一点的经度
|
||
* @param lat1
|
||
* 第一点的纬度
|
||
* @param lon2
|
||
* 第二点的经度
|
||
* @param lat2
|
||
* 第二点的纬度
|
||
* @return 方位角，角度（单位：°）
|
||
* */
|
||
public static double GetAzimuth(double lon1, double lat1, double lon2,
|
||
double lat2)
|
||
{
|
||
lat1 = rad(lat1);
|
||
lat2 = rad(lat2);
|
||
lon1 = rad(lon1);
|
||
lon2 = rad(lon2);
|
||
double azimuth = Math.Sin(lat1) * Math.Sin(lat2) + Math.Cos(lat1)
|
||
* Math.Cos(lat2) * Math.Cos(lon2 - lon1);
|
||
azimuth = Math.Sqrt(1 - azimuth * azimuth);
|
||
azimuth = Math.Cos(lat2) * Math.Sin(lon2 - lon1) / azimuth;
|
||
azimuth = Math.Asin(azimuth) * 180 / Math.PI;
|
||
if (Double.IsNaN(azimuth))
|
||
{
|
||
if (lon1 < lon2)
|
||
{
|
||
azimuth = 90.0;
|
||
}
|
||
else {
|
||
azimuth = 270.0;
|
||
}
|
||
}
|
||
return azimuth;
|
||
}
|
||
|
||
/**
|
||
* 已知一点经纬度A，和与另一点B的距离和方位角，求B的经纬度(计算结果有误)
|
||
*
|
||
* @param lon1
|
||
* A的经度
|
||
* @param lat1
|
||
* A的纬度
|
||
* @param distance
|
||
* AB距离（单位：米）
|
||
* @param azimuth
|
||
* AB方位角
|
||
* @return B的经纬度
|
||
* */
|
||
public static void GetOtherPoint(double lon1, double lat1,
|
||
double distance, double azimuth, out double lng2, out double lat2)
|
||
{
|
||
azimuth = rad(azimuth);
|
||
double ab = distance / EARTH_ARC;// AB间弧线长
|
||
ab = rad(ab);
|
||
double Lat = Math.Asin(Math.Sin(lat1) * Math.Cos(ab) + Math.Cos(lat1)
|
||
* Math.Sin(ab) * Math.Cos(azimuth));
|
||
double Lon = lon1
|
||
+ Math.Asin(Math.Sin(azimuth) * Math.Sin(ab) / Math.Cos(Lat));
|
||
// System.out.println(Lon + "," + Lat);
|
||
|
||
double a = Math.Acos(Math.Cos(90 - lon1) * Math.Cos(ab)
|
||
+ Math.Sin(90 - lon1) * Math.Sin(ab) * Math.Cos(azimuth));
|
||
double C = Math.Asin(Math.Sin(ab) * Math.Sin(azimuth) / Math.Sin(a));
|
||
// System.out.println("c=" + C);
|
||
lng2 = lon1 + C;
|
||
lat2 = 90 - a;
|
||
return ;
|
||
}
|
||
|
||
/**
|
||
* 已知一点经纬度A，和与另一点B的距离和方位角，求B的经纬度
|
||
*
|
||
* @param lon1
|
||
* A的经度
|
||
* @param lat1
|
||
* A的纬度
|
||
* @param distance
|
||
* AB距离（单位：米）
|
||
* @param azimuth
|
||
* AB方位角
|
||
* @return B的经纬度
|
||
* */
|
||
public static void ConvertDistanceToLogLat(double lng1, double lat1,
|
||
double distance, double azimuth, out double lng2, out double lat2)
|
||
{
|
||
azimuth = rad(azimuth);
|
||
// 将距离转换成经度的计算公式
|
||
lng2 = lng1 + (distance * Math.Sin(azimuth))
|
||
/ (EARTH_ARC * Math.Cos(rad(lat1)));
|
||
// 将距离转换成纬度的计算公式
|
||
lat2 = lat1 + (distance * Math.Cos(azimuth)) / EARTH_ARC;
|
||
return ;
|
||
}
|
||
|
||
|
||
|
||
public static MyLatLng getMyLatLng(MyLatLng A, double distance, double angle)
|
||
{
|
||
|
||
double dx = distance * 1000 * Math.Sin((Math.PI / 180) * angle);
|
||
double dy = distance * 1000 * Math.Cos((Math.PI / 180) * angle);
|
||
|
||
double bjd = (dx / A.Ed + A.m_RadLo) * 180.0/ Math.PI;
|
||
double bwd = (dy / A.Ec + A.m_RadLa) * 180.0/ Math.PI;
|
||
return new MyLatLng(bjd, bwd);
|
||
}
|
||
|
||
|
||
/**
|
||
* 获取AB连线与正北方向的角度
|
||
* @param A A点的经纬度
|
||
* @param B B点的经纬度
|
||
* @return AB连线与正北方向的角度（0~360）
|
||
*/
|
||
public static double getAngle(MyLatLng A, MyLatLng B)
|
||
{
|
||
double dx = (B.m_RadLo - A.m_RadLo) * A.Ed;
|
||
double dy = (B.m_RadLa - A.m_RadLa) * A.Ec;
|
||
double angle = 0.0;
|
||
angle = Math.Atan(Math.Abs(dx / dy)) * 180.0/ Math.PI;
|
||
double dLo = B.m_Longitude - A.m_Longitude;
|
||
double dLa = B.m_Latitude - A.m_Latitude;
|
||
if (dLo > 0 && dLa <= 0)
|
||
{
|
||
angle = (90.0- angle) + 90;
|
||
}
|
||
else if (dLo <= 0 && dLa < 0)
|
||
{
|
||
angle = angle + 180.0;
|
||
}
|
||
else if (dLo < 0 && dLa >= 0)
|
||
{
|
||
angle = (90.0- angle) + 270;
|
||
}
|
||
if (Double.IsNaN(angle)) angle = 0.0;
|
||
|
||
return angle;
|
||
}
|
||
public class MyLatLng
|
||
{
|
||
static double Rc = 6378137;
|
||
static double Rj = 6356725;
|
||
public double m_LoDeg, m_LoMin, m_LoSec;
|
||
public double m_LaDeg, m_LaMin, m_LaSec;
|
||
public double m_Longitude, m_Latitude;
|
||
public double m_RadLo, m_RadLa;
|
||
public double Ec;
|
||
public double Ed;
|
||
public MyLatLng(double longitude, double latitude)
|
||
{
|
||
m_LoDeg = (int)longitude;
|
||
m_LoMin = (int)((longitude - m_LoDeg) * 60);
|
||
m_LoSec = (longitude - m_LoDeg - m_LoMin / 60.0) * 3600;
|
||
|
||
m_LaDeg = (int)latitude;
|
||
m_LaMin = (int)((latitude - m_LaDeg) * 60);
|
||
m_LaSec = (latitude - m_LaDeg - m_LaMin / 60.0) * 3600;
|
||
|
||
m_Longitude = longitude;
|
||
m_Latitude = latitude;
|
||
m_RadLo = longitude * Math.PI / 180.0;
|
||
m_RadLa = latitude * Math.PI / 180.0;
|
||
Ec = Rj + (Rc - Rj) * (90.0- m_Latitude) / 90.0;
|
||
Ed = Ec * Math.Cos(m_RadLa);
|
||
}
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
}
|