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