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

Last change on this file since 64 was 64, checked in by Marek Kurdej, 12 years ago

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