source: flair-src/trunk/lib/FlairSensorActuator/src/geodesie.cpp @ 10

Last change on this file since 10 was 3, checked in by Sanahuja Guillaume, 6 years ago

sensoractuator

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