source: flair-src/trunk/lib/FlairSensorActuator/src/unexported/geodesie.h @ 5

Last change on this file since 5 was 3, checked in by Sanahuja Guillaume, 6 years ago

sensoractuator

File size: 5.3 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5#ifndef GEODESIE_H
6#define GEODESIE_H
7
8#include <cmath>
9#include <iostream>
10#include <vector>
11
12namespace Geodesie {
13
14#ifndef M_PI
15#   define M_PI 3.14159265358979323846
16#endif
17#ifndef M_PI_2
18#   define M_PI_2 1.57079632679489661923
19#endif
20#ifndef M_PI_4
21#   define M_PI_4 0.78539816339744830962
22#endif
23
24////////////////////////////////////////////////////////////////////////
25struct Matrice {
26    Matrice(const Matrice & A);
27    Matrice();
28    void Apply(double v0, double v1, double v2, double & Mv0, double & Mv1, double & Mv2);
29    double c0_l0;double c1_l0;double c2_l0;
30    double c0_l1;double c1_l1;double c2_l1;
31    double c0_l2;double c1_l2;double c2_l2;
32}; // class
33
34Matrice TransMat(const Matrice A);
35
36Matrice ProdMat(const Matrice A,const Matrice B);
37void Write(const Matrice A,std::ostream& out);
38
39////////////////////////////////////////////////////////////////////////
40class Raf98 {
41private :
42    std::vector<double> m_dvalues;
43    double LitGrille(unsigned int c,unsigned int l) const;
44public :
45    ~Raf98();
46    Raf98() {}
47    bool Load(const std::string & s);
48    bool Interpol(double longitude/*deg*/, double latitude/*deg*/, double* Hwgs84) const;
49}; // class
50////////////////////////////////////////////////////////////////////////
51
52////////////////////////////////////////////////////////////////////////
53inline double Deg2Rad(double deg) {return deg*M_PI/180.0;}
54inline double Rad2Deg(double rad) {return rad*180.0/M_PI;}
55////////////////////////////////////////////////////////////////////////
56
57const double a_Lambert93=6378137;
58const double f_Lambert93=1 / 298.257222101;
59const double e_Lambert93=sqrt(f_Lambert93*(2-f_Lambert93));
60const double lambda0_Lambert93=Deg2Rad(3.0);//degres
61const double phi0_Lambert93=Deg2Rad(46.5);
62const double phi1_Lambert93=Deg2Rad(44.0);
63const double phi2_Lambert93=Deg2Rad(49.0);//degres
64const double X0_Lambert93=700000;//
65const double Y0_Lambert93=6600000;//
66const double n_Lambert93 = 0.7256077650;
67const double c_Lambert93 = 11754255.426;
68const double xs_Lambert93 = 700000;
69const double ys_Lambert93 = 12655612.050;
70
71const double GRS_a = 6378137;
72const double GRS_f = 1/298.257222101;
73const double GRS_b = GRS_a*(1-GRS_f);
74const double GRS_e = sqrt((pow(GRS_a,2) - pow(GRS_b,2)) / pow(GRS_a,2));
75
76////////////////////////////////////////////////////////////////////////
77void Geographique_2_Lambert93(const Raf98& raf98,double lambda,double phi,double he,Matrice in,double& E,double& N,double& h,Matrice& out);
78void Geographique_2_Lambert93(const Raf98& raf98,double lambda,double phi,double he,double& E,double& N,double& h);
79void Lambert93_2_Geographique(const Raf98& raf98,double E,double N,double h,double& lambda,double& phi,double& he);
80void Lambert93_2_Geographique(const Raf98& raf98,double E,double N,double h,Matrice in,double& lambda,double& phi,double& he,Matrice& out);
81/** Convert from geographique to ECEF.
82 * @param[in] longitude Longitude in radian.
83 * @param[in] latitude Latitude in radian.
84 * @param[in] he Height in meter.
85 */
86void Geographique_2_ECEF(double longitude, double latitude, double he, double& x, double& y, double& z);
87/** Convert from ECEF two ENU.
88 * @param[in] lon0 Longitude of the origin in radian.
89 * @param[in] lat0 Latitude of the origin in radian.
90 * @param[in] he0 Height of the origin in radian.
91 */
92void ECEF_2_ENU(double x,double y,double z,double& e,double& n,double& u,double lon0,double lat0,double he0);
93////////////////////////////////////////////////////////////////////////
94
95//ALGO0001
96double LatitueIsometrique(double latitude,double e);
97//ALGO0002
98double LatitueIsometrique2Lat(double latitude_iso,double e,double epsilon);
99
100//ALGO0003
101void Geo2ProjLambert(
102    double lambda,double phi,
103    double n, double c,double e,
104    double lambdac,double xs,double ys,
105    double& X,double& Y);
106//ALGO0004
107void Proj2GeoLambert(
108    double X,double Y,
109    double n, double c,double e,
110    double lambdac,double xs,double ys,
111    double epsilon,
112    double& lambda,double& phi);
113
114double ConvMerApp(double longitude);
115
116/**
117Converts Cartesian (x, y) coordinates to polar coordinates (r, theta)
118*/
119template <typename _T1, typename _T2>
120void cartesianToPolar(const _T1 x, const _T1 y, _T2 & r, _T2 & theta) {
121    r = std::sqrt(x*x + y*y);
122    theta = std::atan2(x, y);
123}
124
125/**
126Converts polar coordinates (r, theta) to Cartesian (x, y) coordinates
127*/
128template <typename _T1, typename _T2>
129void polarToCartesian(const _T1 r, const _T1 theta, _T2 & x, _T2 & y) {
130    x = r * std::cos(theta);
131    y = r * std::sin(theta);
132}
133
134/**
135Converts Cartesian (x, y, z) coordinates to spherical coordinates (r, theta, phi)
136Angles expressed in radians.
137*/
138template <typename _T1, typename _T2>
139void cartesianToSpherical(const _T1 x, const _T1 y, const _T1 z, _T2 & r, _T2 & theta, _T2 & phi) {
140    r = std::sqrt(x*x + y*y + z*z);
141    theta = std::acos(z / r);
142    phi = std::atan2(y, x);
143}
144
145/**
146Converts spherical coordinates (r, theta, phi) to Cartesian (x, y, z) coordinates.
147Angles expressed in radians.
148*/
149template <typename _T1, typename _T2>
150void sphericalToCartesian(const _T1 r, const _T1 theta, const _T1 phi, _T2 & x, _T2 & y, _T2 & z) {
151    x = r * std::sin(theta) * std::cos(phi);
152    y = r * std::sin(theta) * std::sin(phi);
153    z = r * std::cos(theta);
154}
155
156} // namespace Geodesie
157
158#endif // GEODESIE_H
Note: See TracBrowser for help on using the repository browser.