source: flair-src/trunk/lib/FlairSensorActuator/src/geodesie.cpp @ 140

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

simu gps

File size: 13.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#include <fstream>
6
7#include "geodesie.h"
8
9#ifdef _MSC_VER
10#pragma warning(disable : 4244)
11#endif //_MSC_VER
12
13namespace Geodesie {
14/// ////////////////////////////////////////////////////////////////////
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;
25}
26/// ////////////////////////////////////////////////////////////////////
27Matrice::Matrice() {
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;
44}
45/// ////////////////////////////////////////////////////////////////////
46Matrice ProdMat(const Matrice A, const Matrice B) {
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;
61}
62
63/// ////////////////////////////////////////////////////////////////////
64Matrice TransMat(const Matrice A) {
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(); }
87
88//-----------------------------------------------------------------------------
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);
152    }
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();
164}
165
166} // namespace Geodesie
167
168/// ////////////////////////////////////////////////////////////////////
169/// ////////////////////////////////////////////////////////////////////
170
171/// ////////////////////////////////////////////////////////////////////
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);
215}
216/// ////////////////////////////////////////////////////////////////////
217double Geodesie::ConvMerApp(double longitude) {
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;
222}
223
224////////////////////////////////////////////////////////////////////
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);
254}
255////////////////////////////////////////////////////////////////////////
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;
266}
267/**
268    Converts Lambert93 coordinates (East, North, Height) into geographical
269   coordinates in radians (Longitude = Rad2Deg(lambda), Latitude = Rad2Deg(phi),
270   Height)
271*/
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;
282}
283////////////////////////////////////////////////////////////////////////
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;
313}
314
315////////////////////////////////////////////////////////////////////////
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);
322}
323
324//from https://gist.github.com/klucar/1536194
325void Geodesie::ECEF_2_Geographique(double x, double y, double z,
326                                   double &longitude, double &latitude, double &he) {
327
328  double asq=pow(GRS_a, 2);
329  double esq=pow(GRS_e, 2);
330  double b = sqrt( asq * (1-esq) );
331  double bsq = pow(b,2);
332  double ep = sqrt( (asq - bsq)/bsq);
333  double p = sqrt( pow(x,2) + pow(y,2) );
334  double th = atan2(GRS_a*z, b*p);
335
336  longitude = atan2(y,x);
337  latitude = atan2( (z + pow(ep,2)*b*pow(sin(th),3) ), (p - esq*GRS_a*pow(cos(th),3)) );
338  double N = GRS_a/( sqrt(1-esq*pow(sin(latitude),2)) );
339  double alt = p / cos(latitude) - N;
340
341  // mod longitude to 0-2pi
342  if(longitude<0) longitude +=2*M_PI;
343
344  // correction for altitude near poles left out.
345}
346
347////////////////////////////////////////////////////////////////////////
348void Geodesie::ECEF_2_ENU(double x, double y, double z, double &e, double &n,
349                          double &u, double lon0, double lat0, double he0) {
350  double slat = std::sin(lat0);
351  double clat = std::cos(lat0);
352  double slon = std::sin(lon0);
353  double clon = std::cos(lon0);
354
355  Geodesie::Matrice C;
356  C.c0_l0 = -slon;
357  C.c1_l0 = clon;
358
359  C.c0_l1 = -clon * slat;
360  C.c1_l1 = -slon * slat;
361  C.c2_l1 = clat;
362
363  C.c0_l2 = clon * clat;
364  C.c1_l2 = slon * clat;
365  C.c2_l2 = slat;
366
367  double x0, y0, z0;
368  Geographique_2_ECEF(lon0, lat0, he0, x0, y0, z0);
369
370  x -= x0;
371  y -= y0;
372  z -= z0;
373
374  C.Apply(x, y, z, e, n, u);
375}
376
377void Geodesie::ENU_2_ECEF(double e, double n,double u,
378                          double &x, double &y, double &z,
379                          double lon0, double lat0, double he0) {
380  double slat = std::sin(lat0);//phi
381  double clat = std::cos(lat0);
382  double slon = std::sin(lon0);//lambda
383  double clon = std::cos(lon0);
384
385  Geodesie::Matrice C;
386  C.c0_l0 = -slon;
387  C.c1_l0 = -clon*slat;
388  C.c2_l0 = clon*clat;
389
390  C.c0_l1 = clon;
391  C.c1_l1 = -slon * slat;
392  C.c2_l1 = slon*clat;
393
394  C.c0_l2 = 0;
395  C.c1_l2 = clat;
396  C.c2_l2 = slat;
397
398  C.Apply(e, n, u,x,y,z);
399
400  double x0, y0, z0;
401  Geographique_2_ECEF(lon0, lat0, he0, x0, y0, z0);
402
403  x += x0;
404  y += y0;
405  z += z0;
406}
Note: See TracBrowser for help on using the repository browser.