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

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

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