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

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

Minor: Geodesie.
  • Property svn:executable set to *
File size: 12.6 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
13using boost::math::constants::pi;
14using boost::math::constants::half_pi;
15
16#ifdef _MSC_VER
17# pragma warning(disable:4244)
18#endif //_MSC_VER
19
20namespace Geodesie
21{
22
23/// ////////////////////////////////////////////////////////////////////
24Matrice::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/// ////////////////////////////////////////////////////////////////////
32Matrice::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/// ////////////////////////////////////////////////////////////////////
40void 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/// ////////////////////////////////////////////////////////////////////
48Matrice 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/// ////////////////////////////////////////////////////////////////////
67Matrice 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/// ////////////////////////////////////////////////////////////////////
77void 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/// ////////////////////////////////////////////////////////////////////
84Raf98::~Raf98() {
85 m_dvalues.clear();
86}
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
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/// ////////////////////////////////////////////////////////////////////
128double 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/// ////////////////////////////////////////////////////////////////////
136bool 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 Geodesie
173
174/// ////////////////////////////////////////////////////////////////////
175/// ////////////////////////////////////////////////////////////////////
176
177/// ////////////////////////////////////////////////////////////////////
178//ALGO0001
179double Geodesie::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
191double Geodesie::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/// ////////////////////////////////////////////////////////////////////
215void Geodesie::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
227void Geodesie::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/// ////////////////////////////////////////////////////////////////////
243double Geodesie::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////////////////////////////////////////////////////////////////////
251void Geodesie::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=Geodesie::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 Geodesie::Geo2ProjLambert(
276 lambda,phi,
277 n_Lambert93,c_Lambert93,e_Lambert93,
278 lambda0_Lambert93,xs_Lambert93,ys_Lambert93,
279 E,N);
280}
281////////////////////////////////////////////////////////////////////////
282void Geodesie::Geographique_2_Lambert93(const Raf98& raf98,double lambda,double phi,double he,double& E,double& N,double& h) {
283 Geodesie::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)
295void Geodesie::Lambert93_2_Geographique(const Raf98& raf98,double E,double N,double h,double& lambda,double& phi,double& he) {
296 Geodesie::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////////////////////////////////////////////////////////////////////////
309void Geodesie::Lambert93_2_Geographique(const Raf98& raf98,double E,double N,double h,Matrice in,double& lambda,double& phi,double& he,Matrice& out) {
310 Geodesie::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=Geodesie::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////////////////////////////////////////////////////////////////////////
342void Geodesie::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////////////////////////////////////////////////////////////////////////
350void Geodesie::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 Geodesie::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
378QMatrix4x4 Geodesie::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}
Note: See TracBrowser for help on using the repository browser.