Ignore:
Timestamp:
04/08/16 15:40:57 (8 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairSensorActuator/src/geodesie.cpp

    r3 r15  
    88
    99#ifdef _MSC_VER
    10 #   pragma warning(disable:4244)
     10#pragma warning(disable : 4244)
    1111#endif //_MSC_VER
    1212
    1313namespace Geodesie {
    1414/// ////////////////////////////////////////////////////////////////////
    15 Matrice::Matrice(const Matrice & A) {
    16     c0_l0=A.c0_l0;c1_l0=A.c1_l0;c2_l0=A.c2_l0;
    17     c0_l1=A.c0_l1;c1_l1=A.c1_l1;c2_l1=A.c2_l1;
    18     c0_l2=A.c0_l2;c1_l2=A.c1_l2;c2_l2=A.c2_l2;
     15Matrice::Matrice(const Matrice &A) {
     16  c0_l0 = A.c0_l0;
     17  c1_l0 = A.c1_l0;
     18  c2_l0 = A.c2_l0;
     19  c0_l1 = A.c0_l1;
     20  c1_l1 = A.c1_l1;
     21  c2_l1 = A.c2_l1;
     22  c0_l2 = A.c0_l2;
     23  c1_l2 = A.c1_l2;
     24  c2_l2 = A.c2_l2;
    1925}
    2026/// ////////////////////////////////////////////////////////////////////
    2127Matrice::Matrice() {
    22     c0_l0=0.0;c1_l0=0.0;c2_l0=0.0;
    23     c0_l1=0.0;c1_l1=0.0;c2_l1=0.0;
    24     c0_l2=0.0;c1_l2=0.0;c2_l2=0.0;
    25 }
    26 /// ////////////////////////////////////////////////////////////////////
    27 void Matrice::Apply(double v0,double v1,double v2, double& Mv0,double& Mv1,double& Mv2) {
    28     Mv0 = c0_l0*v0 + c1_l0*v1 + c2_l0*v2;
    29     Mv1 = c0_l1*v0 + c1_l1*v1 + c2_l1*v2;
    30     Mv2 = c0_l2*v0 + c1_l2*v1 + c2_l2*v2;
     28  c0_l0 = 0.0;
     29  c1_l0 = 0.0;
     30  c2_l0 = 0.0;
     31  c0_l1 = 0.0;
     32  c1_l1 = 0.0;
     33  c2_l1 = 0.0;
     34  c0_l2 = 0.0;
     35  c1_l2 = 0.0;
     36  c2_l2 = 0.0;
     37}
     38/// ////////////////////////////////////////////////////////////////////
     39void Matrice::Apply(double v0, double v1, double v2, double &Mv0, double &Mv1,
     40                    double &Mv2) {
     41  Mv0 = c0_l0 * v0 + c1_l0 * v1 + c2_l0 * v2;
     42  Mv1 = c0_l1 * v0 + c1_l1 * v1 + c2_l1 * v2;
     43  Mv2 = c0_l2 * v0 + c1_l2 * v1 + c2_l2 * v2;
    3144}
    3245/// ////////////////////////////////////////////////////////////////////
    3346Matrice ProdMat(const Matrice A, const Matrice B) {
    34     Matrice out;
    35 
    36     out.c0_l0=A.c0_l0 * B.c0_l0 + A.c1_l0 * B.c0_l1 + A.c2_l0 * B.c0_l2;
    37     out.c1_l0=A.c0_l0 * B.c1_l0 + A.c1_l0 * B.c1_l1 + A.c2_l0 * B.c1_l2;
    38     out.c2_l0=A.c0_l0 * B.c2_l0 + A.c1_l0 * B.c2_l1 + A.c2_l0 * B.c2_l2;
    39 
    40     out.c0_l1=A.c0_l1 * B.c0_l0 + A.c1_l1 * B.c0_l1 + A.c2_l1 * B.c0_l2;
    41     out.c1_l1=A.c0_l1 * B.c1_l0 + A.c1_l1 * B.c1_l1 + A.c2_l1 * B.c1_l2;
    42     out.c2_l1=A.c0_l1 * B.c2_l0 + A.c1_l1 * B.c2_l1 + A.c2_l1 * B.c2_l2;
    43 
    44     out.c0_l2=A.c0_l2 * B.c0_l0 + A.c1_l2 * B.c0_l1 + A.c2_l2 * B.c0_l2;
    45     out.c1_l2=A.c0_l2 * B.c1_l0 + A.c1_l2 * B.c1_l1 + A.c2_l2 * B.c1_l2;
    46     out.c2_l2=A.c0_l2 * B.c2_l0 + A.c1_l2 * B.c2_l1 + A.c2_l2 * B.c2_l2;
    47     return out;
     47  Matrice out;
     48
     49  out.c0_l0 = A.c0_l0 * B.c0_l0 + A.c1_l0 * B.c0_l1 + A.c2_l0 * B.c0_l2;
     50  out.c1_l0 = A.c0_l0 * B.c1_l0 + A.c1_l0 * B.c1_l1 + A.c2_l0 * B.c1_l2;
     51  out.c2_l0 = A.c0_l0 * B.c2_l0 + A.c1_l0 * B.c2_l1 + A.c2_l0 * B.c2_l2;
     52
     53  out.c0_l1 = A.c0_l1 * B.c0_l0 + A.c1_l1 * B.c0_l1 + A.c2_l1 * B.c0_l2;
     54  out.c1_l1 = A.c0_l1 * B.c1_l0 + A.c1_l1 * B.c1_l1 + A.c2_l1 * B.c1_l2;
     55  out.c2_l1 = A.c0_l1 * B.c2_l0 + A.c1_l1 * B.c2_l1 + A.c2_l1 * B.c2_l2;
     56
     57  out.c0_l2 = A.c0_l2 * B.c0_l0 + A.c1_l2 * B.c0_l1 + A.c2_l2 * B.c0_l2;
     58  out.c1_l2 = A.c0_l2 * B.c1_l0 + A.c1_l2 * B.c1_l1 + A.c2_l2 * B.c1_l2;
     59  out.c2_l2 = A.c0_l2 * B.c2_l0 + A.c1_l2 * B.c2_l1 + A.c2_l2 * B.c2_l2;
     60  return out;
    4861}
    4962
    5063/// ////////////////////////////////////////////////////////////////////
    5164Matrice TransMat(const Matrice A) {
    52     Matrice out;
    53     out.c0_l0=A.c0_l0 ; out.c1_l0 = A.c0_l1 ; out.c2_l0 = A.c0_l2 ;
    54     out.c0_l1=A.c1_l0 ; out.c1_l1 = A.c1_l1 ; out.c2_l1 = A.c1_l2 ;
    55     out.c0_l2=A.c2_l0 ; out.c1_l2 = A.c2_l1 ; out.c2_l2 = A.c2_l2 ;
    56     return out;
    57 }
    58 
    59 /// ////////////////////////////////////////////////////////////////////
    60 void Write(const Matrice A,std::ostream& out) {
    61     out<< A.c0_l0<<"\t"<< A.c1_l0<<"\t"<< A.c2_l0<<"\n";
    62     out<< A.c0_l1<<"\t"<< A.c1_l1<<"\t"<< A.c2_l1<<"\n";
    63     out<< A.c0_l2<<"\t"<< A.c1_l2<<"\t"<< A.c2_l2<<"\n";
    64 }
    65 
    66 /// ////////////////////////////////////////////////////////////////////
    67 Raf98::~Raf98() {
    68     m_dvalues.clear();
    69 }
     65  Matrice out;
     66  out.c0_l0 = A.c0_l0;
     67  out.c1_l0 = A.c0_l1;
     68  out.c2_l0 = A.c0_l2;
     69  out.c0_l1 = A.c1_l0;
     70  out.c1_l1 = A.c1_l1;
     71  out.c2_l1 = A.c1_l2;
     72  out.c0_l2 = A.c2_l0;
     73  out.c1_l2 = A.c2_l1;
     74  out.c2_l2 = A.c2_l2;
     75  return out;
     76}
     77
     78/// ////////////////////////////////////////////////////////////////////
     79void Write(const Matrice A, std::ostream &out) {
     80  out << A.c0_l0 << "\t" << A.c1_l0 << "\t" << A.c2_l0 << "\n";
     81  out << A.c0_l1 << "\t" << A.c1_l1 << "\t" << A.c2_l1 << "\n";
     82  out << A.c0_l2 << "\t" << A.c1_l2 << "\t" << A.c2_l2 << "\n";
     83}
     84
     85/// ////////////////////////////////////////////////////////////////////
     86Raf98::~Raf98() { m_dvalues.clear(); }
    7087
    7188//-----------------------------------------------------------------------------
    72 bool Raf98::Interpol(double longitude, double latitude, double* Hwgs84) const {
    73     *Hwgs84 = 0.0;
    74     if (m_dvalues.size()==0)
    75         return false;
    76     const double longitude_min =  -5.5;
    77     const double longitude_max =  8.5;
    78     if (longitude < longitude_min)
    79         return false;
    80     if (longitude > longitude_max)
    81         return false;
    82 
    83     const double latitude_min =  42;
    84     const double latitude_max =  51.5;
    85     if (latitude < latitude_min)
    86         return false;
    87     if (latitude > latitude_max)
    88         return false;
    89 
    90     //conversion en position
    91     double longPix = (longitude - longitude_min) * 30.;
    92     double latPix  = (latitude_max - latitude) * 40.;
    93 
    94     double RestCol,RestLig;
    95     double  ColIni,LigIni;
    96     RestCol = modf(longPix,&ColIni);
    97     RestLig = modf(latPix,&LigIni);
    98 
    99 
    100     double Zbd = (1.0-RestCol) * (1.0-RestLig) * LitGrille(ColIni  , LigIni  );
    101     Zbd += RestCol * (1.0-RestLig) * LitGrille(ColIni+1, LigIni  );
    102     Zbd += (1.0-RestCol) *   RestLig  * LitGrille(ColIni  , LigIni+1);
    103     Zbd += RestCol * RestLig * LitGrille(ColIni+1, LigIni+1);
    104     *Hwgs84 = Zbd;
    105 
    106 
    107     return true;
    108 }
    109 /// ////////////////////////////////////////////////////////////////////
    110 double Raf98::LitGrille(unsigned int c,unsigned int l) const{
    111     const unsigned int w=421;
    112     //    const unsigned int h=381;
    113     return m_dvalues.at(c+l*w);
    114 }
    115 /// ////////////////////////////////////////////////////////////////////
    116 bool Raf98::Load(const std::string & sin) {
    117     std::ifstream in(sin.c_str());
    118     unsigned int w = 421;
    119     unsigned int h = 381;
    120 
    121     m_dvalues.reserve(w*h);
    122 
    123     char entete[1024];//sur 3 lignes
    124     in.getline(entete,1023);
    125     in.getline(entete,1023);
    126     in.getline(entete,1023);
    127 
    128     char bidon[1024];
    129     double val;
    130     for (unsigned int i=0; i< h; ++i) {
    131         for (unsigned int j=0; j< 52; ++j)  {
    132             for (unsigned int k=0; k< 8; ++k) {
    133                 in >> val;
    134                 m_dvalues.push_back( val );
    135             }
    136             in.getline(bidon,1023);
    137         }
    138         for (unsigned int k=0; k< 5; ++k) {
    139             in >> val;
    140             m_dvalues.push_back( val );
    141         }
    142         in.getline(bidon,1023);
    143         if (!in.good())         {
    144             m_dvalues.clear();
    145             return false;
    146         }
     89bool Raf98::Interpol(double longitude, double latitude, double *Hwgs84) const {
     90  *Hwgs84 = 0.0;
     91  if (m_dvalues.size() == 0)
     92    return false;
     93  const double longitude_min = -5.5;
     94  const double longitude_max = 8.5;
     95  if (longitude < longitude_min)
     96    return false;
     97  if (longitude > longitude_max)
     98    return false;
     99
     100  const double latitude_min = 42;
     101  const double latitude_max = 51.5;
     102  if (latitude < latitude_min)
     103    return false;
     104  if (latitude > latitude_max)
     105    return false;
     106
     107  // conversion en position
     108  double longPix = (longitude - longitude_min) * 30.;
     109  double latPix = (latitude_max - latitude) * 40.;
     110
     111  double RestCol, RestLig;
     112  double ColIni, LigIni;
     113  RestCol = modf(longPix, &ColIni);
     114  RestLig = modf(latPix, &LigIni);
     115
     116  double Zbd = (1.0 - RestCol) * (1.0 - RestLig) * LitGrille(ColIni, LigIni);
     117  Zbd += RestCol * (1.0 - RestLig) * LitGrille(ColIni + 1, LigIni);
     118  Zbd += (1.0 - RestCol) * RestLig * LitGrille(ColIni, LigIni + 1);
     119  Zbd += RestCol * RestLig * LitGrille(ColIni + 1, LigIni + 1);
     120  *Hwgs84 = Zbd;
     121
     122  return true;
     123}
     124/// ////////////////////////////////////////////////////////////////////
     125double Raf98::LitGrille(unsigned int c, unsigned int l) const {
     126  const unsigned int w = 421;
     127  //    const unsigned int h=381;
     128  return m_dvalues.at(c + l * w);
     129}
     130/// ////////////////////////////////////////////////////////////////////
     131bool Raf98::Load(const std::string &sin) {
     132  std::ifstream in(sin.c_str());
     133  unsigned int w = 421;
     134  unsigned int h = 381;
     135
     136  m_dvalues.reserve(w * h);
     137
     138  char entete[1024]; // sur 3 lignes
     139  in.getline(entete, 1023);
     140  in.getline(entete, 1023);
     141  in.getline(entete, 1023);
     142
     143  char bidon[1024];
     144  double val;
     145  for (unsigned int i = 0; i < h; ++i) {
     146    for (unsigned int j = 0; j < 52; ++j) {
     147      for (unsigned int k = 0; k < 8; ++k) {
     148        in >> val;
     149        m_dvalues.push_back(val);
     150      }
     151      in.getline(bidon, 1023);
    147152    }
    148     return in.good();
     153    for (unsigned int k = 0; k < 5; ++k) {
     154      in >> val;
     155      m_dvalues.push_back(val);
     156    }
     157    in.getline(bidon, 1023);
     158    if (!in.good()) {
     159      m_dvalues.clear();
     160      return false;
     161    }
     162  }
     163  return in.good();
    149164}
    150165
    … …  
    155170
    156171/// ////////////////////////////////////////////////////////////////////
    157 //ALGO0001
    158 double Geodesie::LatitueIsometrique(double latitude,double e) {
    159     double li;
    160     li = log(tan(M_PI_4 + latitude/2.)) + e*log( (1-e*sin(latitude))/(1+e*sin(latitude)) )/2;
    161     return li;
    162 }
    163 
    164 /// ////////////////////////////////////////////////////////////////////
    165 //ALGO0002
    166 double Geodesie::LatitueIsometrique2Lat(double latitude_iso,double e,double epsilon) {
    167     double latitude_i=2*atan(exp(latitude_iso)) - M_PI_2;
    168     double latitude_ip1=latitude_i+epsilon*2;
    169     while (fabs(latitude_i-latitude_ip1)>epsilon) {
    170         latitude_i=latitude_ip1;
    171         latitude_ip1=2*atan(
    172                     exp(e*0.5*
    173                         log(
    174                             (1+e*sin(latitude_i))/(1-e*sin(latitude_i))
    175                             )
    176                         )
    177                     *exp(latitude_iso)
    178                     ) - M_PI_2;
    179     }
    180     return latitude_ip1;
    181 }
    182 /// ////////////////////////////////////////////////////////////////////
    183 void Geodesie::Geo2ProjLambert(
    184     double lambda,double phi,
    185     double n, double c,double e,
    186     double lambdac,double xs,double ys,
    187     double& X,double& Y)
    188 {
    189     double lat_iso=LatitueIsometrique(phi,e);
    190     X=xs+c*exp(-n*lat_iso)*sin(n*(lambda-lambdac));
    191     Y=ys-c*exp(-n*lat_iso)*cos(n*(lambda-lambdac));
    192 }
    193 /// ////////////////////////////////////////////////////////////////////
    194 //ALGO0004
    195 void Geodesie::Proj2GeoLambert(
    196     double X,double Y,
    197     double n, double c,double e,
    198     double lambdac,double xs,double ys,
    199     double epsilon,
    200     double& lambda,double& phi)
    201 {
    202     double X_xs=X-xs;
    203     double ys_Y=ys-Y;
    204     double R=sqrt(X_xs*X_xs+ys_Y*ys_Y);
    205     double gamma=atan(X_xs/ys_Y);
    206     lambda=lambdac+gamma/n;
    207     double lat_iso=-1/n*log(fabs(R/c));
    208     phi=LatitueIsometrique2Lat(lat_iso,e,epsilon);
     172// ALGO0001
     173double Geodesie::LatitueIsometrique(double latitude, double e) {
     174  double li;
     175  li = log(tan(M_PI_4 + latitude / 2.)) +
     176       e * log((1 - e * sin(latitude)) / (1 + e * sin(latitude))) / 2;
     177  return li;
     178}
     179
     180/// ////////////////////////////////////////////////////////////////////
     181// ALGO0002
     182double Geodesie::LatitueIsometrique2Lat(double latitude_iso, double e,
     183                                        double epsilon) {
     184  double latitude_i = 2 * atan(exp(latitude_iso)) - M_PI_2;
     185  double latitude_ip1 = latitude_i + epsilon * 2;
     186  while (fabs(latitude_i - latitude_ip1) > epsilon) {
     187    latitude_i = latitude_ip1;
     188    latitude_ip1 = 2 * atan(exp(e * 0.5 * log((1 + e * sin(latitude_i)) /
     189                                              (1 - e * sin(latitude_i)))) *
     190                            exp(latitude_iso)) -
     191                   M_PI_2;
     192  }
     193  return latitude_ip1;
     194}
     195/// ////////////////////////////////////////////////////////////////////
     196void Geodesie::Geo2ProjLambert(double lambda, double phi, double n, double c,
     197                               double e, double lambdac, double xs, double ys,
     198                               double &X, double &Y) {
     199  double lat_iso = LatitueIsometrique(phi, e);
     200  X = xs + c * exp(-n * lat_iso) * sin(n * (lambda - lambdac));
     201  Y = ys - c * exp(-n * lat_iso) * cos(n * (lambda - lambdac));
     202}
     203/// ////////////////////////////////////////////////////////////////////
     204// ALGO0004
     205void Geodesie::Proj2GeoLambert(double X, double Y, double n, double c, double e,
     206                               double lambdac, double xs, double ys,
     207                               double epsilon, double &lambda, double &phi) {
     208  double X_xs = X - xs;
     209  double ys_Y = ys - Y;
     210  double R = sqrt(X_xs * X_xs + ys_Y * ys_Y);
     211  double gamma = atan(X_xs / ys_Y);
     212  lambda = lambdac + gamma / n;
     213  double lat_iso = -1 / n * log(fabs(R / c));
     214  phi = LatitueIsometrique2Lat(lat_iso, e, epsilon);
    209215}
    210216/// ////////////////////////////////////////////////////////////////////
    211217double Geodesie::ConvMerApp(double longitude) {
    212     double phi0_Lambert93 = Deg2Rad(46.5);
    213     double lambda0_Lambert93 = Deg2Rad(3.0);
    214     double conv=-sin(phi0_Lambert93)*(longitude-lambda0_Lambert93);
    215     return conv;
     218  double phi0_Lambert93 = Deg2Rad(46.5);
     219  double lambda0_Lambert93 = Deg2Rad(3.0);
     220  double conv = -sin(phi0_Lambert93) * (longitude - lambda0_Lambert93);
     221  return conv;
    216222}
    217223
    218224////////////////////////////////////////////////////////////////////
    219 void Geodesie::Geographique_2_Lambert93(const Raf98& raf98,double lambda,double phi,double he,Matrice in,double& E,double& N,double& h,Matrice& out) {
    220     Matrice passage;
    221     double conv=Geodesie::ConvMerApp(lambda);
    222     double c_=cos(conv);
    223     double s_=sin(conv);
    224 
    225     passage.c0_l0 = c_;
    226     passage.c0_l1 = s_;
    227     passage.c0_l2 =  0.0;
    228 
    229     passage.c1_l0 = -s_;
    230     passage.c1_l1 = c_;
    231     passage.c1_l2 =  0.0;
    232 
    233     passage.c2_l0 =  0.0;
    234     passage.c2_l1 =  0.0;
    235     passage.c2_l2 =  1.0;
    236 
    237     out=ProdMat(passage,in);
    238     double diff_h;
    239     raf98.Interpol(Rad2Deg(lambda),Rad2Deg(phi),&diff_h);
    240     h=he-diff_h;
    241 
    242     Geodesie::Geo2ProjLambert(
    243                 lambda,phi,
    244                 n_Lambert93,c_Lambert93,e_Lambert93,
    245                 lambda0_Lambert93,xs_Lambert93,ys_Lambert93,
    246                 E,N);
     225void Geodesie::Geographique_2_Lambert93(const Raf98 &raf98, double lambda,
     226                                        double phi, double he, Matrice in,
     227                                        double &E, double &N, double &h,
     228                                        Matrice &out) {
     229  Matrice passage;
     230  double conv = Geodesie::ConvMerApp(lambda);
     231  double c_ = cos(conv);
     232  double s_ = sin(conv);
     233
     234  passage.c0_l0 = c_;
     235  passage.c0_l1 = s_;
     236  passage.c0_l2 = 0.0;
     237
     238  passage.c1_l0 = -s_;
     239  passage.c1_l1 = c_;
     240  passage.c1_l2 = 0.0;
     241
     242  passage.c2_l0 = 0.0;
     243  passage.c2_l1 = 0.0;
     244  passage.c2_l2 = 1.0;
     245
     246  out = ProdMat(passage, in);
     247  double diff_h;
     248  raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h);
     249  h = he - diff_h;
     250
     251  Geodesie::Geo2ProjLambert(lambda, phi, n_Lambert93, c_Lambert93, e_Lambert93,
     252                            lambda0_Lambert93, xs_Lambert93, ys_Lambert93, E,
     253                            N);
    247254}
    248255////////////////////////////////////////////////////////////////////////
    249 void Geodesie::Geographique_2_Lambert93(const Raf98& raf98,double lambda,double phi,double he,double& E,double& N,double& h) {
    250     Geodesie::Geo2ProjLambert(
    251                 lambda,phi,
    252                 n_Lambert93,c_Lambert93,e_Lambert93,
    253                 lambda0_Lambert93,xs_Lambert93,ys_Lambert93,
    254                 E,N);
    255 
    256     double diff_h;
    257     raf98.Interpol(Rad2Deg(lambda),Rad2Deg(phi),&diff_h);
    258     h=he-diff_h;
     256void Geodesie::Geographique_2_Lambert93(const Raf98 &raf98, double lambda,
     257                                        double phi, double he, double &E,
     258                                        double &N, double &h) {
     259  Geodesie::Geo2ProjLambert(lambda, phi, n_Lambert93, c_Lambert93, e_Lambert93,
     260                            lambda0_Lambert93, xs_Lambert93, ys_Lambert93, E,
     261                            N);
     262
     263  double diff_h;
     264  raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h);
     265  h = he - diff_h;
    259266}
    260267/**
    261     Converts Lambert93 coordinates (East, North, Height) into geographical coordinates in radians (Longitude = Rad2Deg(lambda), Latitude = Rad2Deg(phi), Height)
     268    Converts Lambert93 coordinates (East, North, Height) into geographical
     269   coordinates in radians (Longitude = Rad2Deg(lambda), Latitude = Rad2Deg(phi),
     270   Height)
    262271*/
    263 void Geodesie::Lambert93_2_Geographique(const Raf98& raf98,double E,double N,double h,double& lambda,double& phi,double& he) {
    264     Geodesie::Proj2GeoLambert(
    265                 E,N,
    266                 n_Lambert93,c_Lambert93,e_Lambert93,
    267                 lambda0_Lambert93,xs_Lambert93,ys_Lambert93,
    268                 0.0000000000000001,
    269                 lambda,phi);
    270 
    271     double diff_h;
    272     raf98.Interpol(Rad2Deg(lambda),Rad2Deg(phi),&diff_h);
    273     he=h+diff_h;
     272void Geodesie::Lambert93_2_Geographique(const Raf98 &raf98, double E, double N,
     273                                        double h, double &lambda, double &phi,
     274                                        double &he) {
     275  Geodesie::Proj2GeoLambert(E, N, n_Lambert93, c_Lambert93, e_Lambert93,
     276                            lambda0_Lambert93, xs_Lambert93, ys_Lambert93,
     277                            0.0000000000000001, lambda, phi);
     278
     279  double diff_h;
     280  raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h);
     281  he = h + diff_h;
    274282}
    275283////////////////////////////////////////////////////////////////////////
    276 void Geodesie::Lambert93_2_Geographique(const Raf98& raf98,double E,double N,double h,Matrice in,double& lambda,double& phi,double& he,Matrice& out) {
    277     Geodesie::Proj2GeoLambert(
    278                 E,N,
    279                 n_Lambert93,c_Lambert93,e_Lambert93,
    280                 lambda0_Lambert93,xs_Lambert93,ys_Lambert93,
    281                 0.0000000000000001,
    282                 lambda,phi);
    283 
    284     Matrice passage;
    285     double conv=Geodesie::ConvMerApp(lambda);
    286     double c_=cos(conv);
    287     double s_=sin(conv);
    288 
    289     passage.c0_l0 = c_;
    290     passage.c0_l1 = -s_;
    291     passage.c0_l2 =  0.0;
    292 
    293     passage.c1_l0 = s_;
    294     passage.c1_l1 = c_;
    295     passage.c1_l2 =  0.0;
    296 
    297     passage.c2_l0 =  0.0;
    298     passage.c2_l1 =  0.0;
    299     passage.c2_l2 =  1.0;
    300 
    301     out=ProdMat(passage,in);
    302 
    303     double diff_h;
    304     raf98.Interpol(Rad2Deg(lambda),Rad2Deg(phi),&diff_h);
    305     he=h+diff_h;
     284void Geodesie::Lambert93_2_Geographique(const Raf98 &raf98, double E, double N,
     285                                        double h, Matrice in, double &lambda,
     286                                        double &phi, double &he, Matrice &out) {
     287  Geodesie::Proj2GeoLambert(E, N, n_Lambert93, c_Lambert93, e_Lambert93,
     288                            lambda0_Lambert93, xs_Lambert93, ys_Lambert93,
     289                            0.0000000000000001, lambda, phi);
     290
     291  Matrice passage;
     292  double conv = Geodesie::ConvMerApp(lambda);
     293  double c_ = cos(conv);
     294  double s_ = sin(conv);
     295
     296  passage.c0_l0 = c_;
     297  passage.c0_l1 = -s_;
     298  passage.c0_l2 = 0.0;
     299
     300  passage.c1_l0 = s_;
     301  passage.c1_l1 = c_;
     302  passage.c1_l2 = 0.0;
     303
     304  passage.c2_l0 = 0.0;
     305  passage.c2_l1 = 0.0;
     306  passage.c2_l2 = 1.0;
     307
     308  out = ProdMat(passage, in);
     309
     310  double diff_h;
     311  raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h);
     312  he = h + diff_h;
    306313}
    307314
    308315////////////////////////////////////////////////////////////////////////
    309 void Geodesie::Geographique_2_ECEF(double longitude,double latitude,double he,double& x,double& y,double& z) {
    310     const double n = GRS_a / sqrt(1.0 - pow(GRS_e,2) * pow(sin(latitude),2));
    311     x = (n + he) * cos(latitude) * cos(longitude);
    312     y = (n + he) * cos(latitude) * sin(longitude);
    313     z = (n * (1.0 - pow(GRS_e,2)) + he) * sin(latitude);
     316void Geodesie::Geographique_2_ECEF(double longitude, double latitude, double he,
     317                                   double &x, double &y, double &z) {
     318  const double n = GRS_a / sqrt(1.0 - pow(GRS_e, 2) * pow(sin(latitude), 2));
     319  x = (n + he) * cos(latitude) * cos(longitude);
     320  y = (n + he) * cos(latitude) * sin(longitude);
     321  z = (n * (1.0 - pow(GRS_e, 2)) + he) * sin(latitude);
    314322}
    315323
    316324////////////////////////////////////////////////////////////////////////
    317 void Geodesie::ECEF_2_ENU(double x,double y,double z,double& e,double& n,double& u,double lon0,double lat0,double he0) {
    318     double slat = std::sin(lat0);
    319     double clat = std::cos(lat0);
    320     double slon = std::sin(lon0);
    321     double clon = std::cos(lon0);
    322 
    323     Geodesie::Matrice C;
    324     C.c0_l0 = -slon;
    325     C.c1_l0 = clon;
    326 
    327     C.c0_l1 = -clon * slat;
    328     C.c1_l1 = -slon * slat;
    329     C.c2_l1 = clat;
    330 
    331     C.c0_l2 = clon * clat;
    332     C.c1_l2 = slon * clat;
    333     C.c2_l2 = slat;
    334 
    335     double x0, y0, z0;
    336     Geographique_2_ECEF(lon0,lat0,he0, x0,y0,z0);
    337 
    338     x -= x0;
    339     y -= y0;
    340     z -= z0;
    341 
    342     C.Apply(x,y,z, e,n,u);
    343 }
     325void Geodesie::ECEF_2_ENU(double x, double y, double z, double &e, double &n,
     326                          double &u, double lon0, double lat0, double he0) {
     327  double slat = std::sin(lat0);
     328  double clat = std::cos(lat0);
     329  double slon = std::sin(lon0);
     330  double clon = std::cos(lon0);
     331
     332  Geodesie::Matrice C;
     333  C.c0_l0 = -slon;
     334  C.c1_l0 = clon;
     335
     336  C.c0_l1 = -clon * slat;
     337  C.c1_l1 = -slon * slat;
     338  C.c2_l1 = clat;
     339
     340  C.c0_l2 = clon * clat;
     341  C.c1_l2 = slon * clat;
     342  C.c2_l2 = slat;
     343
     344  double x0, y0, z0;
     345  Geographique_2_ECEF(lon0, lat0, he0, x0, y0, z0);
     346
     347  x -= x0;
     348  y -= y0;
     349  z -= z0;
     350
     351  C.Apply(x, y, z, e, n, u);
     352}
Note: See TracChangeset for help on using the changeset viewer.