source: pacpusframework/branches/0.0.x/include/Pacpus/PacpusTools/math/geodesy.hpp@ 337

Last change on this file since 337 was 76, checked in by Marek Kurdej, 12 years ago

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