| 365 |
kakl |
1 |
/* Navigation code */
|
|
|
2 |
|
|
|
3 |
#include "geocalc.h"
|
|
|
4 |
|
|
|
5 |
double GeoCalc::EllipsoidDistance(double lat1, double lon1, double lat2, double lon2)
|
|
|
6 |
{
|
|
|
7 |
double distance = 0.0;
|
|
|
8 |
double faz, baz;
|
|
|
9 |
double r = 1.0 - GEO::FLATTENING;
|
|
|
10 |
double tu1, tu2, cu1, su1, cu2, x, sx, cx, sy, cy, y, sa, c2a, cz, e, c, d;
|
|
|
11 |
double cosy1, cosy2;
|
|
|
12 |
distance = 0.0;
|
|
|
13 |
|
|
|
14 |
if((lon1 == lon2) && (lat1 == lat2)) return distance;
|
|
|
15 |
lon1 *= GEO::DE2RA;
|
|
|
16 |
lon2 *= GEO::DE2RA;
|
|
|
17 |
lat1 *= GEO::DE2RA;
|
|
|
18 |
lat2 *= GEO::DE2RA;
|
|
|
19 |
|
|
|
20 |
cosy1 = cos(lat1);
|
|
|
21 |
cosy2 = cos(lat2);
|
|
|
22 |
|
|
|
23 |
if(cosy1 == 0.0) cosy1 = 0.0000000001;
|
|
|
24 |
if(cosy2 == 0.0) cosy2 = 0.0000000001;
|
|
|
25 |
|
|
|
26 |
tu1 = r * sin(lat1) / cosy1;
|
|
|
27 |
tu2 = r * sin(lat2) / cosy2;
|
|
|
28 |
cu1 = 1.0 / sqrt(tu1 * tu1 + 1.0);
|
|
|
29 |
su1 = cu1 * tu1;
|
|
|
30 |
cu2 = 1.0 / sqrt(tu2 * tu2 + 1.0);
|
|
|
31 |
x = lon2 - lon1;
|
|
|
32 |
|
|
|
33 |
distance = cu1 * cu2;
|
|
|
34 |
baz = distance * tu2;
|
|
|
35 |
faz = baz * tu1;
|
|
|
36 |
|
|
|
37 |
do {
|
|
|
38 |
sx = sin(x);
|
|
|
39 |
cx = cos(x);
|
|
|
40 |
tu1 = cu2 * sx;
|
|
|
41 |
tu2 = baz - su1 * cu2 * cx;
|
|
|
42 |
sy = sqrt(tu1 * tu1 + tu2 * tu2);
|
|
|
43 |
cy = distance * cx + faz;
|
|
|
44 |
y = atan2(sy, cy);
|
|
|
45 |
sa = distance * sx / sy;
|
|
|
46 |
c2a = -sa * sa + 1.0;
|
|
|
47 |
cz = faz + faz;
|
|
|
48 |
if(c2a > 0.0) cz = -cz / c2a + cy;
|
|
|
49 |
e = cz * cz * 2. - 1.0;
|
|
|
50 |
c = ((-3.0 * c2a + 4.0) * GEO::FLATTENING + 4.0) * c2a * GEO::FLATTENING / 16.0;
|
|
|
51 |
d = x;
|
|
|
52 |
x = ((e * cy * c + cz) * sy * c + y) * sa;
|
|
|
53 |
x = (1.0 - c) * x * GEO::FLATTENING + lon2 - lon1;
|
|
|
54 |
} while(fabs(d - x) > GEO::EPS);
|
|
|
55 |
|
|
|
56 |
x = sqrt((1.0 / r / r - 1.0) * c2a + 1.0) + 1.0;
|
|
|
57 |
x = (x - 2.0) / x;
|
|
|
58 |
c = 1.0 - x;
|
|
|
59 |
c = (x * x / 4.0 + 1.0) / c;
|
|
|
60 |
d = (0.375 * x * x - 1.0) * x;
|
|
|
61 |
x = e * cy;
|
|
|
62 |
distance = 1.0 - e - e;
|
|
|
63 |
distance = ((((sy * sy * 4.0 - 3.0) *
|
|
|
64 |
distance * cz * d / 6.0 - x) * d / 4.0 + cz) * sy * d + y) * c * GEO::ERAD * r;
|
|
|
65 |
|
| 367 |
kakl |
66 |
return distance*1000;
|
| 365 |
kakl |
67 |
}
|
|
|
68 |
|
|
|
69 |
|
|
|
70 |
double GeoCalc::GCAzimuth(double lat1, double lon1, double lat2, double lon2)
|
|
|
71 |
{
|
|
|
72 |
double result = 0.0;
|
|
|
73 |
|
|
|
74 |
INT32 ilat1 = (INT32)(0.50 + lat1 * 360000.0);
|
|
|
75 |
INT32 ilat2 = (INT32)(0.50 + lat2 * 360000.0);
|
|
|
76 |
INT32 ilon1 = (INT32)(0.50 + lon1 * 360000.0);
|
|
|
77 |
INT32 ilon2 = (INT32)(0.50 + lon2 * 360000.0);
|
|
|
78 |
|
|
|
79 |
lat1 *= GEO::DE2RA;
|
|
|
80 |
lon1 *= GEO::DE2RA;
|
|
|
81 |
lat2 *= GEO::DE2RA;
|
|
|
82 |
lon2 *= GEO::DE2RA;
|
|
|
83 |
|
|
|
84 |
if ((ilat1 == ilat2) && (ilon1 == ilon2))
|
|
|
85 |
{
|
|
|
86 |
return result;
|
|
|
87 |
}
|
|
|
88 |
else if (ilon1 == ilon2)
|
|
|
89 |
{
|
|
|
90 |
if (ilat1 > ilat2)
|
|
|
91 |
result = 180.0;
|
|
|
92 |
}
|
|
|
93 |
else
|
|
|
94 |
{
|
|
|
95 |
double c = acos(sin(lat2)*sin(lat1) + cos(lat2)*cos(lat1)*cos((lon2-lon1)));
|
|
|
96 |
double A = asin(cos(lat2)*sin((lon2-lon1))/sin(c));
|
|
|
97 |
result = (A * GEO::RA2DE);
|
|
|
98 |
|
|
|
99 |
if ((ilat2 > ilat1) && (ilon2 > ilon1))
|
|
|
100 |
{
|
|
|
101 |
}
|
|
|
102 |
else if ((ilat2 < ilat1) && (ilon2 < ilon1))
|
|
|
103 |
{
|
|
|
104 |
result = 180.0 - result;
|
|
|
105 |
}
|
|
|
106 |
else if ((ilat2 < ilat1) && (ilon2 > ilon1))
|
|
|
107 |
{
|
|
|
108 |
result = 180.0 - result;
|
|
|
109 |
}
|
|
|
110 |
else if ((ilat2 > ilat1) && (ilon2 < ilon1))
|
|
|
111 |
{
|
|
|
112 |
result += 360.0;
|
|
|
113 |
}
|
|
|
114 |
}
|
|
|
115 |
|
|
|
116 |
return result;
|
|
|
117 |
}
|
|
|
118 |
|