ReferenceFrames.cpp

Go to the documentation of this file.
00001 #pragma ident "$Id: ReferenceFrames.cpp 3140 2012-06-18 15:03:02Z susancummins $"
00002 
00008 //============================================================================
00009 //
00010 //  This file is part of GPSTk, the GPS Toolkit.
00011 //
00012 //  The GPSTk is free software; you can redistribute it and/or modify
00013 //  it under the terms of the GNU Lesser General Public License as published
00014 //  by the Free Software Foundation; either version 2.1 of the License, or
00015 //  any later version.
00016 //
00017 //  The GPSTk is distributed in the hope that it will be useful,
00018 //  but WITHOUT ANY WARRANTY; without even the implied warranty of
00019 //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00020 //  GNU Lesser General Public License for more details.
00021 //
00022 //  You should have received a copy of the GNU Lesser General Public
00023 //  License along with GPSTk; if not, write to the Free Software Foundation,
00024 //  Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
00025 //
00026 //  Wei Yan - Chinese Academy of Sciences . 2009, 2010
00027 //
00028 //============================================================================
00029 
00030 #include "ReferenceFrames.hpp"
00031 #include <iostream>
00032 #include <string>
00033 #include <vector>
00034 #include <cmath>
00035 #include "StringUtils.hpp"
00036 #include "IERS.hpp"
00037 #include "ASConstant.hpp"
00038 
00039 
00040 namespace gpstk
00041 {
00042    using namespace std;
00043       
00044       // Objects to handle JPL ephemeris 405 
00045    SolarSystem ReferenceFrames::solarPlanets;
00046 
00047       // Reference epoch (J2000), Julian Date
00048    const double ReferenceFrames::DJ00 = 2451545.0;
00049 
00050       // Conversion offset, Julian Date to Modified Julian Date.
00051    const double ReferenceFrames::JD_TO_MJD = 2400000.5;
00052 
00053       // 2PI
00054    const double ReferenceFrames::D2PI = 6.283185307179586476925287;
00055 
00056       // Days per Julian century 
00057    const double ReferenceFrames::DJC = 36525.0;
00058 
00059       // Arcseconds to radians 
00060    const double ReferenceFrames::DAS2R = 4.848136811095359935899141e-6;
00061       
00062       // seconds to radians
00063    const double ReferenceFrames::DS2R = 7.272205216643039903848712e-5;
00064 
00065       // Arcseconds in a full circle 
00066    const double ReferenceFrames::TURNAS = 1296000.0;
00067 
00068 
00069       /* Compute planet position in J2000
00070        *  
00071        * @param TT         Time(Modified Julian Date in TT<TAI+32.184>) of interest 
00072        * @param entity     The planet to be computed
00073        * @return           The position of the planet in km
00074        */
00075    Vector<double> ReferenceFrames::getJ2kPosition( const CommonTime&      TT,
00076                                                    SolarSystem::Planet entity)
00077       throw(Exception)
00078    {
00079       Vector<double> rvJ2k = getJ2kPosVel(TT,entity);
00080 
00081       Vector<double> rJ2k(3,0.0);
00082       for(int i=0; i<3; i++)
00083       {
00084          rJ2k[i] = rvJ2k[i];
00085       }
00086 
00087       return rJ2k;
00088    }
00089 
00090       /* Compute planet velocity in J2000
00091        *  
00092        * @param TT         Time(Modified Julian Date in TT<TAI+32.184>) of interest 
00093        * @param entity     The planet to be computed
00094        * @return           The velocity of the planet in km
00095        */
00096    Vector<double> ReferenceFrames::getJ2kVelocity( const CommonTime&      TT, 
00097                                                    SolarSystem::Planet entity)
00098       throw(Exception)
00099    {
00100       Vector<double> rvJ2k = getJ2kPosVel(TT,entity);
00101 
00102       Vector<double> vJ2k(3,0.0);
00103       for(int i = 0; i < 3; i++) 
00104       {
00105          vJ2k[i] = rvJ2k[i+3];
00106       }
00107 
00108       return vJ2k;
00109    }
00110 
00111       /* Compute planet position and velocity
00112        *  
00113        * @param TT         Time(Modified Julian Date in TT<TAI+32.184>) of interest 
00114        * @param entity     The planet to be computed
00115        * @param center     relative to whick the result apply
00116        * @return           The position and velocity of the planet in km and km/s
00117        */
00118    Vector<double> ReferenceFrames::getJ2kPosVel( const CommonTime&      TT, 
00119                                                  SolarSystem::Planet entity,
00120                                                  SolarSystem::Planet center)
00121       throw(Exception)
00122    {
00123       Vector<double> rvJ2k(6,0.0);
00124 
00125       try
00126       {
00127          double rvState[6] = {0.0};
00128          int ret = solarPlanets.computeState(JD_TO_MJD + static_cast<Epoch>(TT).MJD(),
00129             entity,
00130             center,
00131             rvState);
00132          
00133             // change the unit to km/s from km/day
00134          rvState[3] /= 86400.0;
00135          rvState[4] /= 86400.0;
00136          rvState[5] /= 86400.0;
00137 
00138          if(ret == 0)
00139          {
00140             rvJ2k = rvState;
00141             return rvJ2k;
00142          }
00143          else
00144          {
00145             rvJ2k.resize(6,0.0);
00146 
00147             // failed to compute
00148             InvalidRequest e("Failed to compute, error code: "
00149                +StringUtils::asString(ret)+" with meaning\n"
00150                +"-1 and -2 given time is out of the file \n"
00151                +"-3 and -4 input stream is not open or not valid,"
00152                +" or EOF was found prematurely");
00153 
00154             GPSTK_THROW(e);
00155          }
00156       }
00157       catch(Exception& e)
00158       {
00159          GPSTK_RETHROW(e);
00160       }
00161       catch(exception& e)
00162       {
00163          Exception ee(e.what());
00164          GPSTK_THROW(ee);
00165       }
00166       catch(...)
00167       {
00168          Exception e("Unknown error!");
00169          GPSTK_THROW(e);
00170       }
00171 
00172       return rvJ2k;
00173       
00174    }  // End of method 'ReferenceFrames::getJ2kPosVel()'
00175 
00176 
00177       /* Compute planet position in ECEF
00178        *  
00179        * @param UTC        Time(Modified Julian Date in UTC) of interest 
00180        * @param entity     The planet to be computed
00181        * @param center     relative to whick the result apply
00182        * @return           The position of the planet in km and km/s
00183        */
00184    Vector<double> ReferenceFrames::getECEFPosition(UTCTime             UTC, 
00185                                                    SolarSystem::Planet entity,
00186                                                    SolarSystem::Planet center)
00187          throw(Exception)
00188    {
00189       Vector<double> ecefPosVel = getECEFPosVel(UTC, entity, center);
00190 
00191       Vector<double> ecefPos(3,0.0);
00192       ecefPos(0) = ecefPosVel(0);
00193       ecefPos(1) = ecefPosVel(1);
00194       ecefPos(2) = ecefPosVel(2);
00195 
00196       return ecefPos;
00197    }
00198 
00199       /* Compute planet velocity in ECEF
00200        *  
00201        * @param UTC        Time(Modified Julian Date in UTC) of interest 
00202        * @param entity     The planet to be computed
00203        * @param center     relative to whick the result apply
00204        * @return           The position of the planet in km and km/s
00205        */
00206    Vector<double> ReferenceFrames::getECEFVelocity(UTCTime             UTC, 
00207                                                    SolarSystem::Planet entity,
00208                                                    SolarSystem::Planet center)
00209          throw(Exception)
00210    {
00211       Vector<double> ecefPosVel = getECEFPosVel(UTC, entity, center);
00212 
00213       Vector<double> ecefVel(3,0.0);
00214       ecefVel(0) = ecefPosVel(3);
00215       ecefVel(1) = ecefPosVel(4);
00216       ecefVel(2) = ecefPosVel(5);
00217 
00218       return ecefVel;
00219    }
00220 
00221       /* Compute planet position and velocity in ECEF
00222        *  
00223        * @param UTC        Time(Modified Julian Date in UTC) of interest 
00224        * @param entity     The planet to be computed
00225        * @param center     relative to whick the result apply
00226        * @return           The position and velocity of the planet in km and km/s
00227        */
00228    Vector<double> ReferenceFrames::getECEFPosVel( UTCTime             UTC, 
00229                                                   SolarSystem::Planet entity,
00230                                                   SolarSystem::Planet center)
00231       throw(Exception)
00232    {
00233         Vector<double> j2kPosVel = getJ2kPosVel( UTC.asTT(), entity, center);
00234         Vector<double> ecefPosVel = J2kPosVelToECEF(UTC, j2kPosVel);
00235 
00236         return ecefPosVel;
00237    }
00238 
00239       // ECEF = W * S * NP * J2k
00240    void ReferenceFrames::J2kToECEFMatrix(UTCTime         UTC,
00241                                          Matrix<double>& POM,
00242                                          Matrix<double>& Theta, 
00243                                          Matrix<double>& NP)
00244       throw(Exception)
00245    {
00246       // Earth orientation data
00247       double xp = UTC.xPole() * DAS2R;
00248       double yp = UTC.yPole() * DAS2R;
00249      
00250       CommonTime TT = UTC.asTT();
00251       CommonTime UT1 = UTC.asUT1();
00252       
00253 
00254       // IAU 1976 precession matrix       
00255       Matrix<double> P = iauPmat76(TT);
00256 
00257       // Nutation correction wrt IAU 1976/1980 (mas->radians)
00258       const double DDP80 = 0.0; //-55.0655 * DAS2R/1000.0;
00259       const double DDE80 = 0.0; //-6.3580 * DAS2R/1000.0;
00260 
00261       // Nutation angle
00262       double DPSI = 0.0;
00263       double DEPS = 0.0;         
00264       nutationAngles(TT, DPSI, DEPS);
00265 
00266       DPSI += DDP80;
00267       DEPS += DDE80;
00268 
00269       // Mean obliquity
00270       double EPSA = meanObliquity(TT); 
00271       
00272       // IAU 1980 Nutation matrix
00273       Matrix<double> N = iauNmat(EPSA, DPSI , DEPS);
00274 
00275       // NP
00276       NP = N * P;
00277 
00278       // Euqation of the equinoxes, including nutation correction
00279       double EE = iauEqeq94(TT) + DDP80 * std::cos(EPSA);
00280 
00281       // Greenwich apparent sidereal time(IAU 1982/1994)
00282       double GST = normalizeAngle(iauGmst82(UT1) + EE);
00283       
00284       Theta =  Rz(GST);
00285      
00286       // Polar motion matrix
00287       POM = Ry(-xp) * Rx(-yp);
00288       
00289       // All Matrix are ready now
00290 
00291       return;
00292       
00293    }  // End of method 'ReferenceFrames::J2kToECEFMatrix()'
00294 
00295 
00296       // return POM * Theta * NP 
00297    Matrix<double> ReferenceFrames::J2kToECEFMatrix(UTCTime UTC)
00298    {
00299       Matrix<double> POM, Theta, NP;
00300       J2kToECEFMatrix(UTC,POM,Theta,NP);
00301 
00302       return (POM * Theta * NP);
00303    }
00304    
00306    Matrix<double> ReferenceFrames::J2kToTODMatrix(UTCTime UTC)
00307    {
00308       Matrix<double> POM, Theta, NP;
00309       J2kToECEFMatrix(UTC,POM,Theta,NP);
00310 
00311       return NP;
00312    }
00313 
00314 
00315    Vector<double> ReferenceFrames::J2kPosVelToECEF(UTCTime UTC, Vector<double> j2kPosVel)
00316       throw(Exception)
00317    {
00318       
00319       Matrix<double> POM, Theta, NP;
00320       J2kToECEFMatrix(UTC,POM,Theta,NP);
00321 
00322       const double dera = earthRotationAngleRate1(UTC.mjdTT());
00323       
00324          // Derivative of Earth rotation 
00325       Matrix<double> S(3,3,0.0);
00326       S(0,1) = 1.0; S(1,0) = -1.0;      
00327       
00328       Matrix<double> dTheta = dera * S * Theta;
00329       
00330       Matrix<double> c2t = POM * Theta * NP;
00331       Matrix<double> dc2t = POM * dTheta * NP;
00332 
00333       Vector<double> j2kPos(3, 0.0), j2kVel(3, 0.0);
00334       for(int i=0; i<3; i++)
00335       {
00336          j2kPos(i) = j2kPosVel(i);
00337          j2kVel(i) = j2kPosVel(i+3);
00338       }
00339 
00340       Vector<double> ecefPos = c2t * j2kPos;
00341       Vector<double> ecefVel = c2t * j2kVel + dc2t * j2kPos;
00342       
00343       Vector<double> ecefPosVel(6,0.0);
00344       for(int i=0; i<3; i++)
00345       {
00346          ecefPosVel(i) = ecefPos(i);
00347          ecefPosVel(i+3) = ecefVel(i);
00348       }
00349 
00350       return ecefPosVel;
00351 
00352    }  // End of method 'ReferenceFrames::J2kPosVelToECEF()'
00353 
00354 
00355    Vector<double> ReferenceFrames::ECEFPosVelToJ2k(UTCTime UTC, Vector<double> ecefPosVel)
00356       throw(Exception)
00357    {
00358       Matrix<double> POM, Theta, NP;
00359       J2kToECEFMatrix(UTC,POM,Theta,NP);
00360 
00361       const double dera = earthRotationAngleRate1(UTC.mjdTT());
00362 
00363       // Derivative of Earth rotation 
00364       Matrix<double> S(3,3,0.0);
00365       S(0,1) = 1.0; S(1,0) = -1.0;      
00366 
00367       Matrix<double> dTheta = dera * S * Theta;
00368 
00369       Matrix<double> c2t = POM * Theta * NP;
00370       Matrix<double> dc2t = POM * dTheta * NP;
00371       
00372       Vector<double> ecefPos(3, 0.0), ecefVel(3, 0.0);
00373       for(int i=0; i<3; i++)
00374       {
00375          ecefPos(i) = ecefPosVel(i);
00376          ecefVel(i) = ecefPosVel(i+3);
00377       }
00378 
00379       Vector<double> j2kPos = transpose(c2t) * ecefPos;
00380       Vector<double> j2kVel = transpose(c2t) * ecefVel 
00381                              +transpose(dc2t)* ecefPos;
00382 
00383       Vector<double> j2kPosVel(6,0.0);
00384       for(int i=0; i<3; i++)
00385       {
00386          j2kPosVel(i) = j2kPos(i);
00387          j2kPosVel(i+3) = j2kVel(i);
00388       }
00389 
00390       return j2kPosVel;
00391 
00392    }  // End of method 'ReferenceFrames::ECEFPosVelToJ2k()'
00393  
00395    Vector<double> ReferenceFrames::J2kStateToECEF(UTCTime UTC, Vector<double> j2kState)
00396       throw(Exception)
00397    {
00398       
00399       Matrix<double> POM, Theta, NP;
00400       J2kToECEFMatrix(UTC,POM,Theta,NP);
00401 
00402       // get Theta rates
00403       double dera1 = earthRotationAngleRate1(UTC.asTT().MJD());
00404       double dera2 = earthRotationAngleRate2(UTC.asTT().MJD());
00405       double dera3 = earthRotationAngleRate3(UTC.asTT().MJD());
00406 
00407       double cs1[3][3]={{0,1,0},{-1,0,0},{0,0,0}};              
00408       double cs2[3][3]={{-1,0,0},{0,-1,0},{0,0,0}};     
00409       double cs3[3][3]={{0,-1,0},{1,0,0},{0,0,0}};      
00410 
00411       Matrix<double> s1(3,3,0.0),s2(3,3,0.0),s3(3,3,0.0);
00412       s1 = &cs1[0][0];
00413       s2 = &cs2[0][0];
00414       s3 = &cs3[0][0];
00415 
00416       // dTheta1 dTheta2 dTheta3
00417       Matrix<double> dTheta1 = s1 * Theta * dera1;
00418 
00419       Matrix<double> dTheta2 = s2 * Theta * ( dera1 * dera1) 
00420                              + dTheta1*dera2;
00421 
00422       Matrix<double> dTheta3 = s3 * Theta * ( dera1 * dera1 * dera1)
00423                              + s2 * Theta * (2.0 * dera1 * dera2) 
00424                              + dTheta2 * dera2
00425                              + dTheta1 * dera3;
00426 
00427       Vector<double> r(3,0.0),v(3,0.0),a(3,0.0),d(3,0.0);
00428       for(int i=0; i<3; i++)
00429       {
00430          r(i) =  j2kState(i+0);
00431          v(i) =  j2kState(i+3);
00432          a(i) =  j2kState(i+6);
00433          d(i) =  j2kState(i+9);
00434       }
00435 
00436       // tm1 = POM*Theta*NP
00437       Matrix<double> tm1 = POM * Theta * NP;
00438       // tm2 = POM*dTheta1*NP
00439       Matrix<double> tm2 = POM * dTheta1 * NP;
00440       // tm3 = POM*dTheta3*NP
00441       Matrix<double> tm3 = POM * dTheta2 * NP;
00442       // tm4 = POM*dTheta4*NP
00443       Matrix<double> tm4 = POM * dTheta3 * NP;
00444      
00445       Vector<double> r2, v2, a2, d2;
00446 
00447       // r = tm1*r
00448       r2 = tm1 * r;
00449       
00450       // v = tm1*v+tm2*r
00451       v2 = tm1 * v 
00452          + tm2 * r;
00453 
00454       // a = tm1*a+2.0*tm2*v+tm3*r
00455       a2 = tm1 * a
00456          + tm2 * v * 2.0
00457          + tm3 * r;
00458 
00459       // da = tm1*da+3.0*tm2*a+3.0*tm3*v+tm4*r
00460       d2 = tm1 * d
00461          + tm2 * a * 3.0
00462          + tm3 * v * 3.0 
00463          + tm4 * r;
00464 
00465       Vector<double> state(12,0.0);
00466       for(int i=0; i<3; i++)
00467       {
00468          state(i+0) = r2(i);
00469          state(i+3) = v2(i);
00470          state(i+6) = a2(i);
00471          state(i+9) = d2(i);
00472       }
00473       
00474       return state;
00475 
00476    }  // End of method 'ReferenceFrames::J2kStateToECEF()'
00477 
00478 
00480    Vector<double> ReferenceFrames::ECEFStateToJ2k(UTCTime UTC, Vector<double> ecefState)
00481       throw(Exception)
00482    {
00483 
00484       Matrix<double> POM, Theta, NP;
00485       J2kToECEFMatrix(UTC,POM,Theta,NP);
00486 
00487       // get Theta rates
00488       double dera1 = earthRotationAngleRate1(UTC.asTT().MJD());
00489       double dera2 = earthRotationAngleRate2(UTC.asTT().MJD());
00490       double dera3 = earthRotationAngleRate3(UTC.asTT().MJD());
00491 
00492       double cs1[3][3]={{0,1,0},{-1,0,0},{0,0,0}};              
00493       double cs2[3][3]={{-1,0,0},{0,-1,0},{0,0,0}};     
00494       double cs3[3][3]={{0,-1,0},{1,0,0},{0,0,0}};      
00495 
00496       Matrix<double> s1(3,3,0.0),s2(3,3,0.0),s3(3,3,0.0);
00497       s1 = &cs1[0][0];
00498       s2 = &cs2[0][0];
00499       s3 = &cs3[0][0];
00500 
00501       // dTheta1 dTheta2 dTheta3
00502       Matrix<double> dTheta1 = s1 * Theta * dera1;
00503 
00504       Matrix<double> dTheta2 = s2 * Theta * ( dera1 * dera1) 
00505          + dTheta1*dera2;
00506 
00507       Matrix<double> dTheta3 = s3 * Theta * ( dera1 * dera1 * dera1)
00508          + s2 * Theta * (2.0 * dera1 * dera2) 
00509          + dTheta2 * dera2
00510          + dTheta1 * dera3;
00511 
00512       Vector<double> r(3,0.0),v(3,0.0),a(3,0.0),d(3,0.0);
00513       for(int i=0; i<3; i++)
00514       {
00515          r(i) =  ecefState(i+0);
00516          v(i) =  ecefState(i+3);
00517          a(i) =  ecefState(i+6);
00518          d(i) =  ecefState(i+9);
00519       }
00520 
00521       // tm1 = POM*Theta*NP
00522       Matrix<double> tm1 = transpose( POM * Theta * NP );
00523       // tm2 = POM*dTheta1*NP
00524       Matrix<double> tm2 = transpose( POM * dTheta1 * NP );
00525       // tm3 = POM*dTheta3*NP
00526       Matrix<double> tm3 = transpose( POM * dTheta2 * NP );
00527       // tm4 = POM*dTheta4*NP
00528       Matrix<double> tm4 = transpose( POM * dTheta3 * NP );
00529 
00530       Vector<double> r2, v2, a2, d2;
00531 
00532       // r = tm1*r
00533       r2 = tm1 * r;
00534 
00535       // v = tm1*v+tm2*r
00536       v2 = tm1 * v 
00537          + tm2 * r;
00538 
00539       // a = tm1*a+2.0*tm2*v+tm3*r
00540       a2 = tm1 * a
00541          + tm2 * v * 2.0
00542          + tm3 * r;
00543 
00544       // da = tm1*da+3.0*tm2*a+3.0*tm3*v+tm4*r
00545       d2 = tm1 * d
00546          + tm2 * a * 3.0
00547          + tm3 * v * 3.0 
00548          + tm4 * r;
00549 
00550       Vector<double> state(12,0.0);
00551       for(int i=0; i<3; i++)
00552       {
00553          state(i+0) = r2(i);
00554          state(i+3) = v2(i);
00555          state(i+6) = a2(i);
00556          state(i+9) = d2(i);
00557       }
00558 
00559       return state;
00560 
00561    }  // End of method 'ReferenceFrames::ECEFStateToJ2k()'
00562 
00563 
00564       // Get earth rotation angle
00565    double ReferenceFrames::earthRotationAngle(CommonTime UT1)
00566    {
00567       // IAU 2000 model
00568       double t = static_cast<Epoch>(UT1).MJD() + (JD_TO_MJD - DJ00);
00569       double f = fmod(double(static_cast<Epoch>(UT1).MJD()),1.0) + fmod(JD_TO_MJD, 1.0);
00570       
00571       double era = normalizeAngle(D2PI*(f+0.7790572732640+0.00273781191135448*t));
00572       
00573       return era;
00574    }
00575 
00576       /*Earth rotation angle first order rate.
00577        *  @param mjdTT         Modified Julian Date in TT
00578        *  @return              d(GAST)/d(t) in [rad]
00579        */
00580    double ReferenceFrames::earthRotationAngleRate1(const double& mjdTT)
00581    {
00582       double T = (mjdTT + (JD_TO_MJD - DJ00) )/36525.0;
00583       double dera = (1.002737909350795 + 5.9006e-11 * T - 5.9e-15 * T * T ) 
00584          * D2PI / 86400.0;
00585 
00586       return dera;
00587    }
00588 
00589 
00590       /*Earth rotation angle second order rate .
00591        * @param   Modified Julian Date in TT
00592        * @return  d(GAST)2/d(t)2 in [rad]
00593        */
00594    double ReferenceFrames::earthRotationAngleRate2(const double& mjdTT)
00595    {
00596       double T = ( mjdTT + (JD_TO_MJD - DJ00) ) / 36525.0;
00597       double dera = (5.9006e-11 - 5.9e-15 * T) * D2PI / 86400.0;
00598 
00599       return dera;
00600    }
00601 
00602 
00603       /*Earth rotation angle third order rate.
00604        * @param   Modified Julian Date in TT
00605        * @return  d(GAST)3/d(t)3 in [rad]
00606        */
00607    double ReferenceFrames::earthRotationAngleRate3(const double& mjdTT)
00608    {
00609       double T = ( mjdTT + (JD_TO_MJD - DJ00) ) / 36525.0;
00610       double dera = ( -5.9e-15 ) * D2PI / 86400.0;
00611 
00612       return dera;
00613    }
00614 
00639    void ReferenceFrames::doodsonArguments(CommonTime UT1, 
00640                                           CommonTime TT, 
00641                                           double BETA[6],
00642                                           double FNUT[5])
00643    {
00644       // GMST based IAU2000
00645       double THETA = iauGmst00(UT1,TT);
00646 
00647       // Fundamental Arguments (from IERS Conventions 2003) 
00648       //-----------------------------------------------------
00649       //Julian centuries since J2000 
00650       double  t = (static_cast<Epoch>(TT).MJD() + 2400000.5 - 2451545.0) / 36525.0; 
00651 
00652       // Mean anomaly of the Moon.
00653       double temp = fmod(           485868.249036  +
00654          t * ( 1717915923.2178 +
00655          t * (         31.8792 +
00656          t * (          0.051635 +
00657          t * (        - 0.00024470 ) ) ) ), TURNAS ) * DAS2R;
00658 
00659       double F1 = normalizeAngle(temp);         // -pi,pi
00660 
00661       // Mean anomaly of the Sun
00662       temp = fmod(         1287104.793048 +
00663          t * ( 129596581.0481 +
00664          t * (       - 0.5532 +
00665          t * (         0.000136 +
00666          t * (       - 0.00001149 ) ) ) ), TURNAS ) * DAS2R;
00667 
00668       double F2 = normalizeAngle(temp);      // -pi,pi
00669       
00670       // Mean longitude of the Moon minus that of the ascending node.
00671       temp = fmod(           335779.526232 +
00672          t * ( 1739527262.8478 +
00673          t * (       - 12.7512 +
00674          t * (        - 0.001037 +
00675          t * (          0.00000417 ) ) ) ), TURNAS ) * DAS2R;
00676       double F3 = normalizeAngle(temp);         // -pi,pi
00677       
00678       // Mean elongation of the Moon from the Sun. 
00679       temp = fmod(          1072260.703692 +
00680          t * ( 1602961601.2090 +
00681          t * (        - 6.3706 +
00682          t * (          0.006593 +
00683          t * (        - 0.00003169 ) ) ) ), TURNAS ) * DAS2R;
00684 
00685       double F4 = normalizeAngle(temp);         // -pi,pi
00686       
00687       // Mean longitude of the ascending node of the Moon.
00688       temp = fmod(          450160.398036 +
00689          t * ( - 6962890.5431 +
00690          t * (         7.4722 +
00691          t * (         0.007702 +
00692          t * (       - 0.00005939 ) ) ) ), TURNAS ) * DAS2R;
00693       double F5 = normalizeAngle(temp);      // -pi,pi
00694 
00695       FNUT[0] = F1;
00696       FNUT[1] = F2;
00697       FNUT[2] = F3;
00698       FNUT[3] = F4;
00699       FNUT[4] = F5;
00700 
00701 
00702       double S = F3+F5;
00703 
00704       BETA[0] = THETA+ASConstant::PI-S;
00705       BETA[1] = F3+F5;
00706       BETA[2] = S-F4;
00707       BETA[3] = S-F1;
00708       BETA[4] = -F5;
00709       BETA[5] = S-F4-F2;
00710 
00711    }  // End of method 'ReferenceFrames::doodsonArguments()'
00712    
00713 
00714    void ReferenceFrames::test()
00715    {
00716       IERS::loadSTKFile("InputData/EOP-v1.1.txt");
00717 
00718       
00719       double rv_j2k[6]={-23830.593e3,-9747.074e3,-6779.829e3,
00720                         +1.561964e3,-1.754346e3,-3.068851e3};
00721 
00722       Vector<double> j2kPosVel(6,0.0);
00723       Vector<double> ecefPosVel(6,0.0);
00724 
00725       // 2007 07 01 54282  0.192316  0.407299 -0.1582305
00726       UTCTime utc(2007,07,01,00,0,0.0);
00727      
00728       j2kPosVel = rv_j2k;
00729       ecefPosVel = J2kPosVelToECEF(utc,j2kPosVel);
00730 
00731       for(int i=0;i<6;i++)
00732          cout<<setprecision(12)<<ecefPosVel(i)<<endl;
00733 
00734       int a =0;
00735 
00736       /*
00737       std::string fileHeader = "de405/header.405";
00738       std::vector<std::string> fileData;
00739 
00740       std::ifstream fin("de405.txt");
00741 
00742       std::string path,header,testpo;
00743 
00744       getline(fin,path);
00745       getline(fin,header);
00746       getline(fin,testpo);
00747 
00748       fileHeader = path + header;
00749 
00750       std::string buf;
00751       while(getline(fin,buf))
00752       {
00753          std::string file = path + buf;
00754          fileData.push_back(file);
00755       }
00756 
00757       fin.close();
00758 
00759       solarPlanets.readASCIIheader(fileHeader);
00760       cout<<solarPlanets.readASCIIdata(fileData)<<endl;
00761 
00762       cout<<solarPlanets.writeBinaryFile("jplde405")<<endl;
00763       */
00764       
00765       /*
00766       solarPlanets.initializeWithBinaryFile("jplde405");
00767       double rv[6] = {0.0};
00768       CommonTime t(2010,1,1,0,0,0.0);
00769       int ret = solarPlanets.computeState(t.JD(),SolarSystem::Moon,SolarSystem::Earth,rv);
00770       rv[3]/=86400.0;
00771       rv[4]/=86400.0;
00772       rv[5]/=86400.0;
00773       cout<<ret<<endl;
00774 
00775       int a = 0;
00776       */
00777      
00778    }
00779 
00780       // Normalize angle into the range -pi <= a < +pi.
00781    double ReferenceFrames::normalizeAngle(double a)
00782    {
00783       double w = fmod(a, D2PI);
00784       if (fabs(w) >= (D2PI*0.5)) 
00785       {
00786          w-= ((a<0.0)?-D2PI:D2PI);
00787       }
00788 
00789       return w;
00790    }
00791 
00792       // Rotate an r-matrix about the x-axis.
00793    Matrix<double> ReferenceFrames::Rx(const double& angle)
00794    {
00795       const double s = std::sin(angle);
00796       const double c = std::cos(angle);
00797 
00798       const double a[9] = { 1, 0, 0, 0, c, s, 0,-s, c };
00799 
00800       Matrix<double> r(3,3,0.0);
00801       r = a;
00802 
00803       return r;
00804    }
00805 
00806       // Rotate an r-matrix about the y-axis.
00807    Matrix<double> ReferenceFrames::Ry(const double& angle)
00808    {
00809       const double s = std::sin(angle);
00810       const double c = std::cos(angle);
00811 
00812       const double a[9] = { c, 0,-s, 0, 1, 0, s, 0, c };
00813 
00814       Matrix<double> r(3,3,0.0);
00815       r = a;
00816 
00817       return r;
00818    }
00819 
00820       // Rotate an r-matrix about the z-axis.
00821    Matrix<double> ReferenceFrames::Rz(const double& angle)
00822    {
00823       const double s = std::sin(angle);
00824       const double c = std::cos(angle);
00825 
00826       const double a[9] = { c, s, 0,-s, c, 0, 0, 0, 1 };
00827 
00828       Matrix<double> r(3,3,0.0);
00829       r = a;
00830 
00831       return r;
00832    }
00833 
00834    Matrix<double> ReferenceFrames::iauPmat76(CommonTime TT)
00835    {
00836       
00837       // Interval between fundamental epoch J2000.0 and start epoch (JC). 
00838       const double t0 = 0.0;
00839 
00840       // Interval over which precession required (JC). 
00841       const double t = (JD_TO_MJD - DJ00 + static_cast<Epoch>(TT).MJD()) / DJC;
00842 
00843       // Euler angles. 
00844       const double tas2r = t * DAS2R;
00845       const double w = 2306.2181 + (1.39656 - 0.000139 * t0) * t0;
00846 
00847       double zeta = (w + ((0.30188 - 0.000344 * t0) + 0.017998 * t) * t) * tas2r;
00848 
00849       double z = (w + ((1.09468 + 0.000066 * t0) + 0.018203 * t) * t) * tas2r;
00850 
00851       double theta = ((2004.3109 + (-0.85330 - 0.000217 * t0) * t0)
00852          + ((-0.42665 - 0.000217 * t0) - 0.041833 * t) * t) * tas2r;
00853 
00854       return ( Rz(-z) * Ry(theta) * Rz(-zeta) );
00855 
00856    }  // End of method 'ReferenceFrames::iauPmat76()'
00857    
00858 
00859    void ReferenceFrames::nutationAngles(CommonTime TT, double& dpsi, double& deps)
00860    {
00861       // Units of 0.1 milliarcsecond to radians 
00862       const double U2R = DAS2R / 1e4;
00863 
00864 
00865       // Table of multiples of arguments and coefficients 
00866       // ------------------------------------------------ 
00867 
00868       // The units for the sine and cosine coefficients are 0.1 mas and 
00869       // the same per Julian century 
00870 
00871       static const struct 
00872       {
00873          int nl,nlp,nf,nd,nom; // coefficients of l,l',F,D,Om 
00874          double sp,spt;        // longitude sine, 1 and t coefficients 
00875          double ce,cet;        // obliquity cosine, 1 and t coefficients 
00876       } x[] = {
00877 
00878          /* 1-10 */
00879          {  0,  0,  0,  0,  1, -171996.0, -174.2,  92025.0,    8.9 },
00880          {  0,  0,  0,  0,  2,    2062.0,    0.2,   -895.0,    0.5 },
00881          { -2,  0,  2,  0,  1,      46.0,    0.0,    -24.0,    0.0 },
00882          {  2,  0, -2,  0,  0,      11.0,    0.0,      0.0,    0.0 },
00883          { -2,  0,  2,  0,  2,      -3.0,    0.0,      1.0,    0.0 },
00884          {  1, -1,  0, -1,  0,      -3.0,    0.0,      0.0,    0.0 },
00885          {  0, -2,  2, -2,  1,      -2.0,    0.0,      1.0,    0.0 },
00886          {  2,  0, -2,  0,  1,       1.0,    0.0,      0.0,    0.0 },
00887          {  0,  0,  2, -2,  2,  -13187.0,   -1.6,   5736.0,   -3.1 },
00888          {  0,  1,  0,  0,  0,    1426.0,   -3.4,     54.0,   -0.1 },
00889 
00890          /* 11-20 */
00891          {  0,  1,  2, -2,  2,    -517.0,    1.2,    224.0,   -0.6 },
00892          {  0, -1,  2, -2,  2,     217.0,   -0.5,    -95.0,    0.3 },
00893          {  0,  0,  2, -2,  1,     129.0,    0.1,    -70.0,    0.0 },
00894          {  2,  0,  0, -2,  0,      48.0,    0.0,      1.0,    0.0 },
00895          {  0,  0,  2, -2,  0,     -22.0,    0.0,      0.0,    0.0 },
00896          {  0,  2,  0,  0,  0,      17.0,   -0.1,      0.0,    0.0 },
00897          {  0,  1,  0,  0,  1,     -15.0,    0.0,      9.0,    0.0 },
00898          {  0,  2,  2, -2,  2,     -16.0,    0.1,      7.0,    0.0 },
00899          {  0, -1,  0,  0,  1,     -12.0,    0.0,      6.0,    0.0 },
00900          { -2,  0,  0,  2,  1,      -6.0,    0.0,      3.0,    0.0 },
00901 
00902          /* 21-30 */
00903          {  0, -1,  2, -2,  1,      -5.0,    0.0,      3.0,    0.0 },
00904          {  2,  0,  0, -2,  1,       4.0,    0.0,     -2.0,    0.0 },
00905          {  0,  1,  2, -2,  1,       4.0,    0.0,     -2.0,    0.0 },
00906          {  1,  0,  0, -1,  0,      -4.0,    0.0,      0.0,    0.0 },
00907          {  2,  1,  0, -2,  0,       1.0,    0.0,      0.0,    0.0 },
00908          {  0,  0, -2,  2,  1,       1.0,    0.0,      0.0,    0.0 },
00909          {  0,  1, -2,  2,  0,      -1.0,    0.0,      0.0,    0.0 },
00910          {  0,  1,  0,  0,  2,       1.0,    0.0,      0.0,    0.0 },
00911          { -1,  0,  0,  1,  1,       1.0,    0.0,      0.0,    0.0 },
00912          {  0,  1,  2, -2,  0,      -1.0,    0.0,      0.0,    0.0 },
00913 
00914          /* 31-40 */
00915          {  0,  0,  2,  0,  2,   -2274.0,   -0.2,    977.0,   -0.5 },
00916          {  1,  0,  0,  0,  0,     712.0,    0.1,     -7.0,    0.0 },
00917          {  0,  0,  2,  0,  1,    -386.0,   -0.4,    200.0,    0.0 },
00918          {  1,  0,  2,  0,  2,    -301.0,    0.0,    129.0,   -0.1 },
00919          {  1,  0,  0, -2,  0,    -158.0,    0.0,     -1.0,    0.0 },
00920          { -1,  0,  2,  0,  2,     123.0,    0.0,    -53.0,    0.0 },
00921          {  0,  0,  0,  2,  0,      63.0,    0.0,     -2.0,    0.0 },
00922          {  1,  0,  0,  0,  1,      63.0,    0.1,    -33.0,    0.0 },
00923          { -1,  0,  0,  0,  1,     -58.0,   -0.1,     32.0,    0.0 },
00924          { -1,  0,  2,  2,  2,     -59.0,    0.0,     26.0,    0.0 },
00925 
00926          /* 41-50 */
00927          {  1,  0,  2,  0,  1,     -51.0,    0.0,     27.0,    0.0 },
00928          {  0,  0,  2,  2,  2,     -38.0,    0.0,     16.0,    0.0 },
00929          {  2,  0,  0,  0,  0,      29.0,    0.0,     -1.0,    0.0 },
00930          {  1,  0,  2, -2,  2,      29.0,    0.0,    -12.0,    0.0 },
00931          {  2,  0,  2,  0,  2,     -31.0,    0.0,     13.0,    0.0 },
00932          {  0,  0,  2,  0,  0,      26.0,    0.0,     -1.0,    0.0 },
00933          { -1,  0,  2,  0,  1,      21.0,    0.0,    -10.0,    0.0 },
00934          { -1,  0,  0,  2,  1,      16.0,    0.0,     -8.0,    0.0 },
00935          {  1,  0,  0, -2,  1,     -13.0,    0.0,      7.0,    0.0 },
00936          { -1,  0,  2,  2,  1,     -10.0,    0.0,      5.0,    0.0 },
00937 
00938          /* 51-60 */
00939          {  1,  1,  0, -2,  0,      -7.0,    0.0,      0.0,    0.0 },
00940          {  0,  1,  2,  0,  2,       7.0,    0.0,     -3.0,    0.0 },
00941          {  0, -1,  2,  0,  2,      -7.0,    0.0,      3.0,    0.0 },
00942          {  1,  0,  2,  2,  2,      -8.0,    0.0,      3.0,    0.0 },
00943          {  1,  0,  0,  2,  0,       6.0,    0.0,      0.0,    0.0 },
00944          {  2,  0,  2, -2,  2,       6.0,    0.0,     -3.0,    0.0 },
00945          {  0,  0,  0,  2,  1,      -6.0,    0.0,      3.0,    0.0 },
00946          {  0,  0,  2,  2,  1,      -7.0,    0.0,      3.0,    0.0 },
00947          {  1,  0,  2, -2,  1,       6.0,    0.0,     -3.0,    0.0 },
00948          {  0,  0,  0, -2,  1,      -5.0,    0.0,      3.0,    0.0 },
00949 
00950          /* 61-70 */
00951          {  1, -1,  0,  0,  0,       5.0,    0.0,      0.0,    0.0 },
00952          {  2,  0,  2,  0,  1,      -5.0,    0.0,      3.0,    0.0 },
00953          {  0,  1,  0, -2,  0,      -4.0,    0.0,      0.0,    0.0 },
00954          {  1,  0, -2,  0,  0,       4.0,    0.0,      0.0,    0.0 },
00955          {  0,  0,  0,  1,  0,      -4.0,    0.0,      0.0,    0.0 },
00956          {  1,  1,  0,  0,  0,      -3.0,    0.0,      0.0,    0.0 },
00957          {  1,  0,  2,  0,  0,       3.0,    0.0,      0.0,    0.0 },
00958          {  1, -1,  2,  0,  2,      -3.0,    0.0,      1.0,    0.0 },
00959          { -1, -1,  2,  2,  2,      -3.0,    0.0,      1.0,    0.0 },
00960          { -2,  0,  0,  0,  1,      -2.0,    0.0,      1.0,    0.0 },
00961 
00962          /* 71-80 */
00963          {  3,  0,  2,  0,  2,      -3.0,    0.0,      1.0,    0.0 },
00964          {  0, -1,  2,  2,  2,      -3.0,    0.0,      1.0,    0.0 },
00965          {  1,  1,  2,  0,  2,       2.0,    0.0,     -1.0,    0.0 },
00966          { -1,  0,  2, -2,  1,      -2.0,    0.0,      1.0,    0.0 },
00967          {  2,  0,  0,  0,  1,       2.0,    0.0,     -1.0,    0.0 },
00968          {  1,  0,  0,  0,  2,      -2.0,    0.0,      1.0,    0.0 },
00969          {  3,  0,  0,  0,  0,       2.0,    0.0,      0.0,    0.0 },
00970          {  0,  0,  2,  1,  2,       2.0,    0.0,     -1.0,    0.0 },
00971          { -1,  0,  0,  0,  2,       1.0,    0.0,     -1.0,    0.0 },
00972          {  1,  0,  0, -4,  0,      -1.0,    0.0,      0.0,    0.0 },
00973 
00974          /* 81-90 */
00975          { -2,  0,  2,  2,  2,       1.0,    0.0,     -1.0,    0.0 },
00976          { -1,  0,  2,  4,  2,      -2.0,    0.0,      1.0,    0.0 },
00977          {  2,  0,  0, -4,  0,      -1.0,    0.0,      0.0,    0.0 },
00978          {  1,  1,  2, -2,  2,       1.0,    0.0,     -1.0,    0.0 },
00979          {  1,  0,  2,  2,  1,      -1.0,    0.0,      1.0,    0.0 },
00980          { -2,  0,  2,  4,  2,      -1.0,    0.0,      1.0,    0.0 },
00981          { -1,  0,  4,  0,  2,       1.0,    0.0,      0.0,    0.0 },
00982          {  1, -1,  0, -2,  0,       1.0,    0.0,      0.0,    0.0 },
00983          {  2,  0,  2, -2,  1,       1.0,    0.0,     -1.0,    0.0 },
00984          {  2,  0,  2,  2,  2,      -1.0,    0.0,      0.0,    0.0 },
00985 
00986          /* 91-100 */
00987          {  1,  0,  0,  2,  1,      -1.0,    0.0,      0.0,    0.0 },
00988          {  0,  0,  4, -2,  2,       1.0,    0.0,      0.0,    0.0 },
00989          {  3,  0,  2, -2,  2,       1.0,    0.0,      0.0,    0.0 },
00990          {  1,  0,  2, -2,  0,      -1.0,    0.0,      0.0,    0.0 },
00991          {  0,  1,  2,  0,  1,       1.0,    0.0,      0.0,    0.0 },
00992          { -1, -1,  0,  2,  1,       1.0,    0.0,      0.0,    0.0 },
00993          {  0,  0, -2,  0,  1,      -1.0,    0.0,      0.0,    0.0 },
00994          {  0,  0,  2, -1,  2,      -1.0,    0.0,      0.0,    0.0 },
00995          {  0,  1,  0,  2,  0,      -1.0,    0.0,      0.0,    0.0 },
00996          {  1,  0, -2, -2,  0,      -1.0,    0.0,      0.0,    0.0 },
00997 
00998          /* 101-106 */
00999          {  0, -1,  2,  0,  1,      -1.0,    0.0,      0.0,    0.0 },
01000          {  1,  1,  0, -2,  1,      -1.0,    0.0,      0.0,    0.0 },
01001          {  1,  0, -2,  2,  0,      -1.0,    0.0,      0.0,    0.0 },
01002          {  2,  0,  0,  2,  0,       1.0,    0.0,      0.0,    0.0 },
01003          {  0,  0,  2,  4,  2,      -1.0,    0.0,      0.0,    0.0 },
01004          {  0,  1,  0,  1,  0,       1.0,    0.0,      0.0,    0.0 }
01005       };
01006 
01007       // Number of terms in the series 
01008       const int NT = (int) (sizeof x / sizeof x[0]);
01009 
01010      
01011       // Interval between fundamental epoch J2000.0 and given date (JC). 
01012       const double t = ((JD_TO_MJD - DJ00) + static_cast<Epoch>(TT).MJD()) / DJC;
01013 
01014       // Fundamental arguments 
01015       // --------------------- 
01016 
01017       // Mean longitude of Moon minus mean longitude of Moon's perigee. 
01018       double el = normalizeAngle(
01019          (485866.733 + (715922.633 + (31.310 + 0.064 * t) * t) * t)
01020          * DAS2R + fmod(1325.0 * t, 1.0) * D2PI);
01021 
01022       // Mean longitude of Sun minus mean longitude of Sun's perigee. 
01023       double elp = normalizeAngle(
01024          (1287099.804 + (1292581.224 + (-0.577 - 0.012 * t) * t) * t)
01025          * DAS2R + fmod(99.0 * t, 1.0) * D2PI);
01026 
01027       // Mean longitude of Moon minus mean longitude of Moon's node. 
01028       double f = normalizeAngle(
01029          (335778.877 + (295263.137 + (-13.257 + 0.011 * t) * t) * t)
01030          * DAS2R + fmod(1342.0 * t, 1.0) * D2PI);
01031 
01032       // Mean elongation of Moon from Sun. 
01033       double d = normalizeAngle(
01034          (1072261.307 + (1105601.328 + (-6.891 + 0.019 * t) * t) * t)
01035          * DAS2R + fmod(1236.0 * t, 1.0) * D2PI);
01036 
01037       // Longitude of the mean ascending node of the lunar orbit on the 
01038       // ecliptic, measured from the mean equinox of date. 
01039       double om = normalizeAngle(
01040          (450160.280 + (-482890.539 + (7.455 + 0.008 * t) * t) * t)
01041          * DAS2R + fmod(-5.0 * t, 1.0) * D2PI);
01042 
01043 
01044       // Nutation series 
01045       // --------------- 
01046 
01047       // Initialize nutation components. 
01048       double dp = 0.0;
01049       double de = 0.0;
01050 
01051       // Sum the nutation terms, ending with the biggest. 
01052       for (int j = NT-1; j >= 0; j--) 
01053       {
01054 
01055          // Form argument for current term. 
01056          double arg = (double)x[j].nl  * el
01057             + (double)x[j].nlp * elp
01058             + (double)x[j].nf  * f
01059             + (double)x[j].nd  * d
01060             + (double)x[j].nom * om;
01061 
01062          // Accumulate current nutation term. 
01063          double s = x[j].sp + x[j].spt * t;
01064          double c = x[j].ce + x[j].cet * t;
01065          if (s != 0.0) dp += s * std::sin(arg);
01066          if (c != 0.0) de += c * std::cos(arg);
01067       }
01068 
01069       // Convert results from 0.1 mas units to radians. 
01070       dpsi = dp * U2R;
01071       deps = de * U2R;
01072 
01073    }  // End of 'ReferenceFrames::nutationAngles()'
01074 
01075 
01076    double ReferenceFrames::meanObliquity(CommonTime TT)
01077    {
01078      
01079       // Interval between fundamental epoch J2000.0 and given date (JC)
01080       double t = ((JD_TO_MJD - DJ00) + static_cast<Epoch>(TT).MJD()) / DJC;
01081 
01082       // Mean obliquity of date. 
01083       double eps0 = DAS2R * (84381.448  +
01084          (-46.8150   +
01085          (-0.00059   +
01086          ( 0.001813) * t) * t) * t);
01087 
01088       return eps0;
01089    }
01090 
01091    double ReferenceFrames::iauEqeq94(CommonTime TT)
01092    {
01093       // Interval between fundamental epoch J2000.0 and given date (JC). 
01094       double t = ((JD_TO_MJD - DJ00) + static_cast<Epoch>(TT).MJD()) / DJC;
01095 
01096       // Longitude of the mean ascending node of the lunar orbit on the 
01097       // ecliptic, measured from the mean equinox of date. 
01098       double om = normalizeAngle((450160.280 + (-482890.539
01099          + (7.455 + 0.008 * t) * t) * t) * DAS2R
01100          + fmod(-5.0 * t, 1.0) * D2PI);
01101 
01102       // Nutation components and mean obliquity. 
01103       double dpsi(0.0), deps(0.0);
01104       nutationAngles(TT, dpsi, deps);
01105       
01106       double eps0 = meanObliquity(TT);
01107 
01108       // Equation of the equinoxes. 
01109       double ee = dpsi * std::cos(eps0) 
01110          + DAS2R*(0.00264 * std::sin(om) + 0.000063 * std::sin(om + om));
01111 
01112       return ee;
01113    }
01114 
01115    double ReferenceFrames::iauGmst82(CommonTime UT1)
01116    {
01117       // Coefficients of IAU 1982 GMST-UT1 model 
01118       const double A = 24110.54841  -  86400.0 / 2.0;
01119       const double B = 8640184.812866;
01120       const double C = 0.093104;
01121       const double D =  -6.2e-6;
01122 
01123       // Note: the first constant, A, has to be adjusted by 12 hours 
01124       // because the UT1 is supplied as a Julian date, which begins  
01125       // at noon.                                                    
01126 
01127       // Julian centuries since fundamental epoch. 
01128       double d2 = JD_TO_MJD;
01129       double d1 = static_cast<Epoch>(UT1).MJD();
01130       double t = (d1 + (d2 - DJ00)) / DJC;
01131 
01132       // Fractional part of JD(UT1), in seconds. 
01133       double f = 86400.0 * (fmod(d1, 1.0) + fmod(d2, 1.0));
01134 
01135       // GMST at this UT1. 
01136       double gmst = normalizeAngle(
01137          DS2R * ((A + (B + (C + D * t) * t) * t) + f));
01138 
01139       return gmst;
01140 
01141    }  // End of method 'ReferenceFrames::iauGmst82()'
01142 
01143       // Greenwich mean sidereal time by IAU 2000 model
01144    double ReferenceFrames::iauGmst00(CommonTime UT1,CommonTime TT)
01145    {
01146 
01147       // TT Julian centuries since J2000.0. 
01148       double t = ((JD_TO_MJD - DJ00) + static_cast<Epoch>(TT).MJD()) / DJC;
01149       /* Greenwich Mean Sidereal Time, IAU 2000. */
01150       double gmst = normalizeAngle(earthRotationAngle(UT1) +
01151          (     0.014506   +
01152          (  4612.15739966 +
01153          (     1.39667721 +
01154          (    -0.00009344 +
01155          (     0.00001882 )
01156          * t) * t) * t) * t) * DAS2R);
01157 
01158       return gmst;
01159 
01160    }  // End of method 'ReferenceFrames::iauGmst00()'
01161 
01162       // Nutation matrix from nutation angles
01163    Matrix<double> ReferenceFrames::iauNmat(const double& epsa,
01164                                            const double& dpsi, 
01165                                            const double& deps)
01166    {
01167       return ( Rx(-(epsa+deps)) * Rz(-dpsi) * Rx(epsa) );
01168    }
01169 
01170 
01171    Matrix<double> ReferenceFrames::enuMatrix(double longitude, double latitude)
01172    {
01173       const double sb = std::sin(latitude);
01174       const double cb = std::cos(latitude);
01175       const double sl = std::sin(longitude);
01176       const double cl = std::cos(longitude);
01177 
01178       double r[3][3]={{-sl,cl,0.0},{-sb*cl,-sb*sl, cb},{ cb*cl,cb*sl,sb}};
01179       
01180       Matrix<double> enuMat(3,3,0.0);
01181       enuMat = &r[0][0];
01182 
01183       return enuMat;
01184 
01185    }  // End of method 'ReferenceFrames::enuMatrix()'
01186 
01187    // return Azimuth Elevation slant
01188    Vector<double> ReferenceFrames::enuToAzElDt(Vector<double> enu)
01189    {
01190       gpstk::Vector<double> r(3,0.0);
01191 
01192       const double rho = std::sqrt(enu(0)*enu(0)+enu(1)*enu(1));
01193 
01194       // Angles
01195       double A = std::atan2(enu(0),enu(1));
01196       A = (A<0.0)? (A+ASConstant::TWO_PI) : A;
01197       double E = std::atan ( enu(2) / rho );
01198 
01199       r(0) = A;
01200       r(1) = E;
01201       r(2) = norm(enu);
01202 
01203       return r;
01204    }
01205 
01206 
01207    void ReferenceFrames::XYZ2BLH(double xyz[3],double blh[3])
01208    {
01209       const double f = ASConstant::f_Earth; //sqrt(0.00669437999013);
01210       const double R_equ = ASConstant::R_Earth; //Equator radius [m]
01211       const double e2 = f*(2.0-f);          // Square of eccentricity
01212       const double e = std::sqrt(e2);
01213 
01214       const double  eps     = 1.0e3*std::numeric_limits<double>::epsilon();;   // Convergence criterion 
01215       const double  epsRequ = eps*R_equ;
01216 
01217 
01218       const double  X = xyz[0];                   // Cartesian coordinates
01219       const double  Y = xyz[1];
01220       const double  Z = xyz[2];
01221       const double  rho2 = X*X + Y*Y;           // Square of distance from z-axis
01222       const double  rho = std::sqrt(rho2+Z*Z);
01223 
01224       // Check validity of input data
01225       if (rho==0.0) 
01226       {
01227          blh[0] = 0.0;
01228          blh[1] = 0.0;
01229          blh[2] = -R_equ;
01230          return;
01231       }
01232 
01233       // Iteration 
01234       double  dZ, dZ_new, SinPhi;
01235       double  ZdZ, Nh, N;
01236 
01237       dZ = e2*Z;
01238       while(1) 
01239       {
01240          ZdZ    =  Z + dZ;
01241          Nh     =  std::sqrt ( rho2 + ZdZ*ZdZ ); 
01242          SinPhi =  ZdZ / Nh;                    // Sine of geodetic latitude
01243          N      =  R_equ / std::sqrt(1.0-e2*SinPhi*SinPhi); 
01244          dZ_new =  N*e2*SinPhi;
01245          if ( std::fabs(dZ-dZ_new) < epsRequ ) break;
01246          dZ = dZ_new;
01247       }
01248 
01249       // latitude, Longitude, altitude
01250       blh[0] = std::atan2 ( ZdZ, std::sqrt(rho2) );
01251       blh[1] = std::atan2 ( Y, X );
01252       blh[2] = Nh - N;
01253 
01254    }  // End of method 'ReferenceFrames::XYZ2BLH()'
01255 
01256    void ReferenceFrames::BLH2XYZ(double blh[3],double xyz[3])
01257    {
01258       const double f = ASConstant::f_Earth; //sqrt(0.00669437999013);
01259       const double a = ASConstant::R_Earth; //Equator radius [m]
01260       const double e2 = f*(2.0-f);          // Square of eccentricity
01261 
01262       double N=a/(std::sqrt(1-e2*std::sin(blh[0])*std::sin(blh[0])));
01263       xyz[0]=(N+blh[2])*std::cos(blh[0])*std::cos(blh[1]);
01264       xyz[1]=(N+blh[2])*std::cos(blh[0])*std::sin(blh[1]);
01265       xyz[2]=(N*(1-e2)+blh[2])*std::sin(blh[0]);
01266 
01267    }  // End of method 'ReferenceFrames::BLH2XYZ()'
01268 
01269 
01270    void ReferenceFrames::XYZ2ENU(double blh[3],double xyz[3],double enu[3])
01271    {
01272       double xyz0[3]={0.0};
01273       BLH2XYZ(blh,xyz0);
01274 
01275       double dxyz[3]={0.0};
01276       dxyz[0] = xyz[0]-xyz0[0];
01277       dxyz[1] = xyz[1]-xyz0[1];
01278       dxyz[2] = xyz[2]-xyz0[2];
01279 
01280       const double sb = std::sin(blh[0]);
01281       const double cb = std::cos(blh[0]);
01282       const double sl = std::sin(blh[1]);
01283       const double cl = std::cos(blh[1]);
01284 
01285       double r[3][3]={{-sl,cl,0.0},{-sb*cl,-sb*sl, cb},{ cb*cl,cb*sl,sb}};
01286       
01287       enu[0] = r[0][0] * dxyz[0] + r[0][1] * dxyz[1] + r[0][2] * dxyz[2];
01288       enu[1] = r[1][0] * dxyz[0] + r[1][1] * dxyz[1] + r[1][2] * dxyz[2];
01289       enu[2] = r[2][0] * dxyz[0] + r[2][1] * dxyz[1] + r[2][2] * dxyz[2];
01290 
01291    }  // End of method 'ReferenceFrames::XYZ2ENU()'
01292 
01293 
01294 }  // End of namespace 'gsptk'
01295 
01296 

Generated on Sun May 19 03:31:12 2013 for GPS ToolKit Software Library by  doxygen 1.3.9.1