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 namespace GeographicLib {
00032
00033 using namespace std;
00034
00035 GeodesicLine::GeodesicLine(const Geodesic& g,
00036 real lat1, real lon1, real azi1,
00037 unsigned caps)
00038 : tiny_(g.tiny_)
00039 , _a(g._a)
00040 , _f(g._f)
00041 , _b(g._b)
00042 , _c2(g._c2)
00043 , _f1(g._f1)
00044
00045 , _caps(caps | LATITUDE | AZIMUTH)
00046 {
00047
00048 azi1 = Geodesic::AngRound(Math::AngNormalize(azi1));
00049 lon1 = Math::AngNormalize(lon1);
00050 _lat1 = lat1;
00051 _lon1 = lon1;
00052 _azi1 = azi1;
00053
00054 real alp1 = azi1 * Math::degree();
00055
00056
00057 _salp1 = azi1 == -180 ? 0 : sin(alp1);
00058 _calp1 = abs(azi1) == 90 ? 0 : cos(alp1);
00059 real cbet1, sbet1, phi;
00060 phi = lat1 * Math::degree();
00061
00062 sbet1 = _f1 * sin(phi);
00063 cbet1 = abs(lat1) == 90 ? tiny_ : cos(phi);
00064 Geodesic::SinCosNorm(sbet1, cbet1);
00065 _dn1 = sqrt(1 + g._ep2 * Math::sq(sbet1));
00066
00067
00068 _salp0 = _salp1 * cbet1;
00069
00070
00071 _calp0 = Math::hypot(_calp1, _salp1 * sbet1);
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081 _ssig1 = sbet1; _somg1 = _salp0 * sbet1;
00082 _csig1 = _comg1 = sbet1 != 0 || _calp1 != 0 ? cbet1 * _calp1 : 1;
00083 Geodesic::SinCosNorm(_ssig1, _csig1);
00084
00085
00086 _k2 = Math::sq(_calp0) * g._ep2;
00087 real eps = _k2 / (2 * (1 + sqrt(1 + _k2)) + _k2);
00088
00089 if (_caps & CAP_C1) {
00090 _A1m1 = Geodesic::A1m1f(eps);
00091 Geodesic::C1f(eps, _C1a);
00092 _B11 = Geodesic::SinCosSeries(true, _ssig1, _csig1, _C1a, nC1_);
00093 real s = sin(_B11), c = cos(_B11);
00094
00095 _stau1 = _ssig1 * c + _csig1 * s;
00096 _ctau1 = _csig1 * c - _ssig1 * s;
00097
00098
00099 }
00100
00101 if (_caps & CAP_C1p)
00102 Geodesic::C1pf(eps, _C1pa);
00103
00104 if (_caps & CAP_C2) {
00105 _A2m1 = Geodesic::A2m1f(eps);
00106 Geodesic::C2f(eps, _C2a);
00107 _B21 = Geodesic::SinCosSeries(true, _ssig1, _csig1, _C2a, nC2_);
00108 }
00109
00110 if (_caps & CAP_C3) {
00111 g.C3f(eps, _C3a);
00112 _A3c = -_f * _salp0 * g.A3f(eps);
00113 _B31 = Geodesic::SinCosSeries(true, _ssig1, _csig1, _C3a, nC3_-1);
00114 }
00115
00116 if (_caps & CAP_C4) {
00117 g.C4f(eps, _C4a);
00118
00119 _A4 = Math::sq(_a) * _calp0 * _salp0 * g._e2;
00120 _B41 = Geodesic::SinCosSeries(false, _ssig1, _csig1, _C4a, nC4_);
00121 }
00122 }
00123
00124 Math::real GeodesicLine::GenPosition(bool arcmode, real s12_a12,
00125 unsigned outmask,
00126 real& lat2, real& lon2, real& azi2,
00127 real& s12, real& m12,
00128 real& M12, real& M21,
00129 real& S12)
00130 const {
00131 outmask &= _caps & OUT_ALL;
00132 if (!( Init() && (arcmode || (_caps & DISTANCE_IN & OUT_ALL)) ))
00133
00134 return Math::NaN();
00135
00136
00137 real sig12, ssig12, csig12, B12 = 0, AB1 = 0;
00138 if (arcmode) {
00139
00140 sig12 = s12_a12 * Math::degree();
00141 real s12a = abs(s12_a12);
00142 s12a -= 180 * floor(s12a / 180);
00143 ssig12 = s12a == 0 ? 0 : sin(sig12);
00144 csig12 = s12a == 90 ? 0 : cos(sig12);
00145 } else {
00146
00147 real
00148 tau12 = s12_a12 / (_b * (1 + _A1m1)),
00149 s = sin(tau12),
00150 c = cos(tau12);
00151
00152 B12 = - Geodesic::SinCosSeries(true,
00153 _stau1 * c + _ctau1 * s,
00154 _ctau1 * c - _stau1 * s,
00155 _C1pa, nC1p_);
00156 sig12 = tau12 - (B12 - _B11);
00157 ssig12 = sin(sig12); csig12 = cos(sig12);
00158 if (abs(_f) > 0.01) {
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180 real
00181 ssig2 = _ssig1 * csig12 + _csig1 * ssig12,
00182 csig2 = _csig1 * csig12 - _ssig1 * ssig12;
00183 B12 = Geodesic::SinCosSeries(true, ssig2, csig2, _C1a, nC1_);
00184 real serr = (1 + _A1m1) * (sig12 + (B12 - _B11)) - s12_a12 / _b;
00185 sig12 = sig12 - serr / sqrt(1 + _k2 * Math::sq(ssig2));
00186 ssig12 = sin(sig12); csig12 = cos(sig12);
00187
00188 }
00189 }
00190
00191 real omg12, lam12, lon12;
00192 real ssig2, csig2, sbet2, cbet2, somg2, comg2, salp2, calp2;
00193
00194 ssig2 = _ssig1 * csig12 + _csig1 * ssig12;
00195 csig2 = _csig1 * csig12 - _ssig1 * ssig12;
00196 real dn2 = sqrt(1 + _k2 * Math::sq(ssig2));
00197 if (outmask & (DISTANCE | REDUCEDLENGTH | GEODESICSCALE)) {
00198 if (arcmode || abs(_f) > 0.01)
00199 B12 = Geodesic::SinCosSeries(true, ssig2, csig2, _C1a, nC1_);
00200 AB1 = (1 + _A1m1) * (B12 - _B11);
00201 }
00202
00203 sbet2 = _calp0 * ssig2;
00204
00205 cbet2 = Math::hypot(_salp0, _calp0 * csig2);
00206 if (cbet2 == 0)
00207
00208 cbet2 = csig2 = tiny_;
00209
00210 somg2 = _salp0 * ssig2; comg2 = csig2;
00211
00212 salp2 = _salp0; calp2 = _calp0 * csig2;
00213
00214 omg12 = atan2(somg2 * _comg1 - comg2 * _somg1,
00215 comg2 * _comg1 + somg2 * _somg1);
00216
00217 if (outmask & DISTANCE)
00218 s12 = arcmode ? _b * ((1 + _A1m1) * sig12 + AB1) : s12_a12;
00219
00220 if (outmask & LONGITUDE) {
00221 lam12 = omg12 + _A3c *
00222 ( sig12 + (Geodesic::SinCosSeries(true, ssig2, csig2, _C3a, nC3_-1)
00223 - _B31));
00224 lon12 = lam12 / Math::degree();
00225
00226
00227 lon12 = Math::AngNormalize2(lon12);
00228 lon2 = Math::AngNormalize(_lon1 + lon12);
00229 }
00230
00231 if (outmask & LATITUDE)
00232 lat2 = atan2(sbet2, _f1 * cbet2) / Math::degree();
00233
00234 if (outmask & AZIMUTH)
00235
00236 azi2 = 0 - atan2(-salp2, calp2) / Math::degree();
00237
00238 if (outmask & (REDUCEDLENGTH | GEODESICSCALE)) {
00239 real
00240 B22 = Geodesic::SinCosSeries(true, ssig2, csig2, _C2a, nC2_),
00241 AB2 = (1 + _A2m1) * (B22 - _B21),
00242 J12 = (_A1m1 - _A2m1) * sig12 + (AB1 - AB2);
00243 if (outmask & REDUCEDLENGTH)
00244
00245
00246 m12 = _b * ((dn2 * (_csig1 * ssig2) - _dn1 * (_ssig1 * csig2))
00247 - _csig1 * csig2 * J12);
00248 if (outmask & GEODESICSCALE) {
00249 real t = _k2 * (ssig2 - _ssig1) * (ssig2 + _ssig1) / (_dn1 + dn2);
00250 M12 = csig12 + (t * ssig2 - csig2 * J12) * _ssig1 / _dn1;
00251 M21 = csig12 - (t * _ssig1 - _csig1 * J12) * ssig2 / dn2;
00252 }
00253 }
00254
00255 if (outmask & AREA) {
00256 real
00257 B42 = Geodesic::SinCosSeries(false, ssig2, csig2, _C4a, nC4_);
00258 real salp12, calp12;
00259 if (_calp0 == 0 || _salp0 == 0) {
00260
00261 salp12 = salp2 * _calp1 - calp2 * _salp1;
00262 calp12 = calp2 * _calp1 + salp2 * _salp1;
00263
00264
00265
00266 if (salp12 == 0 && calp12 < 0) {
00267 salp12 = tiny_ * _calp1;
00268 calp12 = -1;
00269 }
00270 } else {
00271
00272
00273
00274
00275
00276
00277
00278
00279 salp12 = _calp0 * _salp0 *
00280 (csig12 <= 0 ? _csig1 * (1 - csig12) + ssig12 * _ssig1 :
00281 ssig12 * (_csig1 * ssig12 / (1 + csig12) + _ssig1));
00282 calp12 = Math::sq(_salp0) + Math::sq(_calp0) * _csig1 * csig2;
00283 }
00284 S12 = _c2 * atan2(salp12, calp12) + _A4 * (B42 - _B41);
00285 }
00286
00287 return arcmode ? s12_a12 : sig12 / Math::degree();
00288 }
00289
00290 }