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

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

simu gps

File size: 6.2 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,
29             double &Mv2);
30  double c0_l0;
31  double c1_l0;
32  double c2_l0;
33  double c0_l1;
34  double c1_l1;
35  double c2_l1;
36  double c0_l2;
37  double c1_l2;
38  double c2_l2;
39}; // class
40
41Matrice TransMat(const Matrice A);
42
43Matrice ProdMat(const Matrice A, const Matrice B);
44void Write(const Matrice A, std::ostream &out);
45
46////////////////////////////////////////////////////////////////////////
47class Raf98 {
48private:
49  std::vector<double> m_dvalues;
50  double LitGrille(unsigned int c, unsigned int l) const;
51
52public:
53  ~Raf98();
54  Raf98() {}
55  bool Load(const std::string &s);
56  bool Interpol(double longitude /*deg*/, double latitude /*deg*/,
57                double *Hwgs84) const;
58}; // class
59////////////////////////////////////////////////////////////////////////
60
61////////////////////////////////////////////////////////////////////////
62inline double Deg2Rad(double deg) { return deg * M_PI / 180.0; }
63inline double Rad2Deg(double rad) { return rad * 180.0 / M_PI; }
64////////////////////////////////////////////////////////////////////////
65
66const double a_Lambert93 = 6378137;
67const double f_Lambert93 = 1 / 298.257222101;
68const double e_Lambert93 = sqrt(f_Lambert93 * (2 - f_Lambert93));
69const double lambda0_Lambert93 = Deg2Rad(3.0); // degres
70const double phi0_Lambert93 = Deg2Rad(46.5);
71const double phi1_Lambert93 = Deg2Rad(44.0);
72const double phi2_Lambert93 = Deg2Rad(49.0); // degres
73const double X0_Lambert93 = 700000;          //
74const double Y0_Lambert93 = 6600000;         //
75const double n_Lambert93 = 0.7256077650;
76const double c_Lambert93 = 11754255.426;
77const double xs_Lambert93 = 700000;
78const double ys_Lambert93 = 12655612.050;
79
80const double GRS_a = 6378137;
81const double GRS_f = 1 / 298.257222101;
82const double GRS_b = GRS_a * (1 - GRS_f);
83const double GRS_e = sqrt((pow(GRS_a, 2) - pow(GRS_b, 2)) / pow(GRS_a, 2));
84
85////////////////////////////////////////////////////////////////////////
86void Geographique_2_Lambert93(const Raf98 &raf98, double lambda, double phi,
87                              double he, Matrice in, double &E, double &N,
88                              double &h, Matrice &out);
89void Geographique_2_Lambert93(const Raf98 &raf98, double lambda, double phi,
90                              double he, double &E, double &N, double &h);
91void Lambert93_2_Geographique(const Raf98 &raf98, double E, double N, double h,
92                              double &lambda, double &phi, double &he);
93void Lambert93_2_Geographique(const Raf98 &raf98, double E, double N, double h,
94                              Matrice in, double &lambda, double &phi,
95                              double &he, Matrice &out);
96/** Convert from geographique to ECEF.
97 * @param[in] longitude Longitude in radian.
98 * @param[in] latitude Latitude in radian.
99 * @param[in] he Height in meter.
100 */
101void Geographique_2_ECEF(double longitude, double latitude, double he,
102                         double &x, double &y, double &z);
103
104/** Convert from ECEF to geographique.
105 */
106void ECEF_2_Geographique(double x, double y, double z,
107                          double &longitude, double &latitude, double &he);
108
109/** Convert from ECEF to ENU.
110 * @param[in] lon0 Longitude of the origin in radian.
111 * @param[in] lat0 Latitude of the origin in radian.
112 * @param[in] he0 Height of the origin in radian.
113 */
114void ECEF_2_ENU(double x, double y, double z, double &e, double &n, double &u,
115                double lon0, double lat0, double he0);
116
117/** Convert from ECEF to ENU.
118 * @param[in] lon0 Longitude of the origin in radian.
119 * @param[in] lat0 Latitude of the origin in radian.
120 * @param[in] he0 Height of the origin in radian.
121 */
122void ENU_2_ECEF(double e, double n, double u,double &x, double &y, double &z,
123                double lon0, double lat0, double he0);
124
125////////////////////////////////////////////////////////////////////////
126
127// ALGO0001
128double LatitueIsometrique(double latitude, double e);
129// ALGO0002
130double LatitueIsometrique2Lat(double latitude_iso, double e, double epsilon);
131
132// ALGO0003
133void Geo2ProjLambert(double lambda, double phi, double n, double c, double e,
134                     double lambdac, double xs, double ys, double &X,
135                     double &Y);
136// ALGO0004
137void Proj2GeoLambert(double X, double Y, double n, double c, double e,
138                     double lambdac, double xs, double ys, double epsilon,
139                     double &lambda, double &phi);
140
141double ConvMerApp(double longitude);
142
143/**
144Converts Cartesian (x, y) coordinates to polar coordinates (r, theta)
145*/
146template <typename _T1, typename _T2>
147void cartesianToPolar(const _T1 x, const _T1 y, _T2 &r, _T2 &theta) {
148  r = std::sqrt(x * x + y * y);
149  theta = std::atan2(x, y);
150}
151
152/**
153Converts polar coordinates (r, theta) to Cartesian (x, y) coordinates
154*/
155template <typename _T1, typename _T2>
156void polarToCartesian(const _T1 r, const _T1 theta, _T2 &x, _T2 &y) {
157  x = r * std::cos(theta);
158  y = r * std::sin(theta);
159}
160
161/**
162Converts Cartesian (x, y, z) coordinates to spherical coordinates (r, theta,
163phi)
164Angles expressed in radians.
165*/
166template <typename _T1, typename _T2>
167void cartesianToSpherical(const _T1 x, const _T1 y, const _T1 z, _T2 &r,
168                          _T2 &theta, _T2 &phi) {
169  r = std::sqrt(x * x + y * y + z * z);
170  theta = std::acos(z / r);
171  phi = std::atan2(y, x);
172}
173
174/**
175Converts spherical coordinates (r, theta, phi) to Cartesian (x, y, z)
176coordinates.
177Angles expressed in radians.
178*/
179template <typename _T1, typename _T2>
180void sphericalToCartesian(const _T1 r, const _T1 theta, const _T1 phi, _T2 &x,
181                          _T2 &y, _T2 &z) {
182  x = r * std::sin(theta) * std::cos(phi);
183  y = r * std::sin(theta) * std::sin(phi);
184  z = r * std::cos(theta);
185}
186
187} // namespace Geodesie
188
189#endif // GEODESIE_H
Note: See TracBrowser for help on using the repository browser.