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

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

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