Subversion Repositories svnkaklik

Rev

Go to most recent revision | Details | Last modification | View Log

Rev Author Line No. Line
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 
 
66 
	return distance;
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