source: pacpusframework/trunk/src/PacpusTools/src/geodesie.cpp@ 212

Last change on this file since 212 was 212, checked in by Marek Kurdej, 11 years ago

Geodesie: minor fixes.

  • Property svn:executable set to *
File size: 13.1 KB
Line 
1// %pacpus:license{
2// This file is part of the PACPUS framework distributed under the
3// CECILL-C License, Version 1.0.
4/// @author Marek Kurdej <firstname.surname@utc.fr>
5/// @author Jean Laneurit <firstname.surname@utc.fr>
6/// @date April, 2010
7// %pacpus:license}
8
9#include <Pacpus/PacpusTools/geodesie.h>
10
11#include <fstream>
12#include <QMatrix4x4>
13#include <QVector3D>
14
15using ::boost::math::constants::pi;
16using ::boost::math::constants::half_pi;
17using ::std::abs;
18using ::std::atan;
19using ::std::exp;
20using ::std::ifstream;
21using ::std::ostream;
22using ::std::log;
23using ::std::sin;
24using ::std::string;
25
26#ifdef _MSC_VER
27#pragma warning(disable : 4244)
28#endif //_MSC_VER
29
30namespace Geodesy
31{
32
33////////////////////////////////////////////////////////////////////////////////
34Matrice::Matrice(const Matrice& A)
35{
36 c0_l0 = A.c0_l0;
37 c1_l0 = A.c1_l0;
38 c2_l0 = A.c2_l0;
39 c0_l1 = A.c0_l1;
40 c1_l1 = A.c1_l1;
41 c2_l1 = A.c2_l1;
42 c0_l2 = A.c0_l2;
43 c1_l2 = A.c1_l2;
44 c2_l2 = A.c2_l2;
45}
46
47////////////////////////////////////////////////////////////////////////////////
48Matrice::Matrice()
49{
50 c0_l0 = 0.0;
51 c1_l0 = 0.0;
52 c2_l0 = 0.0;
53 c0_l1 = 0.0;
54 c1_l1 = 0.0;
55 c2_l1 = 0.0;
56 c0_l2 = 0.0;
57 c1_l2 = 0.0;
58 c2_l2 = 0.0;
59}
60
61////////////////////////////////////////////////////////////////////////////////
62void Matrice::Apply(double v0, double v1, double v2, double& Mv0, double& Mv1, double& Mv2)
63{
64 Mv0 = c0_l0 * v0 + c1_l0 * v1 + c2_l0 * v2;
65 Mv1 = c0_l1 * v0 + c1_l1 * v1 + c2_l1 * v2;
66 Mv2 = c0_l2 * v0 + c1_l2 * v1 + c2_l2 * v2;
67}
68
69////////////////////////////////////////////////////////////////////////////////
70Matrice ProdMat(Matrice const& A, Matrice const& B)
71{
72 Matrice out;
73
74 out.c0_l0 = A.c0_l0 * B.c0_l0 + A.c1_l0 * B.c0_l1 + A.c2_l0 * B.c0_l2;
75 out.c1_l0 = A.c0_l0 * B.c1_l0 + A.c1_l0 * B.c1_l1 + A.c2_l0 * B.c1_l2;
76 out.c2_l0 = A.c0_l0 * B.c2_l0 + A.c1_l0 * B.c2_l1 + A.c2_l0 * B.c2_l2;
77
78 out.c0_l1 = A.c0_l1 * B.c0_l0 + A.c1_l1 * B.c0_l1 + A.c2_l1 * B.c0_l2;
79 out.c1_l1 = A.c0_l1 * B.c1_l0 + A.c1_l1 * B.c1_l1 + A.c2_l1 * B.c1_l2;
80 out.c2_l1 = A.c0_l1 * B.c2_l0 + A.c1_l1 * B.c2_l1 + A.c2_l1 * B.c2_l2;
81
82 out.c0_l2 = A.c0_l2 * B.c0_l0 + A.c1_l2 * B.c0_l1 + A.c2_l2 * B.c0_l2;
83 out.c1_l2 = A.c0_l2 * B.c1_l0 + A.c1_l2 * B.c1_l1 + A.c2_l2 * B.c1_l2;
84 out.c2_l2 = A.c0_l2 * B.c2_l0 + A.c1_l2 * B.c2_l1 + A.c2_l2 * B.c2_l2;
85 return out;
86}
87
88////////////////////////////////////////////////////////////////////////////////
89Matrice TransMat(Matrice const& A)
90{
91 Matrice out;
92 out.c0_l0 = A.c0_l0;
93 out.c1_l0 = A.c0_l1;
94 out.c2_l0 = A.c0_l2;
95 out.c0_l1 = A.c1_l0;
96 out.c1_l1 = A.c1_l1;
97 out.c2_l1 = A.c1_l2;
98 out.c0_l2 = A.c2_l0;
99 out.c1_l2 = A.c2_l1;
100 out.c2_l2 = A.c2_l2;
101 return out;
102}
103
104////////////////////////////////////////////////////////////////////////////////
105ostream& operator<<(ostream& os, Matrice const& A)
106{
107 os << A.c0_l0 << "\t" << A.c1_l0 << "\t" << A.c2_l0 << "\n";
108 os << A.c0_l1 << "\t" << A.c1_l1 << "\t" << A.c2_l1 << "\n";
109 os << A.c0_l2 << "\t" << A.c1_l2 << "\t" << A.c2_l2 << "\n";
110 return os;
111}
112
113void Write(Matrice const& A, ostream& out)
114{
115 out << A;
116}
117
118////////////////////////////////////////////////////////////////////////////////
119Raf98::Raf98()
120{
121}
122
123////////////////////////////////////////////////////////////////////////////////
124Raf98::~Raf98()
125{
126 m_dvalues.clear();
127}
128
129////////////////////////////////////////////////////////////////////////////////
130bool Raf98::Interpol(double longitude, double latitude, double* Hwgs84) const
131{
132 *Hwgs84 = 0.0;
133 if (m_dvalues.size() == 0) {
134 return false;
135 }
136 const double longitude_min = -5.5;
137 const double longitude_max = 8.5;
138 if (longitude < longitude_min) {
139 return false;
140 }
141 if (longitude > longitude_max) {
142 return false;
143 }
144 const double latitude_min = 42;
145 const double latitude_max = 51.5;
146 if (latitude < latitude_min) {
147 return false;
148 }
149 if (latitude > latitude_max) {
150 return false;
151 }
152 //conversion en position
153 double longPix = (longitude - longitude_min) * 30.;
154 double latPix = (latitude_max - latitude) * 40.;
155
156 double RestCol, RestLig;
157 double ColIni, LigIni;
158 RestCol = modf(longPix, &ColIni);
159 RestLig = modf(latPix, &LigIni);
160
161 double Zbd = (1.0 - RestCol) * (1.0 - RestLig) * LitGrille(ColIni, LigIni);
162 Zbd += RestCol * (1.0 - RestLig) * LitGrille(ColIni + 1, LigIni);
163 Zbd += (1.0 - RestCol) * RestLig * LitGrille(ColIni, LigIni + 1);
164 Zbd += RestCol * RestLig * LitGrille(ColIni + 1, LigIni + 1);
165 *Hwgs84 = Zbd;
166
167 return true;
168}
169
170////////////////////////////////////////////////////////////////////////////////
171double Raf98::LitGrille(unsigned int c, unsigned int l) const
172{
173 const unsigned int w = 421;
174 // const unsigned int h=381;
175 return m_dvalues.at(c + l * w);
176}
177
178////////////////////////////////////////////////////////////////////////////////
179bool Raf98::Load(const string& s)
180{
181 ifstream in(s.c_str());
182 unsigned int w = 421;
183 unsigned int h = 381;
184
185 m_dvalues.reserve(w * h);
186
187 char entete[1024]; //sur 3 lignes
188 in.getline(entete, 1023);
189 in.getline(entete, 1023);
190 in.getline(entete, 1023);
191
192 char bidon[1024];
193 double val;
194 for (unsigned int i = 0; i < h; ++i) {
195 for (unsigned int j = 0; j < 52; ++j) {
196 for (unsigned int k = 0; k < 8; ++k) {
197 in >> val;
198 m_dvalues.push_back(val);
199 }
200 in.getline(bidon, 1023);
201 }
202 for (unsigned int k = 0; k < 5; ++k) {
203 in >> val;
204 m_dvalues.push_back(val);
205 }
206 in.getline(bidon, 1023);
207 if (!in.good()) {
208 m_dvalues.clear();
209 return false;
210 }
211 }
212 return in.good();
213}
214
215} // namespace Geodesy
216
217////////////////////////////////////////////////////////////////////////////////
218////////////////////////////////////////////////////////////////////////////////
219
220////////////////////////////////////////////////////////////////////////////////
221//ALGO0001
222double Geodesy::LatitueIsometrique(double latitude, double e)
223{
224 double li = log(tan(pi<double>() / 4. + latitude / 2.)) + e * log((1 - e * sin(latitude)) / (1 + e * sin(latitude))) / 2;
225 return li;
226}
227
228////////////////////////////////////////////////////////////////////////////////
229//ALGO0002
230double Geodesy::LatitueIsometrique2Lat(double latitude_iso, double e, double epsilon)
231{
232 double latitude_i = 2 * atan(exp(latitude_iso)) - half_pi<double>();
233 double latitude_ip1 = latitude_i + epsilon * 2;
234 while (abs(latitude_i - latitude_ip1) > epsilon) {
235 latitude_i = latitude_ip1;
236 latitude_ip1 = 2 * atan(
237 exp(e * 0.5 * log(
238 (1 + e * sin(latitude_i)) / (1 - e * sin(latitude_i))))
239 * exp(latitude_iso)) - half_pi<double>();
240 }
241 return latitude_ip1;
242}
243
244////////////////////////////////////////////////////////////////////////////////
245void Geodesy::Geo2ProjLambert(
246 double lambda, double phi,
247 double n, double c, double e,
248 double lambdac, double xs, double ys,
249 double& X, double& Y)
250{
251 double lat_iso = LatitueIsometrique(phi, e);
252 X = xs + c * exp(-n * lat_iso) * sin(n * (lambda - lambdac));
253 Y = ys - c * exp(-n * lat_iso) * cos(n * (lambda - lambdac));
254}
255
256////////////////////////////////////////////////////////////////////////////////
257//ALGO0004
258void Geodesy::Proj2GeoLambert(
259 double X, double Y,
260 double n, double c, double e,
261 double lambdac, double xs, double ys,
262 double epsilon,
263 double& lambda, double& phi)
264{
265 double X_xs = X - xs;
266 double ys_Y = ys - Y;
267 double R = sqrt(X_xs * X_xs + ys_Y * ys_Y);
268 double gamma = atan(X_xs / ys_Y);
269 lambda = lambdac + gamma / n;
270 double lat_iso = -1 / n * log(fabs(R / c));
271 phi = LatitueIsometrique2Lat(lat_iso, e, epsilon);
272}
273
274////////////////////////////////////////////////////////////////////////////////
275double Geodesy::ConvMerApp(double longitude)
276{
277 double phi0_Lambert93 = Deg2Rad(46.5);
278 double lambda0_Lambert93 = Deg2Rad(3.0);
279 double conv = -sin(phi0_Lambert93) * (longitude - lambda0_Lambert93);
280 return conv;
281}
282
283////////////////////////////////////////////////////////////////////
284void Geodesy::Geographique_2_Lambert93(const Raf98& raf98, double lambda, double phi, double he, Matrice in, double& E, double& N, double& h, Matrice& out)
285{
286 Matrice passage;
287 double conv = Geodesy::ConvMerApp(lambda);
288 double c_ = cos(conv);
289 double s_ = sin(conv);
290
291 passage.c0_l0 = c_;
292 passage.c0_l1 = s_;
293 passage.c0_l2 = 0.0;
294
295 passage.c1_l0 = -s_;
296 passage.c1_l1 = c_;
297 passage.c1_l2 = 0.0;
298
299 passage.c2_l0 = 0.0;
300 passage.c2_l1 = 0.0;
301 passage.c2_l2 = 1.0;
302
303 out = ProdMat(passage, in);
304 double diff_h;
305 raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h);
306 h = he - diff_h;
307
308 Geodesy::Geo2ProjLambert(
309 lambda, phi,
310 n_Lambert93, c_Lambert93, e_Lambert93,
311 lambda0_Lambert93, xs_Lambert93, ys_Lambert93,
312 E, N);
313}
314
315////////////////////////////////////////////////////////////////////////
316void Geodesy::Geographique_2_Lambert93(const Raf98& raf98, double lambda, double phi, double he, double& E, double& N, double& h)
317{
318 Geodesy::Geo2ProjLambert(
319 lambda, phi,
320 n_Lambert93, c_Lambert93, e_Lambert93,
321 lambda0_Lambert93, xs_Lambert93, ys_Lambert93,
322 E, N);
323
324 double diff_h;
325 raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h);
326 h = he - diff_h;
327}
328
329/// Converts Lambert93 coordinates (East, North, Height) into geographical coordinates in radians (Longitude = Rad2Deg(lambda), Latitude = Rad2Deg(phi), Height)
330void Geodesy::Lambert93_2_Geographique(const Raf98& raf98, double E, double N, double h, double& lambda, double& phi, double& he)
331{
332 Geodesy::Proj2GeoLambert(
333 E, N,
334 n_Lambert93, c_Lambert93, e_Lambert93,
335 lambda0_Lambert93, xs_Lambert93, ys_Lambert93,
336 0.0000000000000001,
337 lambda, phi);
338
339 double diff_h;
340 raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h);
341 he = h + diff_h;
342}
343
344////////////////////////////////////////////////////////////////////////
345void Geodesy::Lambert93_2_Geographique(const Raf98& raf98, double E, double N, double h, Matrice in, double& lambda, double& phi, double& he, Matrice& out)
346{
347 Geodesy::Proj2GeoLambert(
348 E, N,
349 n_Lambert93, c_Lambert93, e_Lambert93,
350 lambda0_Lambert93, xs_Lambert93, ys_Lambert93,
351 0.0000000000000001,
352 lambda, phi);
353
354 Matrice passage;
355 double conv = Geodesy::ConvMerApp(lambda);
356 double c_ = cos(conv);
357 double s_ = sin(conv);
358
359 passage.c0_l0 = c_;
360 passage.c0_l1 = -s_;
361 passage.c0_l2 = 0.0;
362
363 passage.c1_l0 = s_;
364 passage.c1_l1 = c_;
365 passage.c1_l2 = 0.0;
366
367 passage.c2_l0 = 0.0;
368 passage.c2_l1 = 0.0;
369 passage.c2_l2 = 1.0;
370
371 out = ProdMat(passage, in);
372
373 double diff_h;
374 raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h);
375 he = h + diff_h;
376}
377
378////////////////////////////////////////////////////////////////////////
379void Geodesy::Geographique_2_ECEF(double longitude, double latitude, double he, double& x, double& y, double& z)
380{
381 const double n = GRS_a / sqrt(1.0 - pow(GRS_e, 2) * pow(sin(latitude), 2));
382 x = (n + he) * cos(latitude) * cos(longitude);
383 y = (n + he) * cos(latitude) * sin(longitude);
384 z = (n * (1.0 - pow(GRS_e, 2)) + he) * sin(latitude);
385}
386
387////////////////////////////////////////////////////////////////////////
388void Geodesy::ECEF_2_ENU(double x, double y, double z, double& e, double& n, double& u, double lon0, double lat0, double he0)
389{
390 double slat = sin(lat0);
391 double clat = cos(lat0);
392 double slon = sin(lon0);
393 double clon = cos(lon0);
394
395 Matrice C;
396 C.c0_l0 = -slon;
397 C.c1_l0 = clon;
398
399 C.c0_l1 = -clon * slat;
400 C.c1_l1 = -slon * slat;
401 C.c2_l1 = clat;
402
403 C.c0_l2 = clon * clat;
404 C.c1_l2 = slon * clat;
405 C.c2_l2 = slat;
406
407 double x0, y0, z0;
408 Geographique_2_ECEF(lon0, lat0, he0, x0, y0, z0);
409
410 x -= x0;
411 y -= y0;
412 z -= z0;
413
414 C.Apply(x, y, z, e, n, u);
415}
416
417QMatrix4x4 Geodesy::yprenuToMatrix(QVector3D angle, QVector3D position)
418{
419 float c1 = cos(angle.x());
420 float c2 = cos(angle.y());
421 float c3 = cos(angle.z());
422
423 float s1 = sin(angle.x());
424 float s2 = sin(angle.y());
425 float s3 = sin(angle.z());
426
427 // Source : https://en.wikipedia.org/wiki/Euler_angles
428 return QMatrix4x4(c1 * c2, c1 * s2 * s3 - c3 * s1, s1 * s3 + c1 * c3 * s2, position.x(),
429 c2 * s1, c1 * c3 + s1 * s2 * s3, c3 * s1 * s2 - c1 * s3, position.y(),
430 -s2, c2 * s3, c2 * c3, position.z(),
431 0, 0, 0, 1);
432}
Note: See TracBrowser for help on using the repository browser.