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 |
|
---|
13 | namespace Geodesie {
|
---|
14 | /// ////////////////////////////////////////////////////////////////////
|
---|
15 | Matrice::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 | /// ////////////////////////////////////////////////////////////////////
|
---|
27 | Matrice::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 | /// ////////////////////////////////////////////////////////////////////
|
---|
39 | void 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 | /// ////////////////////////////////////////////////////////////////////
|
---|
46 | Matrice 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 | /// ////////////////////////////////////////////////////////////////////
|
---|
64 | Matrice 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 | /// ////////////////////////////////////////////////////////////////////
|
---|
79 | void 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 | /// ////////////////////////////////////////////////////////////////////
|
---|
86 | Raf98::~Raf98() { m_dvalues.clear(); }
|
---|
87 |
|
---|
88 | //-----------------------------------------------------------------------------
|
---|
89 | bool 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 | /// ////////////////////////////////////////////////////////////////////
|
---|
125 | double 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 | /// ////////////////////////////////////////////////////////////////////
|
---|
131 | bool 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
|
---|
173 | double 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
|
---|
182 | double 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 | /// ////////////////////////////////////////////////////////////////////
|
---|
196 | void 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
|
---|
205 | void 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 | /// ////////////////////////////////////////////////////////////////////
|
---|
217 | double 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 | ////////////////////////////////////////////////////////////////////
|
---|
225 | void 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 | ////////////////////////////////////////////////////////////////////////
|
---|
256 | void 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 | */
|
---|
272 | void 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 | ////////////////////////////////////////////////////////////////////////
|
---|
284 | void 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 | ////////////////////////////////////////////////////////////////////////
|
---|
316 | void 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 | ////////////////////////////////////////////////////////////////////////
|
---|
325 | void Geodesie::ECEF_2_ENU(double x, double y, double z, double &e, double &n,
|
---|
326 | double &u, double lon0, double lat0, double he0) {
|
---|
327 | double slat = std::sin(lat0);
|
---|
328 | double clat = std::cos(lat0);
|
---|
329 | double slon = std::sin(lon0);
|
---|
330 | double clon = std::cos(lon0);
|
---|
331 |
|
---|
332 | Geodesie::Matrice C;
|
---|
333 | C.c0_l0 = -slon;
|
---|
334 | C.c1_l0 = clon;
|
---|
335 |
|
---|
336 | C.c0_l1 = -clon * slat;
|
---|
337 | C.c1_l1 = -slon * slat;
|
---|
338 | C.c2_l1 = clat;
|
---|
339 |
|
---|
340 | C.c0_l2 = clon * clat;
|
---|
341 | C.c1_l2 = slon * clat;
|
---|
342 | C.c2_l2 = slat;
|
---|
343 |
|
---|
344 | double x0, y0, z0;
|
---|
345 | Geographique_2_ECEF(lon0, lat0, he0, x0, y0, z0);
|
---|
346 |
|
---|
347 | x -= x0;
|
---|
348 | y -= y0;
|
---|
349 | z -= z0;
|
---|
350 |
|
---|
351 | C.Apply(x, y, z, e, n, u);
|
---|
352 | }
|
---|