1 | /**
|
---|
2 | *
|
---|
3 | * Distributed under the UTC Heudiascy Pacpus License, Version 1.0.
|
---|
4 | * Copyright (c) UTC Heudiasyc 2010 - 2013. All rights reserved.
|
---|
5 | *
|
---|
6 | * See the LICENSE file for more information or a copy at:
|
---|
7 | * http://www.hds.utc.fr/~kurdejma/LICENSE_1_0.txt
|
---|
8 | *
|
---|
9 | */
|
---|
10 |
|
---|
11 | #include <fstream>
|
---|
12 | #include <Pacpus/PacpusTools/geodesie.h>
|
---|
13 |
|
---|
14 | #ifdef _MSC_VER
|
---|
15 | # pragma warning(disable:4244)
|
---|
16 | #endif //_MSC_VER
|
---|
17 |
|
---|
18 | namespace Geodesie {
|
---|
19 | /// ////////////////////////////////////////////////////////////////////
|
---|
20 | Matrice::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 | /// ////////////////////////////////////////////////////////////////////
|
---|
26 | Matrice::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 | /// ////////////////////////////////////////////////////////////////////
|
---|
32 | void 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 | /// ////////////////////////////////////////////////////////////////////
|
---|
38 | Matrice 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 | /// ////////////////////////////////////////////////////////////////////
|
---|
56 | Matrice 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 | /// ////////////////////////////////////////////////////////////////////
|
---|
65 | void 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 | /// ////////////////////////////////////////////////////////////////////
|
---|
72 | Raf98::~Raf98() {
|
---|
73 | m_dvalues.clear();
|
---|
74 | }
|
---|
75 |
|
---|
76 | //-----------------------------------------------------------------------------
|
---|
77 | bool 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 | /// ////////////////////////////////////////////////////////////////////
|
---|
115 | double 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 | /// ////////////////////////////////////////////////////////////////////
|
---|
121 | bool 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
|
---|
163 | double 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
|
---|
171 | double 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 | /// ////////////////////////////////////////////////////////////////////
|
---|
188 | void 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
|
---|
200 | void 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 | /// ////////////////////////////////////////////////////////////////////
|
---|
216 | double 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 | ////////////////////////////////////////////////////////////////////
|
---|
224 | void 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 | ////////////////////////////////////////////////////////////////////////
|
---|
254 | void 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 | */
|
---|
268 | void 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 | ////////////////////////////////////////////////////////////////////////
|
---|
281 | void 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 | ////////////////////////////////////////////////////////////////////////
|
---|
314 | void 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 | ////////////////////////////////////////////////////////////////////////
|
---|
322 | void 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 | }
|
---|