Changeset 210 in pacpusframework for trunk


Ignore:
Timestamp:
Nov 6, 2013, 11:35:22 AM (11 years ago)
Author:
Marek Kurdej
Message:

Geodesie: formatting.

Location:
trunk
Files:
2 edited

Legend:

Unmodified
Added
Removed

  • trunk/include/Pacpus/PacpusTools/geodesie.h

    r209 r210  
    1818#include "PacpusToolsConfig.h"
    1919
     20#include <Pacpus/kernel/pacpus.h>
     21
    2022#include <boost/math/constants/constants.hpp>
    2123#include <cmath>
    … …  
    3638{
    3739    /// Copy ctor
    38     Matrice(const Matrice & A);
     40    Matrice(Matrice const& A);
    3941    /// Ctor
    4042    Matrice();
    4143    /// @todo Documentation
    42     void Apply(double v0, double v1, double v2, double & Mv0, double & Mv1, double & Mv2);
     44    void Apply(double v0, double v1, double v2, double& Mv0, double& Mv1, double& Mv2);
    4345
    4446    /// @todo Documentation
    … …  
    6466};
    6567
    66 PACPUSTOOLS_API Matrice TransMat(const Matrice A);
    67 PACPUSTOOLS_API Matrice ProdMat(const Matrice A, const Matrice B);
    68 PACPUSTOOLS_API void Write(const Matrice A, std::ostream & out);
     68PACPUSTOOLS_API Matrice TransMat(Matrice const& A);
     69PACPUSTOOLS_API Matrice ProdMat(Matrice const& A, Matrice const& B);
     70PACPUSTOOLS_API PACPUS_DEPRECATED_MSG(void Write(Matrice const& A, std::ostream& out), "use operator<<");
     71PACPUSTOOLS_API std::ostream& operator<<(std::ostream& os, Matrice const& A);
    6972
    7073////////////////////////////////////////////////////////////////////////
    … …  
    7477public:
    7578    /// Ctor of Raf98 class.
    76     Raf98() {}
     79    Raf98();
     80
    7781    /// Dtor of Raf98 class.
    7882    ~Raf98();
    79    
     83
    8084    /// @todo Documentation
    8185    /// @param s filepath
    82     bool Load(const std::string & s);
    83    
     86    bool Load(const std::string& s);
     87
    8488    /// @todo Documentation
    8589    /// @param longitude [degrees]
    8690    /// @param latitude [degrees]
    8791    /// @param Hwgs84 Output: interpolated altitude using WGS84 geoid model [meters]
    88     bool Interpol(double longitude/*deg*/, double latitude/*deg*/, double* Hwgs84) const;
    89    
     92    bool Interpol(double longitude /*deg*/, double latitude /*deg*/, double* Hwgs84) const;
     93
    9094private:
    9195    std::vector<double> m_dvalues;
    92     double LitGrille(unsigned int c,unsigned int l) const;
     96    double LitGrille(unsigned int c, unsigned int l) const;
    9397};
    9498
    … …  
    110114////////////////////////////////////////////////////////////////////////
    111115
    112 const double a_Lambert93=6378137;
    113 const double f_Lambert93=1 / 298.257222101;
    114 const double e_Lambert93=sqrt(f_Lambert93*(2-f_Lambert93));
    115 const double lambda0_Lambert93=Deg2Rad(3.0);//degres
    116 const double phi0_Lambert93=Deg2Rad(46.5);
    117 const double phi1_Lambert93=Deg2Rad(44.0);
    118 const double phi2_Lambert93=Deg2Rad(49.0);//degres
    119 const double X0_Lambert93=700000;//
    120 const double Y0_Lambert93=6600000;//
     116const double a_Lambert93 = 6378137;
     117const double f_Lambert93 = 1 / 298.257222101;
     118const double e_Lambert93 = sqrt(f_Lambert93 * (2 - f_Lambert93));
     119const double lambda0_Lambert93 = Deg2Rad(3.0); //degres
     120const double phi0_Lambert93 = Deg2Rad(46.5);
     121const double phi1_Lambert93 = Deg2Rad(44.0);
     122const double phi2_Lambert93 = Deg2Rad(49.0); //degres
     123const double X0_Lambert93 = 700000; //
     124const double Y0_Lambert93 = 6600000; //
    121125const double n_Lambert93 = 0.7256077650;
    122126const double c_Lambert93 = 11754255.426;
    … …  
    125129
    126130const double GRS_a = 6378137;
    127 const double GRS_f = 1/298.257222101;
    128 const double GRS_b = GRS_a*(1-GRS_f);
    129 const double GRS_e = sqrt((pow(GRS_a,2) - pow(GRS_b,2)) / pow(GRS_a,2));
    130 
    131 ////////////////////////////////////////////////////////////////////////
    132 PACPUSTOOLS_API void Geographique_2_Lambert93(const Raf98& raf98,double lambda,double phi,double he,Matrice in,double& E,double& N,double& h,Matrice& out);
    133 PACPUSTOOLS_API void Geographique_2_Lambert93(const Raf98& raf98,double lambda,double phi,double he,double& E,double& N,double& h);
    134 PACPUSTOOLS_API void Lambert93_2_Geographique(const Raf98& raf98,double E,double N,double h,double& lambda,double& phi,double& he);
    135 PACPUSTOOLS_API void Lambert93_2_Geographique(const Raf98& raf98,double E,double N,double h,Matrice in,double& lambda,double& phi,double& he,Matrice& out);
     131const double GRS_f = 1 / 298.257222101;
     132const double GRS_b = GRS_a * (1 - GRS_f);
     133const double GRS_e = sqrt((pow(GRS_a, 2) - pow(GRS_b, 2)) / pow(GRS_a, 2));
     134
     135////////////////////////////////////////////////////////////////////////
     136PACPUSTOOLS_API void Geographique_2_Lambert93(const Raf98& raf98, double lambda, double phi, double he, Matrice in, double& E, double& N, double& h, Matrice& out);
     137PACPUSTOOLS_API void Geographique_2_Lambert93(const Raf98& raf98, double lambda, double phi, double he, double& E, double& N, double& h);
     138PACPUSTOOLS_API void Lambert93_2_Geographique(const Raf98& raf98, double E, double N, double h, double& lambda, double& phi, double& he);
     139PACPUSTOOLS_API void Lambert93_2_Geographique(const Raf98& raf98, double E, double N, double h, Matrice in, double& lambda, double& phi, double& he, Matrice& out);
    136140/** Convert from geographique to ECEF.
    137141 * @param[in] longitude Longitude in radian.
    … …  
    145149 * @param[in] he0 Height of the origin in radian.
    146150 */
    147 PACPUSTOOLS_API void ECEF_2_ENU(double x,double y,double z,double& e,double& n,double& u,double lon0,double lat0,double he0);
     151PACPUSTOOLS_API void ECEF_2_ENU(double x, double y, double z, double& e, double& n, double& u, double lon0, double lat0, double he0);
    148152////////////////////////////////////////////////////////////////////////
    149153
    150154///ALGO0001
    151155/// @todo Rename
    152 PACPUSTOOLS_API double LatitueIsometrique(double latitude,double e);
     156PACPUSTOOLS_API double LatitueIsometrique(double latitude, double e);
    153157///ALGO0002
    154158/// @todo Rename
    155 PACPUSTOOLS_API double LatitueIsometrique2Lat(double latitude_iso,double e,double epsilon);
     159PACPUSTOOLS_API double LatitueIsometrique2Lat(double latitude_iso, double e, double epsilon);
    156160
    157161///ALGO0003
    158162PACPUSTOOLS_API void Geo2ProjLambert(
    159     double lambda,double phi,
    160     double n, double c,double e,
    161     double lambdac,double xs,double ys,
    162     double& X,double& Y);
     163    double lambda, double phi,
     164    double n, double c, double e,
     165    double lambdac, double xs, double ys,
     166    double& X, double& Y);
    163167///ALGO0004
    164168PACPUSTOOLS_API void Proj2GeoLambert(
    165     double X,double Y,
    166     double n, double c,double e,
    167     double lambdac,double xs,double ys,
     169    double X, double Y,
     170    double n, double c, double e,
     171    double lambdac, double xs, double ys,
    168172    double epsilon,
    169     double& lambda,double& phi);
     173    double& lambda, double& phi);
    170174
    171175PACPUSTOOLS_API double ConvMerApp(double longitude);
    … …  
    175179*/
    176180template <typename _T1, typename _T2>
    177 void cartesianToPolar(const _T1 x, const _T1 y, _T2 & r, _T2 & theta) {
    178     r = std::sqrt(x*x + y*y);
     181void cartesianToPolar(const _T1 x, const _T1 y, _T2& r, _T2& theta)
     182{
     183    r = std::sqrt(x * x + y * y);
    179184    theta = std::atan2(x, y);
    180185}
    … …  
    184189*/
    185190template <typename _T1, typename _T2>
    186 void polarToCartesian(const _T1 r, const _T1 theta, _T2 & x, _T2 & y) {
     191void polarToCartesian(const _T1 r, const _T1 theta, _T2& x, _T2& y)
     192{
    187193    x = r * std::cos(theta);
    188194    y = r * std::sin(theta);
    … …  
    194200*/
    195201template <typename _T1, typename _T2>
    196 void cartesianToSpherical(const _T1 x, const _T1 y, const _T1 z, _T2 & r, _T2 & theta, _T2 & phi) {
    197     r = std::sqrt(x*x + y*y + z*z);
     202void cartesianToSpherical(const _T1 x, const _T1 y, const _T1 z, _T2& r, _T2& theta, _T2& phi)
     203{
     204    r = std::sqrt(x * x + y * y + z * z);
    198205    theta = std::acos(z / r);
    199206    phi = std::atan2(y, x);
    … …  
    205212*/
    206213template <typename _T1, typename _T2>
    207 void sphericalToCartesian(const _T1 r, const _T1 theta, const _T1 phi, _T2 & x, _T2 & y, _T2 & z) {
     214void sphericalToCartesian(const _T1 r, const _T1 theta, const _T1 phi, _T2& x, _T2& y, _T2& z)
     215{
    208216    x = r * std::sin(theta) * std::cos(phi);
    209217    y = r * std::sin(theta) * std::sin(phi);
    … …  
    217225namespace Geodesie
    218226{
    219     using namespace Geodesy;
     227using namespace Geodesy;
    220228} // namespace Geodesie
    221229

  • trunk/src/PacpusTools/src/geodesie.cpp

    r209 r210  
    1111#include <fstream>
    1212
    13 using boost::math::constants::pi;
    14 using boost::math::constants::half_pi;
     13using ::boost::math::constants::pi;
     14using ::boost::math::constants::half_pi;
     15using ::std::abs;
     16using ::std::atan;
     17using ::std::exp;
     18using ::std::ifstream;
     19using ::std::ostream;
     20using ::std::log;
     21using ::std::sin;
     22using ::std::string;
    1523
    1624#ifdef _MSC_VER
    17 #   pragma warning(disable:4244)
     25#pragma warning(disable : 4244)
    1826#endif //_MSC_VER
    1927
    … …  
    2129{
    2230
    23 /// ////////////////////////////////////////////////////////////////////
    24 Matrice::Matrice(const Matrice & A)
    25 {
    26     c0_l0=A.c0_l0;c1_l0=A.c1_l0;c2_l0=A.c2_l0;
    27     c0_l1=A.c0_l1;c1_l1=A.c1_l1;c2_l1=A.c2_l1;
    28     c0_l2=A.c0_l2;c1_l2=A.c1_l2;c2_l2=A.c2_l2;
    29 }
    30 
    31 /// ////////////////////////////////////////////////////////////////////
     31////////////////////////////////////////////////////////////////////////////////
     32Matrice::Matrice(const Matrice& A)
     33{
     34    c0_l0 = A.c0_l0;
     35    c1_l0 = A.c1_l0;
     36    c2_l0 = A.c2_l0;
     37    c0_l1 = A.c0_l1;
     38    c1_l1 = A.c1_l1;
     39    c2_l1 = A.c2_l1;
     40    c0_l2 = A.c0_l2;
     41    c1_l2 = A.c1_l2;
     42    c2_l2 = A.c2_l2;
     43}
     44
     45////////////////////////////////////////////////////////////////////////////////
    3246Matrice::Matrice()
    3347{
    34     c0_l0=0.0;c1_l0=0.0;c2_l0=0.0;
    35     c0_l1=0.0;c1_l1=0.0;c2_l1=0.0;
    36     c0_l2=0.0;c1_l2=0.0;c2_l2=0.0;
    37 }
    38 
    39 /// ////////////////////////////////////////////////////////////////////
    40 void Matrice::Apply(double v0,double v1,double v2, double& Mv0,double& Mv1,double& Mv2)
    41 {
    42     Mv0 = c0_l0*v0 + c1_l0*v1 + c2_l0*v2;
    43     Mv1 = c0_l1*v0 + c1_l1*v1 + c2_l1*v2;
    44     Mv2 = c0_l2*v0 + c1_l2*v1 + c2_l2*v2;
    45 }
    46 
    47 /// ////////////////////////////////////////////////////////////////////
    48 Matrice ProdMat(const Matrice A, const Matrice B)
     48    c0_l0 = 0.0;
     49    c1_l0 = 0.0;
     50    c2_l0 = 0.0;
     51    c0_l1 = 0.0;
     52    c1_l1 = 0.0;
     53    c2_l1 = 0.0;
     54    c0_l2 = 0.0;
     55    c1_l2 = 0.0;
     56    c2_l2 = 0.0;
     57}
     58
     59////////////////////////////////////////////////////////////////////////////////
     60void Matrice::Apply(double v0, double v1, double v2, double& Mv0, double& Mv1, double& Mv2)
     61{
     62    Mv0 = c0_l0 * v0 + c1_l0 * v1 + c2_l0 * v2;
     63    Mv1 = c0_l1 * v0 + c1_l1 * v1 + c2_l1 * v2;
     64    Mv2 = c0_l2 * v0 + c1_l2 * v1 + c2_l2 * v2;
     65}
     66
     67////////////////////////////////////////////////////////////////////////////////
     68Matrice ProdMat(Matrice const& A, Matrice const& B)
    4969{
    5070    Matrice out;
    5171
    52     out.c0_l0=A.c0_l0 * B.c0_l0 + A.c1_l0 * B.c0_l1 + A.c2_l0 * B.c0_l2;
    53     out.c1_l0=A.c0_l0 * B.c1_l0 + A.c1_l0 * B.c1_l1 + A.c2_l0 * B.c1_l2;
    54     out.c2_l0=A.c0_l0 * B.c2_l0 + A.c1_l0 * B.c2_l1 + A.c2_l0 * B.c2_l2;
    55 
    56     out.c0_l1=A.c0_l1 * B.c0_l0 + A.c1_l1 * B.c0_l1 + A.c2_l1 * B.c0_l2;
    57     out.c1_l1=A.c0_l1 * B.c1_l0 + A.c1_l1 * B.c1_l1 + A.c2_l1 * B.c1_l2;
    58     out.c2_l1=A.c0_l1 * B.c2_l0 + A.c1_l1 * B.c2_l1 + A.c2_l1 * B.c2_l2;
    59 
    60     out.c0_l2=A.c0_l2 * B.c0_l0 + A.c1_l2 * B.c0_l1 + A.c2_l2 * B.c0_l2;
    61     out.c1_l2=A.c0_l2 * B.c1_l0 + A.c1_l2 * B.c1_l1 + A.c2_l2 * B.c1_l2;
    62     out.c2_l2=A.c0_l2 * B.c2_l0 + A.c1_l2 * B.c2_l1 + A.c2_l2 * B.c2_l2;
     72    out.c0_l0 = A.c0_l0 * B.c0_l0 + A.c1_l0 * B.c0_l1 + A.c2_l0 * B.c0_l2;
     73    out.c1_l0 = A.c0_l0 * B.c1_l0 + A.c1_l0 * B.c1_l1 + A.c2_l0 * B.c1_l2;
     74    out.c2_l0 = A.c0_l0 * B.c2_l0 + A.c1_l0 * B.c2_l1 + A.c2_l0 * B.c2_l2;
     75
     76    out.c0_l1 = A.c0_l1 * B.c0_l0 + A.c1_l1 * B.c0_l1 + A.c2_l1 * B.c0_l2;
     77    out.c1_l1 = A.c0_l1 * B.c1_l0 + A.c1_l1 * B.c1_l1 + A.c2_l1 * B.c1_l2;
     78    out.c2_l1 = A.c0_l1 * B.c2_l0 + A.c1_l1 * B.c2_l1 + A.c2_l1 * B.c2_l2;
     79
     80    out.c0_l2 = A.c0_l2 * B.c0_l0 + A.c1_l2 * B.c0_l1 + A.c2_l2 * B.c0_l2;
     81    out.c1_l2 = A.c0_l2 * B.c1_l0 + A.c1_l2 * B.c1_l1 + A.c2_l2 * B.c1_l2;
     82    out.c2_l2 = A.c0_l2 * B.c2_l0 + A.c1_l2 * B.c2_l1 + A.c2_l2 * B.c2_l2;
    6383    return out;
    6484}
    6585
    66 /// ////////////////////////////////////////////////////////////////////
    67 Matrice TransMat(const Matrice A)
     86////////////////////////////////////////////////////////////////////////////////
     87Matrice TransMat(Matrice const& A)
    6888{
    6989    Matrice out;
    70     out.c0_l0=A.c0_l0 ; out.c1_l0 = A.c0_l1 ; out.c2_l0 = A.c0_l2 ;
    71     out.c0_l1=A.c1_l0 ; out.c1_l1 = A.c1_l1 ; out.c2_l1 = A.c1_l2 ;
    72     out.c0_l2=A.c2_l0 ; out.c1_l2 = A.c2_l1 ; out.c2_l2 = A.c2_l2 ;
     90    out.c0_l0 = A.c0_l0;
     91    out.c1_l0 = A.c0_l1;
     92    out.c2_l0 = A.c0_l2;
     93    out.c0_l1 = A.c1_l0;
     94    out.c1_l1 = A.c1_l1;
     95    out.c2_l1 = A.c1_l2;
     96    out.c0_l2 = A.c2_l0;
     97    out.c1_l2 = A.c2_l1;
     98    out.c2_l2 = A.c2_l2;
    7399    return out;
    74100}
    75101
    76 /// ////////////////////////////////////////////////////////////////////
    77 void Write(const Matrice A,std::ostream& out) {
    78     out<< A.c0_l0<<"\t"<< A.c1_l0<<"\t"<< A.c2_l0<<"\n";
    79     out<< A.c0_l1<<"\t"<< A.c1_l1<<"\t"<< A.c2_l1<<"\n";
    80     out<< A.c0_l2<<"\t"<< A.c1_l2<<"\t"<< A.c2_l2<<"\n";
    81 }
    82 
    83 /// ////////////////////////////////////////////////////////////////////
    84 Raf98::~Raf98() {
     102////////////////////////////////////////////////////////////////////////////////
     103ostream& operator<<(ostream& os, Matrice const& A)
     104{
     105    os << A.c0_l0 << "\t" << A.c1_l0 << "\t" << A.c2_l0 << "\n";
     106    os << A.c0_l1 << "\t" << A.c1_l1 << "\t" << A.c2_l1 << "\n";
     107    os << A.c0_l2 << "\t" << A.c1_l2 << "\t" << A.c2_l2 << "\n";
     108    return os;
     109}
     110
     111void Write(Matrice const& A, ostream& out)
     112{
     113    out << A;
     114}
     115
     116////////////////////////////////////////////////////////////////////////////////
     117Raf98::Raf98()
     118{
     119}
     120
     121////////////////////////////////////////////////////////////////////////////////
     122Raf98::~Raf98()
     123{
    85124    m_dvalues.clear();
    86125}
    87126
    88 //-----------------------------------------------------------------------------
    89 bool Raf98::Interpol(double longitude, double latitude, double* Hwgs84) const {
     127////////////////////////////////////////////////////////////////////////////////
     128bool Raf98::Interpol(double longitude, double latitude, double* Hwgs84) const
     129{
    90130    *Hwgs84 = 0.0;
    91     if (m_dvalues.size()==0)
     131    if (m_dvalues.size() == 0) {
    92132        return false;
    93     const double longitude_min =  -5.5;
    94     const double longitude_max =  8.5;
    95     if (longitude < longitude_min)
     133    }
     134    const double longitude_min = -5.5;
     135    const double longitude_max = 8.5;
     136    if (longitude < longitude_min) {
    96137        return false;
    97     if (longitude > longitude_max)
     138    }
     139    if (longitude > longitude_max) {
    98140        return false;
    99 
    100     const double latitude_min =  42;
    101     const double latitude_max =  51.5;
    102     if (latitude < latitude_min)
     141    }
     142    const double latitude_min = 42;
     143    const double latitude_max = 51.5;
     144    if (latitude < latitude_min) {
    103145        return false;
    104     if (latitude > latitude_max)
     146    }
     147    if (latitude > latitude_max) {
    105148        return false;
    106 
     149    }
    107150    //conversion en position
    108151    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    
    117     double Zbd = (1.0-RestCol) * (1.0-RestLig) * LitGrille(ColIni  , LigIni  );
    118     Zbd += RestCol * (1.0-RestLig) * LitGrille(ColIni+1, LigIni  );
    119     Zbd += (1.0-RestCol) *   RestLig  * LitGrille(ColIni  , LigIni+1);
    120     Zbd += RestCol * RestLig * LitGrille(ColIni+1, LigIni+1);
     152    double latPix = (latitude_max - latitude) * 40.;
     153
     154    double RestCol, RestLig;
     155    double ColIni, LigIni;
     156    RestCol = modf(longPix, &ColIni);
     157    RestLig = modf(latPix, &LigIni);
     158
     159    double Zbd = (1.0 - RestCol) * (1.0 - RestLig) * LitGrille(ColIni, LigIni);
     160    Zbd += RestCol * (1.0 - RestLig) * LitGrille(ColIni + 1, LigIni);
     161    Zbd += (1.0 - RestCol) * RestLig * LitGrille(ColIni, LigIni + 1);
     162    Zbd += RestCol * RestLig * LitGrille(ColIni + 1, LigIni + 1);
    121163    *Hwgs84 = Zbd;
    122164
    123 
    124165    return true;
    125166}
    126167
    127 /// ////////////////////////////////////////////////////////////////////
    128 double Raf98::LitGrille(unsigned int c,unsigned int l) const
    129 {
    130     const unsigned int w=421;
     168////////////////////////////////////////////////////////////////////////////////
     169double Raf98::LitGrille(unsigned int c, unsigned int l) const
     170{
     171    const unsigned int w = 421;
    131172    //    const unsigned int h=381;
    132     return m_dvalues.at(c+l*w);
    133 }
    134 
    135 /// ////////////////////////////////////////////////////////////////////
    136 bool Raf98::Load(const std::string & s)
    137 {
    138     std::ifstream in(s.c_str());
     173    return m_dvalues.at(c + l * w);
     174}
     175
     176////////////////////////////////////////////////////////////////////////////////
     177bool Raf98::Load(const string& s)
     178{
     179    ifstream in(s.c_str());
    139180    unsigned int w = 421;
    140181    unsigned int h = 381;
    141    
    142     m_dvalues.reserve(w*h);
    143 
    144     char entete[1024];//sur 3 lignes
    145     in.getline(entete,1023);
    146     in.getline(entete,1023);
    147     in.getline(entete,1023);
     182
     183    m_dvalues.reserve(w * h);
     184
     185    char entete[1024]; //sur 3 lignes
     186    in.getline(entete, 1023);
     187    in.getline(entete, 1023);
     188    in.getline(entete, 1023);
    148189
    149190    char bidon[1024];
    150191    double val;
    151     for (unsigned int i=0; i< h; ++i) {
    152         for (unsigned int j=0; j< 52; ++j) {
    153             for (unsigned int k=0; k< 8; ++k) {
     192    for (unsigned int i = 0; i < h; ++i) {
     193        for (unsigned int j = 0; j < 52; ++j) {
     194            for (unsigned int k = 0; k < 8; ++k) {
    154195                in >> val;
    155                 m_dvalues.push_back( val );
     196                m_dvalues.push_back(val);
    156197            }
    157             in.getline(bidon,1023);
     198            in.getline(bidon, 1023);
    158199        }
    159         for (unsigned int k=0; k< 5; ++k) {
     200        for (unsigned int k = 0; k < 5; ++k) {
    160201            in >> val;
    161             m_dvalues.push_back( val );
     202            m_dvalues.push_back(val);
    162203        }
    163         in.getline(bidon,1023);
    164         if (!in.good())         {
     204        in.getline(bidon, 1023);
     205        if (!in.good()) {
    165206            m_dvalues.clear();
    166207            return false;
    … …  
    172213} // namespace Geodesy
    173214
    174 /// ////////////////////////////////////////////////////////////////////
    175 /// ////////////////////////////////////////////////////////////////////
    176 
    177 /// ////////////////////////////////////////////////////////////////////
     215////////////////////////////////////////////////////////////////////////////////
     216////////////////////////////////////////////////////////////////////////////////
     217
     218////////////////////////////////////////////////////////////////////////////////
    178219//ALGO0001
    179220double Geodesy::LatitueIsometrique(double latitude, double e)
    180221{
    181     using ::std::log;
    182     using ::std::sin;
    183     using ::std::tan;
    184 
    185     double li = log(tan(pi<double>() / 4. + latitude/2.)) + e*log( (1-e*sin(latitude))/(1+e*sin(latitude)) )/2;
     222    double li = log(tan(pi<double>() / 4. + latitude / 2.)) + e * log((1 - e * sin(latitude)) / (1 + e * sin(latitude))) / 2;
    186223    return li;
    187224}
    188225
    189 /// ////////////////////////////////////////////////////////////////////
     226////////////////////////////////////////////////////////////////////////////////
    190227//ALGO0002
    191 double Geodesy::LatitueIsometrique2Lat(double latitude_iso,double e,double epsilon)
    192 {
    193     using ::std::atan;
    194     using ::std::exp;
    195     using ::std::abs;
    196     using ::std::log;
    197     using ::std::sin;
    198 
     228double Geodesy::LatitueIsometrique2Lat(double latitude_iso, double e, double epsilon)
     229{
    199230    double latitude_i = 2 * atan(exp(latitude_iso)) - half_pi<double>();
    200231    double latitude_ip1 = latitude_i + epsilon * 2;
    201     while (abs(latitude_i-latitude_ip1) > epsilon) {
     232    while (abs(latitude_i - latitude_ip1) > epsilon) {
    202233        latitude_i = latitude_ip1;
    203234        latitude_ip1 = 2 * atan(
    204                     exp(e*0.5*
    205                         log(
    206                             (1+e*sin(latitude_i))/(1-e*sin(latitude_i))
    207                             )
    208                         )
    209                     *exp(latitude_iso)
    210                     ) - half_pi<double>();
     235                               exp(e * 0.5 * log(
     236                                                 (1 + e * sin(latitude_i)) / (1 - e * sin(latitude_i))))
     237                               * exp(latitude_iso)) - half_pi<double>();
    211238    }
    212239    return latitude_ip1;
    213240}
    214 /// ////////////////////////////////////////////////////////////////////
     241
     242////////////////////////////////////////////////////////////////////////////////
    215243void Geodesy::Geo2ProjLambert(
    216     double lambda,double phi,
    217     double n, double c,double e,
    218     double lambdac,double xs,double ys,
    219     double& X,double& Y)
    220 {
    221     double lat_iso=LatitueIsometrique(phi,e);
    222     X=xs+c*exp(-n*lat_iso)*sin(n*(lambda-lambdac));
    223     Y=ys-c*exp(-n*lat_iso)*cos(n*(lambda-lambdac));
    224 }
    225 /// ////////////////////////////////////////////////////////////////////
     244    double lambda, double phi,
     245    double n, double c, double e,
     246    double lambdac, double xs, double ys,
     247    double& X, double& Y)
     248{
     249    double lat_iso = LatitueIsometrique(phi, e);
     250    X = xs + c * exp(-n * lat_iso) * sin(n * (lambda - lambdac));
     251    Y = ys - c * exp(-n * lat_iso) * cos(n * (lambda - lambdac));
     252}
     253
     254////////////////////////////////////////////////////////////////////////////////
    226255//ALGO0004
    227256void Geodesy::Proj2GeoLambert(
    228     double X,double Y,
    229     double n, double c,double e,
    230     double lambdac,double xs,double ys,
     257    double X, double Y,
     258    double n, double c, double e,
     259    double lambdac, double xs, double ys,
    231260    double epsilon,
    232     double& lambda,double& phi)
    233 {
    234     double X_xs=X-xs;
    235     double ys_Y=ys-Y;
    236     double R=sqrt(X_xs*X_xs+ys_Y*ys_Y);
    237     double gamma=atan(X_xs/ys_Y);
    238     lambda=lambdac+gamma/n;
    239     double lat_iso=-1/n*log(fabs(R/c));
    240     phi=LatitueIsometrique2Lat(lat_iso,e,epsilon);
    241 }
    242 /// ////////////////////////////////////////////////////////////////////
    243 double Geodesy::ConvMerApp(double longitude) {
     261    double& lambda, double& phi)
     262{
     263    double X_xs = X - xs;
     264    double ys_Y = ys - Y;
     265    double R = sqrt(X_xs * X_xs + ys_Y * ys_Y);
     266    double gamma = atan(X_xs / ys_Y);
     267    lambda = lambdac + gamma / n;
     268    double lat_iso = -1 / n * log(fabs(R / c));
     269    phi = LatitueIsometrique2Lat(lat_iso, e, epsilon);
     270}
     271
     272////////////////////////////////////////////////////////////////////////////////
     273double Geodesy::ConvMerApp(double longitude)
     274{
    244275    double phi0_Lambert93 = Deg2Rad(46.5);
    245276    double lambda0_Lambert93 = Deg2Rad(3.0);
    246     double conv=-sin(phi0_Lambert93)*(longitude-lambda0_Lambert93);
    247     return  conv;
     277    double conv = -sin(phi0_Lambert93) * (longitude - lambda0_Lambert93);
     278    return conv;
    248279}
    249280
    250281////////////////////////////////////////////////////////////////////
    251 void Geodesy::Geographique_2_Lambert93(const Raf98& raf98,double lambda,double phi,double he,Matrice in,double& E,double& N,double& h,Matrice& out)
     282void Geodesy::Geographique_2_Lambert93(const Raf98& raf98, double lambda, double phi, double he, Matrice in, double& E, double& N, double& h, Matrice& out)
    252283{
    253284    Matrice passage;
    254     double conv=Geodesy::ConvMerApp(lambda);
    255     double c_=cos(conv);
    256     double s_=sin(conv);
     285    double conv = Geodesy::ConvMerApp(lambda);
     286    double c_ = cos(conv);
     287    double s_ = sin(conv);
    257288
    258289    passage.c0_l0 = c_;
    259290    passage.c0_l1 = s_;
    260     passage.c0_l2 =  0.0;
     291    passage.c0_l2 = 0.0;
    261292
    262293    passage.c1_l0 = -s_;
    263294    passage.c1_l1 = c_;
    264     passage.c1_l2 =  0.0;
    265 
    266     passage.c2_l0 =  0.0;
    267     passage.c2_l1 =  0.0;
    268     passage.c2_l2 =  1.0;
    269    
    270     out=ProdMat(passage,in);
     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);
    271302    double diff_h;
    272     raf98.Interpol(Rad2Deg(lambda),Rad2Deg(phi),&diff_h);
    273     h=he-diff_h;
    274    
     303    raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h);
     304    h = he - diff_h;
     305
    275306    Geodesy::Geo2ProjLambert(
    276                 lambda,phi,
    277                 n_Lambert93,c_Lambert93,e_Lambert93,
    278                 lambda0_Lambert93,xs_Lambert93,ys_Lambert93,
    279                 E,N);
    280 }
     307        lambda, phi,
     308        n_Lambert93, c_Lambert93, e_Lambert93,
     309        lambda0_Lambert93, xs_Lambert93, ys_Lambert93,
     310        E, N);
     311}
     312
    281313////////////////////////////////////////////////////////////////////////
    282 void Geodesy::Geographique_2_Lambert93(const Raf98& raf98,double lambda,double phi,double he,double& E,double& N,double& h) {
     314void Geodesy::Geographique_2_Lambert93(const Raf98& raf98, double lambda, double phi, double he, double& E, double& N, double& h)
     315{
    283316    Geodesy::Geo2ProjLambert(
    284                 lambda,phi,
    285                 n_Lambert93,c_Lambert93,e_Lambert93,
    286                 lambda0_Lambert93,xs_Lambert93,ys_Lambert93,
    287                 E,N);
     317        lambda, phi,
     318        n_Lambert93, c_Lambert93, e_Lambert93,
     319        lambda0_Lambert93, xs_Lambert93, ys_Lambert93,
     320        E, N);
    288321
    289322    double diff_h;
    290     raf98.Interpol(Rad2Deg(lambda),Rad2Deg(phi),&diff_h);
    291     h=he-diff_h;
     323    raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h);
     324    h = he - diff_h;
    292325}
    293326
    294327/// Converts Lambert93 coordinates (East, North, Height) into geographical coordinates in radians (Longitude = Rad2Deg(lambda), Latitude = Rad2Deg(phi), Height)
    295 void Geodesy::Lambert93_2_Geographique(const Raf98& raf98,double E,double N,double h,double& lambda,double& phi,double& he) {
     328void Geodesy::Lambert93_2_Geographique(const Raf98& raf98, double E, double N, double h, double& lambda, double& phi, double& he)
     329{
    296330    Geodesy::Proj2GeoLambert(
    297                 E,N,
    298                 n_Lambert93,c_Lambert93,e_Lambert93,
    299                 lambda0_Lambert93,xs_Lambert93,ys_Lambert93,
    300                 0.0000000000000001,
    301                 lambda,phi);
     331        E, N,
     332        n_Lambert93, c_Lambert93, e_Lambert93,
     333        lambda0_Lambert93, xs_Lambert93, ys_Lambert93,
     334        0.0000000000000001,
     335        lambda, phi);
    302336
    303337    double diff_h;
    304     raf98.Interpol(Rad2Deg(lambda),Rad2Deg(phi),&diff_h);
    305     he=h+diff_h;
     338    raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h);
     339    he = h + diff_h;
    306340}
    307341
    308342////////////////////////////////////////////////////////////////////////
    309 void Geodesy::Lambert93_2_Geographique(const Raf98& raf98,double E,double N,double h,Matrice in,double& lambda,double& phi,double& he,Matrice& out) {
     343void Geodesy::Lambert93_2_Geographique(const Raf98& raf98, double E, double N, double h, Matrice in, double& lambda, double& phi, double& he, Matrice& out)
     344{
    310345    Geodesy::Proj2GeoLambert(
    311                 E,N,
    312                 n_Lambert93,c_Lambert93,e_Lambert93,
    313                 lambda0_Lambert93,xs_Lambert93,ys_Lambert93,
    314                 0.0000000000000001,
    315                 lambda,phi);
     346        E, N,
     347        n_Lambert93, c_Lambert93, e_Lambert93,
     348        lambda0_Lambert93, xs_Lambert93, ys_Lambert93,
     349        0.0000000000000001,
     350        lambda, phi);
    316351
    317352    Matrice passage;
    318     double conv=Geodesy::ConvMerApp(lambda);
    319     double c_=cos(conv);
    320     double s_=sin(conv);
     353    double conv = Geodesy::ConvMerApp(lambda);
     354    double c_ = cos(conv);
     355    double s_ = sin(conv);
    321356
    322357    passage.c0_l0 = c_;
    323358    passage.c0_l1 = -s_;
    324     passage.c0_l2 =  0.0;
     359    passage.c0_l2 = 0.0;
    325360
    326361    passage.c1_l0 = s_;
    327362    passage.c1_l1 = c_;
    328     passage.c1_l2 =  0.0;
    329 
    330     passage.c2_l0 =  0.0;
    331     passage.c2_l1 =  0.0;
    332     passage.c2_l2 =  1.0;
    333    
    334     out=ProdMat(passage,in);
    335    
     363    passage.c1_l2 = 0.0;
     364
     365    passage.c2_l0 = 0.0;
     366    passage.c2_l1 = 0.0;
     367    passage.c2_l2 = 1.0;
     368
     369    out = ProdMat(passage, in);
     370
    336371    double diff_h;
    337     raf98.Interpol(Rad2Deg(lambda),Rad2Deg(phi),&diff_h);
    338     he=h+diff_h;
     372    raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h);
     373    he = h + diff_h;
    339374}
    340375
    341376////////////////////////////////////////////////////////////////////////
    342 void Geodesy::Geographique_2_ECEF(double longitude,double latitude,double he,double& x,double& y,double& z) {
    343     const double n = GRS_a / sqrt(1.0 - pow(GRS_e,2) * pow(sin(latitude),2));
     377void Geodesy::Geographique_2_ECEF(double longitude, double latitude, double he, double& x, double& y, double& z)
     378{
     379    const double n = GRS_a / sqrt(1.0 - pow(GRS_e, 2) * pow(sin(latitude), 2));
    344380    x = (n + he) * cos(latitude) * cos(longitude);
    345381    y = (n + he) * cos(latitude) * sin(longitude);
    346     z = (n * (1.0 - pow(GRS_e,2)) + he) * sin(latitude);
     382    z = (n * (1.0 - pow(GRS_e, 2)) + he) * sin(latitude);
    347383}
    348384
    349385////////////////////////////////////////////////////////////////////////
    350 void Geodesy::ECEF_2_ENU(double x,double y,double z,double& e,double& n,double& u,double lon0,double lat0,double he0) {
    351     double slat = std::sin(lat0);
    352     double clat = std::cos(lat0);
    353     double slon = std::sin(lon0);
    354     double clon = std::cos(lon0);
    355 
    356     Geodesy::Matrice C;
     386void Geodesy::ECEF_2_ENU(double x, double y, double z, double& e, double& n, double& u, double lon0, double lat0, double he0)
     387{
     388    double slat = sin(lat0);
     389    double clat = cos(lat0);
     390    double slon = sin(lon0);
     391    double clon = cos(lon0);
     392
     393    Matrice C;
    357394    C.c0_l0 = -slon;
    358395    C.c1_l0 = clon;
    … …  
    367404
    368405    double x0, y0, z0;
    369     Geographique_2_ECEF(lon0,lat0,he0, x0,y0,z0);
     406    Geographique_2_ECEF(lon0, lat0, he0, x0, y0, z0);
    370407
    371408    x -= x0;
    … …  
    373410    z -= z0;
    374411
    375     C.Apply(x,y,z, e,n,u);
     412    C.Apply(x, y, z, e, n, u);
    376413}
    377414
    … …  
    386423    float s3 = sin(angle.z());
    387424
    388 
    389425    // Source : https://en.wikipedia.org/wiki/Euler_angles
    390     return QMatrix4x4 (  c1*c2, c1*s2*s3-c3*s1, s1*s3+c1*c3*s2, position.x(),
    391                          c2*s1, c1*c3+s1*s2*s3, c3*s1*s2-c1*s3, position.y(),
    392                            -s2,          c2*s3,          c2*c3, position.z(),
    393                              0,              0,              0,          1);
    394 }
     426    return QMatrix4x4(c1 * c2, c1 * s2 * s3 - c3 * s1, s1 * s3 + c1 * c3 * s2, position.x(),
     427                      c2 * s1, c1 * c3 + s1 * s2 * s3, c3 * s1 * s2 - c1 * s3, position.y(),
     428                      -s2, c2 * s3, c2 * c3, position.z(),
     429                      0, 0, 0, 1);
     430}
Note: See TracChangeset for help on using the changeset viewer.