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

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