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

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

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