00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #include "GeographicLib/GeodesicLine.hpp"
00030
00031 #define GEOGRAPHICLIB_GEODESICLINE_CPP "$Id: GeodesicLine.cpp 6921 2010-12-31 14:34:50Z karney $"
00032
00033 RCSID_DECL(GEOGRAPHICLIB_GEODESICLINE_CPP)
00034 RCSID_DECL(GEOGRAPHICLIB_GEODESICLINE_HPP)
00035
00036 namespace GeographicLib {
00037
00038 using namespace std;
00039
00040 GeodesicLine::GeodesicLine(const Geodesic& g,
00041 real lat1, real lon1, real azi1,
00042 unsigned caps) throw()
00043 : _a(g._a)
00044 , _r(g._r)
00045 , _b(g._b)
00046 , _c2(g._c2)
00047 , _f1(g._f1)
00048
00049 , _caps(caps | LATITUDE | AZIMUTH)
00050 {
00051 azi1 = Geodesic::AngNormalize(azi1);
00052
00053 azi1 = Geodesic::AngRound(azi1);
00054 lon1 = Geodesic::AngNormalize(lon1);
00055 _lat1 = lat1;
00056 _lon1 = lon1;
00057 _azi1 = azi1;
00058
00059 real alp1 = azi1 * Math::degree<real>();
00060
00061
00062 _salp1 = azi1 == -180 ? 0 : sin(alp1);
00063 _calp1 = abs(azi1) == 90 ? 0 : cos(alp1);
00064 real cbet1, sbet1, phi;
00065 phi = lat1 * Math::degree<real>();
00066
00067 sbet1 = _f1 * sin(phi);
00068 cbet1 = abs(lat1) == 90 ? Geodesic::eps2 : cos(phi);
00069 Geodesic::SinCosNorm(sbet1, cbet1);
00070
00071
00072 _salp0 = _salp1 * cbet1;
00073
00074
00075 _calp0 = Math::hypot(_calp1, _salp1 * sbet1);
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085 _ssig1 = sbet1; _somg1 = _salp0 * sbet1;
00086 _csig1 = _comg1 = sbet1 != 0 || _calp1 != 0 ? cbet1 * _calp1 : 1;
00087 Geodesic::SinCosNorm(_ssig1, _csig1);
00088 Geodesic::SinCosNorm(_somg1, _comg1);
00089
00090 _k2 = sq(_calp0) * g._ep2;
00091 real eps = _k2 / (2 * (1 + sqrt(1 + _k2)) + _k2);
00092
00093 if (_caps & CAP_C1) {
00094 _A1m1 = Geodesic::A1m1f(eps);
00095 Geodesic::C1f(eps, _C1a);
00096 _B11 = Geodesic::SinCosSeries(true, _ssig1, _csig1, _C1a, nC1);
00097 real s = sin(_B11), c = cos(_B11);
00098
00099 _stau1 = _ssig1 * c + _csig1 * s;
00100 _ctau1 = _csig1 * c - _ssig1 * s;
00101
00102
00103 }
00104
00105 if (_caps & CAP_C1p)
00106 Geodesic::C1pf(eps, _C1pa);
00107
00108 if (_caps & CAP_C2) {
00109 _A2m1 = Geodesic::A2m1f(eps);
00110 Geodesic::C2f(eps, _C2a);
00111 _B21 = Geodesic::SinCosSeries(true, _ssig1, _csig1, _C2a, nC2);
00112 }
00113
00114 if (_caps & CAP_C3) {
00115 g.C3f(eps, _C3a);
00116 _A3c = -g._f * _salp0 * g.A3f(eps);
00117 _B31 = Geodesic::SinCosSeries(true, _ssig1, _csig1, _C3a, nC3-1);
00118 }
00119
00120 if (_caps & CAP_C4) {
00121 g.C4f(_k2, _C4a);
00122
00123 _A4 = sq(g._a) * _calp0 * _salp0 * g._e2;
00124 _B41 = Geodesic::SinCosSeries(false, _ssig1, _csig1, _C4a, nC4);
00125 }
00126 }
00127
00128 Math::real GeodesicLine::GenPosition(bool arcmode, real s12_a12,
00129 unsigned outmask,
00130 real& lat2, real& lon2, real& azi2,
00131 real& s12, real& m12,
00132 real& M12, real& M21,
00133 real& S12)
00134 const throw() {
00135 outmask &= _caps & OUT_ALL;
00136 if (!( Init() && (arcmode || (_caps & DISTANCE_IN & OUT_ALL)) ))
00137
00138 return Math::NaN();
00139
00140
00141 real sig12, ssig12, csig12, B12 = 0, AB1 = 0;
00142 if (arcmode) {
00143
00144 sig12 = s12_a12 * Math::degree<real>();
00145 real s12a = abs(s12_a12);
00146 s12a -= 180 * floor(s12a / 180);
00147 ssig12 = s12a == 0 ? 0 : sin(sig12);
00148 csig12 = s12a == 90 ? 0 : cos(sig12);
00149 } else {
00150
00151 real
00152 tau12 = s12_a12 / (_b * (1 + _A1m1)),
00153 s = sin(tau12),
00154 c = cos(tau12);
00155
00156 B12 = - Geodesic::SinCosSeries(true, _stau1 * c + _ctau1 * s,
00157 _ctau1 * c - _stau1 * s,
00158 _C1pa, nC1p);
00159 sig12 = tau12 - (B12 - _B11);
00160 ssig12 = sin(sig12);
00161 csig12 = cos(sig12);
00162 }
00163
00164 real omg12, lam12, lon12;
00165 real ssig2, csig2, sbet2, cbet2, somg2, comg2, salp2, calp2;
00166
00167 ssig2 = _ssig1 * csig12 + _csig1 * ssig12;
00168 csig2 = _csig1 * csig12 - _ssig1 * ssig12;
00169 if (outmask & (DISTANCE | REDUCEDLENGTH | GEODESICSCALE)) {
00170 if (arcmode)
00171 B12 = Geodesic::SinCosSeries(true, ssig2, csig2, _C1a, nC1);
00172 AB1 = (1 + _A1m1) * (B12 - _B11);
00173 }
00174
00175 sbet2 = _calp0 * ssig2;
00176
00177 cbet2 = Math::hypot(_salp0, _calp0 * csig2);
00178 if (cbet2 == 0)
00179
00180 cbet2 = csig2 = Geodesic::eps2;
00181
00182 somg2 = _salp0 * ssig2; comg2 = csig2;
00183
00184 salp2 = _salp0; calp2 = _calp0 * csig2;
00185
00186 omg12 = atan2(somg2 * _comg1 - comg2 * _somg1,
00187 comg2 * _comg1 + somg2 * _somg1);
00188
00189 if (outmask & DISTANCE)
00190 s12 = arcmode ? _b * ((1 + _A1m1) * sig12 + AB1) : s12_a12;
00191
00192 if (outmask & LONGITUDE) {
00193 lam12 = omg12 + _A3c *
00194 ( sig12 + (Geodesic::SinCosSeries(true, ssig2, csig2, _C3a, nC3-1)
00195 - _B31));
00196 lon12 = lam12 / Math::degree<real>();
00197
00198
00199 lon12 = lon12 - 360 * floor(lon12/360 + real(0.5));
00200 lon2 = Geodesic::AngNormalize(_lon1 + lon12);
00201 }
00202
00203 if (outmask & LATITUDE)
00204 lat2 = atan2(sbet2, _f1 * cbet2) / Math::degree<real>();
00205
00206 if (outmask & AZIMUTH)
00207
00208 azi2 = 0 - atan2(-salp2, calp2) / Math::degree<real>();
00209
00210 if (outmask & (REDUCEDLENGTH | GEODESICSCALE)) {
00211 real
00212 ssig1sq = sq(_ssig1),
00213 ssig2sq = sq( ssig2),
00214 w1 = sqrt(1 + _k2 * ssig1sq),
00215 w2 = sqrt(1 + _k2 * ssig2sq),
00216 B22 = Geodesic::SinCosSeries(true, ssig2, csig2, _C2a, nC2),
00217 AB2 = (1 + _A2m1) * (B22 - _B21),
00218 J12 = (_A1m1 - _A2m1) * sig12 + (AB1 - AB2);
00219 if (outmask & REDUCEDLENGTH)
00220
00221
00222 m12 = _b * ((w2 * (_csig1 * ssig2) - w1 * (_ssig1 * csig2))
00223 - _csig1 * csig2 * J12);
00224 if (outmask & GEODESICSCALE) {
00225 M12 = csig12 + (_k2 * (ssig2sq - ssig1sq) * ssig2 / (w1 + w2)
00226 - csig2 * J12) * _ssig1 / w1;
00227 M21 = csig12 - (_k2 * (ssig2sq - ssig1sq) * _ssig1 / (w1 + w2)
00228 - _csig1 * J12) * ssig2 / w2;
00229 }
00230 }
00231
00232 if (outmask & AREA) {
00233 real
00234 B42 = Geodesic::SinCosSeries(false, ssig2, csig2, _C4a, nC4);
00235 real salp12, calp12;
00236 if (_calp0 == 0 || _salp0 == 0) {
00237
00238 salp12 = salp2 * _calp1 - calp2 * _salp1;
00239 calp12 = calp2 * _calp1 + salp2 * _salp1;
00240
00241
00242
00243 if (salp12 == 0 && calp12 < 0) {
00244 salp12 = Geodesic::eps2 * _calp1;
00245 calp12 = -1;
00246 }
00247 } else {
00248
00249
00250
00251
00252
00253
00254
00255
00256 salp12 = _calp0 * _salp0 *
00257 (csig12 <= 0 ? _csig1 * (1 - csig12) + ssig12 * _ssig1 :
00258 ssig12 * (_csig1 * ssig12 / (1 + csig12) + _ssig1));
00259 calp12 = sq(_salp0) + sq(_calp0) * _csig1 * csig2;
00260 }
00261 S12 = _c2 * atan2(salp12, calp12) + _A4 * (B42 - _B41);
00262 }
00263
00264 return arcmode ? s12_a12 : sig12 / Math::degree<real>();
00265 }
00266 }
00267