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); } } } }