00001 #include "gis.h"
00002 #include "pi.h"
00003
00004
00005
00006 double
00007 G_ellipsoid_grid_dist (a, e2, lon1, lat1, lon2, lat2)
00008 double a, e2, lon1, lat1, lon2, lat2;
00009 {
00010 double cos1, cos2, sin1, sin2;
00011 double q, q2, m, m2;
00012 double A,B,C,D;
00013 double Ap, Bp, Cp, Dp;
00014 double distance;
00015 double e4, e6;
00016 double sin(), cos(), sqrt();
00017 double sc();
00018
00019
00020 if (lon1 > lon2)
00021 while ((lon1-lon2) > 180)
00022 lon2 += 360;
00023 else if (lon2 > lon1)
00024 while ((lon2-lon1) > 180)
00025 lon1 += 360;
00026
00027 lon1 = Radians(lon1);
00028 lon2 = Radians(lon2);
00029 lat1 = Radians(lat1);
00030 lat2 = Radians(lat2);
00031
00032 sin1 = sin(lat1);
00033 cos1 = cos(lon1);
00034 sin2 = sin(lat2);
00035 cos2 = cos(lon2);
00036
00037 e4 = e2 * e2;
00038 e6 = e2 * e4;
00039
00040
00041 if (lat1 == lat2)
00042 {
00043 distance = a * cos1 * (lon2 - lon1) / sqrt(1 - e2 * sin1 * sin1);
00044 }
00045
00046
00047 else if (lon1 == lon2)
00048 {
00049 A = 1 + e2 * (3.0/4.0) + e4 * (45.0/64.0) + e6 * (175.0/256.0);
00050 B = - e2 * (3.0/4.0) - e4 * (45.0/64.0) - e6 * (175.0/256.0);
00051 C = - e4 * (15.0/32.0) - e6 * (175.0/384.0);
00052 D = - e6 * (35.0/96.0);
00053
00054 distance = a * (1-e2) * (sc(A,B,C,D,lat2,0.,0.)-sc(A,B,C,D,lat1,0.,0.));
00055 }
00056
00057
00058 else
00059 {
00060 q = 1 - e2;
00061 q2 = q*q;
00062
00063 m = (lat2-lat1)/(lon2-lon1);
00064 m2 = m*m;
00065
00066 A = sqrt(q2 + m2);
00067 B = (3.0*e2*q2 - q*m2)/(2.0*A);
00068 C = (6.0*e4*q2 - e2*q*m2 - B*B)/(2.0*A);
00069 D = (10.0*e6*q2 - e4*q*m2 - 2.0*B*C)/(2.0*A);
00070
00071 Ap = A + B/2.0 + 3.0*C/8.0 + 5.0*D/16.0;
00072 Bp = - B/2.0 - 3.0*C/8.0 - 5.0*D/16.0;
00073 Cp = - C/4.0 - 5.0*D/24.0;
00074 Dp = - D/6.0;
00075
00076 distance = a *
00077 (sc(Ap,Bp,Cp,Dp,lat2,sin2,cos2)-sc(Ap,Bp,Cp,Dp,lat1,sin1,cos1));
00078 }
00079
00080 if (distance < 0.0)
00081 distance = -distance;
00082 return distance;
00083 }
00084
00085 static double sc(A,B,C,D,lat,s,c)
00086 double A,B,C,D,lat,s,c;
00087 {
00088 double s2;
00089
00090 s2 = s*s;
00091
00092 return A * lat + c*s*(B + s2 * (C + s2*D));
00093 }