source: pacpusframework/trunk/include/Pacpus/PacpusTools/math/geodesy.hpp@ 64

Last change on this file since 64 was 64, checked in by Marek Kurdej, 11 years ago

Modified property: added svn:keywords=Id.
  • Property svn:keywords set to Id
File size: 14.8 KB
Line 
1#ifndef __GEO_UTILITIES__
2#define __GEO_UTILITIES__
3
4#include <cmath>
5#include <iostream>
6
7#include <boost/numeric/ublas/vector.hpp>
8#include <boost/numeric/ublas/matrix.hpp>
9
10namespace math{
11
12 namespace geodesy {
13
14#ifndef M_PI
15 const double M_PI = std::atan(1.0)*4.0;
16#endif
17
18 /*!
19 * \enum CoordinateSystem
20 * \brief coordinate system enumeration
21 */
22 enum CoordinateSystem {_LLH_,_ECEF_,_LAMBERT93_,_ENU_,_NED_,_NONE_};
23
24 /*!
25 * \def WGS84_a
26 * \brief semi-major axis of WGS84 ellipsoid
27 */
28 const double WGS84_a = 6378137.0;
29 /*!
30 * \def WGS84_e
31 * \brief eccentricity of WGS84 ellipsoid
32 */
33 const double WGS84_e = 0.0818191913108695;
34
35 /*!
36 * \def GRS80_a
37 * \brief semi-major axis of GRS80 ellipsoid
38 */
39 const double GRS80_a = 6378137.0;
40 /*!
41 * \def GRS80_a
42 * \brief eccentricity of GRS80 ellipsoid
43 */
44 const double GRS80_e =0.0818191910428152;
45
46
47 /**Lambert93 conversion parameters */
48 const double LAMBERT93_n=0.725607765053267;
49 const double LAMBERT93_c = 11754255.4261;
50 const double LAMBERT93_Xs = 700000.0;
51 const double LAMBERT93_Ys = 12655612.0499;
52 const double LAMBERT93_lon0 = 0.0523598775598299;
53
54 /**Conversion functions*/
55
56 /*!
57 * \fn inline boost::numeric::ublas::vector<T> LLH2ECEF(const boost::numeric::ublas::vector<T> &llh, const T &a, const T &e)
58 * \brief convert an LLH position to a ECEF position
59 * \param llh : ublas vector describing a llh position (latitude, longitude,ellipsoidal height)
60 * \param a : semi-major axis of ellipsoid (by default WGS84 value is used)
61 * \param e : ellipsoid eccentricity (by default WGS84 value is used)
62 * \return ublas vector describing a ecef position (x,y,z)
63 */
64 template<class T> inline boost::numeric::ublas::vector<T> LLH2ECEF(const boost::numeric::ublas::vector<T> &llh, const T &a=WGS84_a, const T &e=WGS84_e){
65
66 T N=TranversalRadius(llh[0],a,e);
67 boost::numeric::ublas::vector<T> ecef(3);
68 ecef[0]=(N+llh[2])*std::cos(llh[0])*std::cos(llh[1]);
69 ecef[1]=(N+llh[2])*std::cos(llh[0])*std::sin(llh[1]);
70 ecef[2]=(N*(1-std::pow(e,2))+llh[2])*std::sin(llh[0]);
71
72 return ecef;
73 }
74
75 /*!
76 * \fn inline boost::numeric::ublas::vector<T> ECEF2LLH(const boost::numeric::ublas::vector<T> &ecef,const T &a, const T &e)
77 * \brief convert an ECEF position to a LLH position
78 * \param ecef : ublas vector describing a ecef position (x,y,z)
79 * \param a : semi-major axis of ellipsoid (by default WGS84 value is used)
80 * \param e : ellipsoid eccentricity (by default WGS84 value is used)
81 * \return ublas vector describing a llh position (latitude, longitude,ellipsoidal height)
82 */
83 template<class T> inline boost::numeric::ublas::vector<T> ECEF2LLH(const boost::numeric::ublas::vector<T> &ecef,const T &a=WGS84_a, const T &e=WGS84_e){
84 boost::numeric::ublas::vector<T> lla(3);
85 T P= std::sqrt(pow(ecef[0],2) + pow(ecef[1],2));
86 T l0 =std::atan(ecef[2]/(P*(1-a*std::pow(e,2)/std::sqrt(std::pow(ecef[0],2) + std::pow(ecef[1],2) + std::pow(ecef[2],2)))));
87 lla[1]= std::atan(ecef[1]/ecef[0]);
88 lla[0]= std::atan((ecef[2]/P)/(1-a*std::pow(e,2)*std::cos(l0)/(P*std::sqrt(1-std::pow(e,2)*std::pow(sin(l0),2)))));
89 while(std::fabs(lla[0]*180/M_PI-l0*180/M_PI)>std::pow(10.0,-12.0)){
90 l0=lla[0];
91 lla[0]=std::atan((ecef[2]/P)/(1-a*std::pow(e,2)*std::cos(l0)/(P*sqrt(1-std::pow(e*std::sin(l0),2)))));
92 }
93 lla[2]=(P/std::cos(lla[0]))-(a/std::sqrt(1-pow(e*sin(lla[0]),2)));
94 return lla;
95 }
96
97 /*!
98 * \fn inline boost::numeric::ublas::vector<T> ECEF2NTF(const boost::numeric::ublas::vector<T> &ecef)
99 * \brief convert an ECEF position to a NTF position
100 * \param ecef : ublas vector describing a ecef position (x,y,z)
101 * \return ublas vector describing a ntf position (x,y,z)
102 */
103 template <class T> inline boost::numeric::ublas::vector<T> ECEF2NTF(const boost::numeric::ublas::vector<T> &ecef){
104 boost::numeric::ublas::vector<T> ntf(3);
105 ntf[0]=ecef[0]+168;
106 ntf[1]=ecef[1]+60;
107 ntf[2]=ecef[2]-320;
108 return ntf;
109 }
110
111 /*!
112 * \fn inline boost::numeric::ublas::vector<T> NTF2ECEF(const boost::numeric::ublas::vector<T> &ntf)
113 * \brief convert an ECEF position to a NTF position
114 * \param ntf : ublas vector describing a ntf position (x,y,z)
115 * \return ublas vector describing a ecef position (x,y,z)
116 */
117 template <class T> inline boost::numeric::ublas::vector<T> NTF2ECEF(const boost::numeric::ublas::vector<T> &ntf){
118 boost::numeric::ublas::vector<T> ecef(3);
119 ecef[0]=ntf[0]-168;
120 ecef[1]=ntf[1]-60;
121 ecef[2]=ntf[2]+320;
122 return ecef;
123 }
124
125
126 /*!
127 * \fn inline boost::numeric::ublas::matrix<T> C_ECEF2ENU(const T &lat0,const T &lon0)
128 * \brief compute transformation matrix using to convert an ECEF position to a ENU position
129 * \param lat0 : reference latitude
130 * \param lon0 : reference longitude
131 * \return ublas matrix
132 */
133 template <class T> inline boost::numeric::ublas::matrix<T> C_ECEF2ENU(const T &lat0,const T &lon0){
134
135 T slat = std::sin(lat0);
136 T clat = std::cos(lat0);
137 T slon = std::sin(lon0);
138 T clon = std::cos(lon0);
139
140 boost::numeric::ublas::matrix<T> C(3,3);
141 C(0,0) = -slon;
142 C(0,1) = clon;
143 C(0,2) = 0.0;
144
145 C(1,0) = -clon * slat;
146 C(1,1) = -slon * slat;
147 C(1,2) = clat;
148
149 C(2,0) = clon * clat;
150 C(2,1) = slon * clat;
151 C(2,2) = slat;
152
153 return C;
154 }
155
156 /*!
157 * \fn inline boost::numeric::ublas::matrix<T> C_ENU2ECEF(const T &lat0, const T &lon0)
158 * \brief compute transformation matrix using to convert an ENU position to a ECEF position
159 * \param lat0 : reference latitude
160 * \param lon0 : reference longitude
161 * \return ublas matrix
162 */
163 template <class T> inline boost::numeric::ublas::matrix<T> C_ENU2ECEF(const T &lat0, const T &lon0){
164 T clat = std::cos(lat0);
165 T slat = std::sin(lat0);
166 T clon = std::cos(lon0);
167 T slon = std::sin(lon0);
168
169 boost::numeric::ublas::matrix<T> C(3,3);
170 C(0,0) = -slon;
171 C(0,1) = -clon * slat;
172 C(0,2) = clon * clat;
173
174 C(1,0) = clon;
175 C(1,1) = -slon * slat;
176 C(1,2) = slon * clat;
177
178 C(2,0) = 0.0;
179 C(2,1) = clat;
180 C(2,2) = slat;
181
182 return C;
183 }
184
185 /*!
186 * \fn inline boost::numeric::ublas::matrix<T> C_ECEF2NED(const T &lat0,const T &lon0)
187 * \brief compute transformation matrix using to convert an ECEF position to a NED position
188 * \param lat0 : reference latitude
189 * \param lon0 : reference longitude
190 * \return ublas matrix
191 */
192 template <class T> inline boost::numeric::ublas::matrix<T> C_ECEF2NED(const T &lat0,const T &lon0){
193
194 T clat = std::cos(lat0);
195 T clon = std::cos(lon0);
196 T slat = std::sin(lat0);
197 T slon = std::sin(lon0);
198
199 boost::numeric::ublas::matrix<T> C(3,3);
200
201 C(0,0) = -slat * clon;
202 C(0,1) = -slat * slon;
203 C(0,2) = clat;
204
205 C(1,0) = -slon;
206 C(1,1) = clon;
207 C(1,2) = 0.0;
208
209 C(2,0) = -clat * clon;
210 C(2,1) = -clat * slon;
211 C(2,2) = -slat;
212
213 return C;
214 }
215
216 /*!
217 * \fn inline boost::numeric::ublas::matrix<T> C_NED2ECEF(const T &lat0, const T &lon0)
218 * \brief compute transformation matrix using to convert NED position to a ECEF position
219 * \param lat0 : reference latitude
220 * \param lon0 : reference longitude
221 * \return ublas matrix
222 */
223 template <class T> inline boost::numeric::ublas::matrix<T> C_NED2ECEF(const T &lat0, const T &lon0){
224
225 T clat = std::cos(lat0);
226 T clon = std::cos(lon0);
227 T slat = std::sin(lat0);
228 T slon = std::sin(lon0);
229
230 boost::numeric::ublas::matrix<T> C(3,3);
231
232 C(0,0) = -slat * clon;
233 C(1,0) = -slat * slon;
234 C(2,0) = clat;
235
236 C(0,1) = -slon;
237 C(1,1) = clon;
238 C(2,1) = 0.0;
239
240 C(0,2) = -clat * clon;
241 C(1,2) = -clat * slon;
242 C(2,2) = -slat;
243
244 return C;
245 }
246
247 /*!
248 * \fn inline boost::numeric::ublas::vector<T> ECEF2TANGENT (const boost::numeric::ublas::matrix<T> & C, const boost::numeric::ublas::vector<T> & P ,const boost::numeric::ublas::vector<T> Ref )
249 * \brief convert an ECEF position to a ENU or NED position
250 * \param C : transformation matrix
251 * \param P : ECEF position
252 * \param Ref : reference position in ECEF frame
253 * \return ublas vector
254 */
255 template <class T> inline boost::numeric::ublas::vector<T>
256 ECEF2TANGENT (const boost::numeric::ublas::matrix<T> & C, const boost::numeric::ublas::vector<T> & P ,const boost::numeric::ublas::vector<T> Ref ){
257 return prod(C,(P-Ref));
258 }
259
260 /*!
261 * \fn inline boost::numeric::ublas::vector<T> TANGENT2ECEF (const boost::numeric::ublas::matrix<T> & C, const boost::numeric::ublas::vector<T> & P ,const boost::numeric::ublas::vector<T> Ref )
262 * \brief convert an ENU or NED position to an ECEF position
263 * \param C : transformation matrix
264 * \param P : ENU or NED position
265 * \param Ref : reference position in ECEF frame
266 * \return ublas vector
267 */
268 template <class T> inline boost::numeric::ublas::vector<T>
269 TANGENT2ECEF (const boost::numeric::ublas::matrix<T> & C, const boost::numeric::ublas::vector<T> & P ,const boost::numeric::ublas::vector<T> Ref ){
270 return Ref+prod(C,P);
271 }
272
273 /*!
274 * \fn inline boost::numeric::ublas::vector<T> ENU2NED(const boost::numeric::ublas::vector<T> &enu)
275 * \brief convert an ENU position to an NED position
276 * \param enu : ENU position
277 * \return ublas vector
278 */
279 template <class T> inline boost::numeric::ublas::vector<T> ENU2NED(const boost::numeric::ublas::vector<T> &enu){
280 boost::numeric::ublas::vector<T> ned(3);
281 ned[0]=enu[1];
282 ned[1]=enu[0];
283 ned[2]=-enu[2];
284 return ned;
285 }
286
287 /*!
288 * \fn inline boost::numeric::ublas::vector<T> NED2ENU(const boost::numeric::ublas::vector<T> &ned)
289 * \brief convert an NED position to an ENU position
290 * \param ned : NED position
291 * \return ublas vector
292 */
293 template <class T> inline boost::numeric::ublas::vector<T> NED2ENU(const boost::numeric::ublas::vector<T> &ned){
294 return ENU2NED(ned);
295 }
296
297
298 /*!
299 * \fn inline boost::numeric::ublas::vector<T> LLH2LAMBERT(const boost::numeric::ublas::vector<T> &llh, T lon0 ,T e, T n, T c ,T Xs, T Ys)
300 * \brief convert an LLH position to a LAMBERT position
301 * \param llh : LLH position
302 * \param lon0 : reference longitude
303 * \param e : ellipsoid eccentricity
304 * \param n :
305 * \param c :
306 * \param Xs : false easting
307 * \param Ys : false northing
308 * \return ublas vector
309 */
310 template <class T> inline boost::numeric::ublas::vector<T> LLH2LAMBERT(const boost::numeric::ublas::vector<T> &llh, T lon0 ,T e, T n, T c ,T Xs, T Ys){;
311 boost::numeric::ublas::vector<T> lambert(3);
312 T latiso= std::log(std::tan((M_PI / 4) + (llh[0] / 2)) * std::pow((1 - e * std::sin(llh[0])) / (1 + e * std::sin(llh[0])),e / 2));
313 lambert[0] = Xs + c*std::exp(-n*latiso)*std::sin(n*(llh[1]-lon0));
314 lambert[1] = Ys - c*std::exp(-n*latiso)*std::cos(n*(llh[1]-lon0));
315 return lambert;
316 }
317
318
319 /*!
320 * \fn inline boost::numeric::ublas::vector<T> LLH2LAMBERT93(const boost::numeric::ublas::vector<T> &llh)
321 * \brief convert an LLH position to a LAMBERT 93 position
322 * \param llh : llh position
323 * \return ublas vector
324 */
325 template <class T> inline boost::numeric::ublas::vector<T> LLH2LAMBERT93(const boost::numeric::ublas::vector<T> &llh){
326// boost::numeric::ublas::vector<T> ecef=LLH2ECEF<T>(llh);
327// boost::numeric::ublas::vector<T> ntf=ECEF2NTF<T>(ecef);
328// boost::numeric::ublas::vector<T> llhntf=ECEF2LLH<T>(ntf,GRS80_a,GRS80_e);
329 return LLH2LAMBERT<T>(llh,LAMBERT93_lon0 ,GRS80_e, LAMBERT93_n, LAMBERT93_c , LAMBERT93_Xs, LAMBERT93_Ys);
330 }
331
332
333 /*!
334 * \fn inline boost::numeric::ublas::vector <T> LAMBERT2LLH(const boost::numeric::ublas::vector<T> &lambert, T lon0, T e , T n, T c, T Xs ,T Ys )
335 * \brief convert an LLH position to a LAMBERT position
336 * \param lambert : lambert position
337 * \param lon0 : reference longitude
338 * \param e : ellipsoid eccentricity
339 * \param n :
340 * \param c :
341 * \param Xs : false easting
342 * \param Ys : false northing
343 * \return ublas vector
344 */
345 template <class T> inline boost::numeric::ublas::vector<T> LAMBERT2LLH(const boost::numeric::ublas::vector<T> &lambert, T lon0, T e , T n, T c, T Xs ,T Ys ){
346 boost::numeric::ublas::vector<T> llh(3);
347
348 T latiso=std::log(c/std::abs(std::sqrt(std::pow(lambert[0]-Xs,2)+std::pow(lambert[1]-Ys,2))))/n;
349 llh[0]=2*std::atan(std::exp(latiso))-M_PI/2;
350 llh[1]=lon0+std::atan(-(lambert[0]-Xs)/(lambert[1]-Ys))/n;
351 T l0=llh[0]+1;
352
353 while(std::abs(llh[0]-l0)>std::pow(10.0,-12.0)){
354 l0=llh[0];
355 llh[0]=2 * std::atan(std::pow((1 + e * std::sin(l0)) / (1 - e * std::sin(l0)), e / 2)*std::exp(latiso)) - M_PI / 2;
356 }
357 return llh;
358 }
359
360 /*!
361 * \fn inline boost::numeric::ublas::vector<T> LAMBERT932LLH(const boost::numeric::ublas::vector<T> &lambert)
362 * \brief convert an LAMBERT93 position to a LLH position
363 * \param lambert : lmabert 9 position
364 * \return ublas vector
365 */
366 template <class T> inline boost::numeric::ublas::vector<T> LAMBERT932LLH(const boost::numeric::ublas::vector<T> &lambert){
367 return LAMBERT2LLH<T>(lambert, LAMBERT93_lon0 ,GRS80_e, LAMBERT93_n, LAMBERT93_c , LAMBERT93_Xs, LAMBERT93_Ys);
368 //boost::numeric::ublas::vector<T> llhntf=LAMBERT2LLHCARTO<T>(lambert, LAMBERT93_lon0 ,GRS80_e, LAMBERT93_n, LAMBERT93_c , LAMBERT93_Xs, LAMBERT93_Ys);
369 //boost::numeric::ublas::vector<T> ecefntf=LLH2ECEF<T>(llhntf);
370 //boost::numeric::ublas::vector<T> ecef=NTF2ECEF<T>(ecefntf);
371 //return ECEF2LLH<T>(ecef);
372 }
373
374 /*!
375 * \fn inline T MeridionalRadius(const T &latitude,const T &a, const T &e)
376 * \brief compute the prime vertical curvature radius
377 * \param latitude : a latitude
378 * \param a : semi-major axis of ellipsoid (by default WGS84 value is used)
379 * \param e : ellipsoid eccentricity (by default WGS84 value is used)
380 * \return ublas vector
381 */
382 template<class T> inline T MeridionalRadius(const T &latitude,const T &a=WGS84_a, const T &e=WGS84_e){
383 return a/(std::pow(1-std::pow(e*std::sin(latitude),2),1.5));
384 }
385
386 /*!
387 * \fn inline T TranversalRadius(const T &latitude,const T &a, const T &e)
388 * \brief compute the prime horizontal curvature radius
389 * \param latitude : a latitude
390 * \param a : semi-major axis of ellipsoid (by default WGS84 value is used)
391 * \param e : ellipsoid eccentricity (by default WGS84 value is used)
392 * \return ublas vector
393 */
394 template<class T> inline T TranversalRadius(const T &latitude,const T &a=WGS84_a, const T &e=WGS84_e){
395 return a/std::sqrt(1-std::pow(e*std::sin(latitude),2));
396 }
397
398
399};
400};
401#endif
402
403
404
Note: See TracBrowser for help on using the repository browser.