| 0,0 → 1,118 |
|---|
| /* Navigation code */ |
| |
| #include "geocalc.h" |
| |
| double GeoCalc::EllipsoidDistance(double lat1, double lon1, double lat2, double lon2) |
| { |
| double distance = 0.0; |
| double faz, baz; |
| double r = 1.0 - GEO::FLATTENING; |
| double tu1, tu2, cu1, su1, cu2, x, sx, cx, sy, cy, y, sa, c2a, cz, e, c, d; |
| double cosy1, cosy2; |
| distance = 0.0; |
| |
| if((lon1 == lon2) && (lat1 == lat2)) return distance; |
| lon1 *= GEO::DE2RA; |
| lon2 *= GEO::DE2RA; |
| lat1 *= GEO::DE2RA; |
| lat2 *= GEO::DE2RA; |
| |
| cosy1 = cos(lat1); |
| cosy2 = cos(lat2); |
| |
| if(cosy1 == 0.0) cosy1 = 0.0000000001; |
| if(cosy2 == 0.0) cosy2 = 0.0000000001; |
| |
| tu1 = r * sin(lat1) / cosy1; |
| tu2 = r * sin(lat2) / cosy2; |
| cu1 = 1.0 / sqrt(tu1 * tu1 + 1.0); |
| su1 = cu1 * tu1; |
| cu2 = 1.0 / sqrt(tu2 * tu2 + 1.0); |
| x = lon2 - lon1; |
| |
| distance = cu1 * cu2; |
| baz = distance * tu2; |
| faz = baz * tu1; |
| |
| do { |
| sx = sin(x); |
| cx = cos(x); |
| tu1 = cu2 * sx; |
| tu2 = baz - su1 * cu2 * cx; |
| sy = sqrt(tu1 * tu1 + tu2 * tu2); |
| cy = distance * cx + faz; |
| y = atan2(sy, cy); |
| sa = distance * sx / sy; |
| c2a = -sa * sa + 1.0; |
| cz = faz + faz; |
| if(c2a > 0.0) cz = -cz / c2a + cy; |
| e = cz * cz * 2. - 1.0; |
| c = ((-3.0 * c2a + 4.0) * GEO::FLATTENING + 4.0) * c2a * GEO::FLATTENING / 16.0; |
| d = x; |
| x = ((e * cy * c + cz) * sy * c + y) * sa; |
| x = (1.0 - c) * x * GEO::FLATTENING + lon2 - lon1; |
| } while(fabs(d - x) > GEO::EPS); |
| |
| x = sqrt((1.0 / r / r - 1.0) * c2a + 1.0) + 1.0; |
| x = (x - 2.0) / x; |
| c = 1.0 - x; |
| c = (x * x / 4.0 + 1.0) / c; |
| d = (0.375 * x * x - 1.0) * x; |
| x = e * cy; |
| distance = 1.0 - e - e; |
| distance = ((((sy * sy * 4.0 - 3.0) * |
| distance * cz * d / 6.0 - x) * d / 4.0 + cz) * sy * d + y) * c * GEO::ERAD * r; |
| |
| return distance; |
| } |
| |
| |
| double GeoCalc::GCAzimuth(double lat1, double lon1, double lat2, double lon2) |
| { |
| double result = 0.0; |
| |
| INT32 ilat1 = (INT32)(0.50 + lat1 * 360000.0); |
| INT32 ilat2 = (INT32)(0.50 + lat2 * 360000.0); |
| INT32 ilon1 = (INT32)(0.50 + lon1 * 360000.0); |
| INT32 ilon2 = (INT32)(0.50 + lon2 * 360000.0); |
| |
| lat1 *= GEO::DE2RA; |
| lon1 *= GEO::DE2RA; |
| lat2 *= GEO::DE2RA; |
| lon2 *= GEO::DE2RA; |
| |
| if ((ilat1 == ilat2) && (ilon1 == ilon2)) |
| { |
| return result; |
| } |
| else if (ilon1 == ilon2) |
| { |
| if (ilat1 > ilat2) |
| result = 180.0; |
| } |
| else |
| { |
| double c = acos(sin(lat2)*sin(lat1) + cos(lat2)*cos(lat1)*cos((lon2-lon1))); |
| double A = asin(cos(lat2)*sin((lon2-lon1))/sin(c)); |
| result = (A * GEO::RA2DE); |
| |
| if ((ilat2 > ilat1) && (ilon2 > ilon1)) |
| { |
| } |
| else if ((ilat2 < ilat1) && (ilon2 < ilon1)) |
| { |
| result = 180.0 - result; |
| } |
| else if ((ilat2 < ilat1) && (ilon2 > ilon1)) |
| { |
| result = 180.0 - result; |
| } |
| else if ((ilat2 > ilat1) && (ilon2 < ilon1)) |
| { |
| result += 360.0; |
| } |
| } |
| |
| return result; |
| } |
| |