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

Last change on this file since 124 was 55, checked in by Sanahuja Guillaume, 8 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.