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

Last change on this file since 91 was 91, checked in by DHERBOMEZ Gérald, 12 years ago

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