source: ntrip/trunk/GnssCenter/monitor/utils.cpp@ 5447

Last change on this file since 5447 was 5447, checked in by mervart, 9 years ago
File size: 2.5 KB
Line 
1
2#include <math.h>
3
4#include "utils.h"
5
6using namespace std;
7using namespace GnssCenter;
8
9// Rectangular Coordinates -> Ellipsoidal Coordinates
10////////////////////////////////////////////////////////////////////////////
11t_irc xyz2ell(const double* XYZ, double* Ell) {
12
13 const double bell = t_CST::aell*(1.0-1.0/t_CST::fInv) ;
14 const double e2 = (t_CST::aell*t_CST::aell-bell*bell)/(t_CST::aell*t_CST::aell) ;
15 const double e2c = (t_CST::aell*t_CST::aell-bell*bell)/(bell*bell) ;
16
17 double nn, ss, zps, hOld, phiOld, theta, sin3, cos3;
18
19 ss = sqrt(XYZ[0]*XYZ[0]+XYZ[1]*XYZ[1]) ;
20 zps = XYZ[2]/ss ;
21 theta = atan( (XYZ[2]*t_CST::aell) / (ss*bell) );
22 sin3 = sin(theta) * sin(theta) * sin(theta);
23 cos3 = cos(theta) * cos(theta) * cos(theta);
24
25 // Closed formula
26 Ell[0] = atan( (XYZ[2] + e2c * bell * sin3) / (ss - e2 * t_CST::aell * cos3) );
27 Ell[1] = atan2(XYZ[1],XYZ[0]) ;
28 nn = t_CST::aell/sqrt(1.0-e2*sin(Ell[0])*sin(Ell[0])) ;
29 Ell[2] = ss / cos(Ell[0]) - nn;
30
31 const int MAXITER = 100;
32 for (int ii = 1; ii <= MAXITER; ii++) {
33 nn = t_CST::aell/sqrt(1.0-e2*sin(Ell[0])*sin(Ell[0])) ;
34 hOld = Ell[2] ;
35 phiOld = Ell[0] ;
36 Ell[2] = ss/cos(Ell[0])-nn ;
37 Ell[0] = atan(zps/(1.0-e2*nn/(nn+Ell[2]))) ;
38 if ( fabs(phiOld-Ell[0]) <= 1.0e-11 && fabs(hOld-Ell[2]) <= 1.0e-5 ) {
39 return success;
40 }
41 }
42
43 return failure;
44}
45
46// Rectangular Coordinates -> North, East, Up Components
47////////////////////////////////////////////////////////////////////////////
48void xyz2neu(const double* Ell, const double* xyz, double* neu) {
49
50 double sinPhi = sin(Ell[0]);
51 double cosPhi = cos(Ell[0]);
52 double sinLam = sin(Ell[1]);
53 double cosLam = cos(Ell[1]);
54
55 neu[0] = - sinPhi*cosLam * xyz[0]
56 - sinPhi*sinLam * xyz[1]
57 + cosPhi * xyz[2];
58
59 neu[1] = - sinLam * xyz[0]
60 + cosLam * xyz[1];
61
62 neu[2] = + cosPhi*cosLam * xyz[0]
63 + cosPhi*sinLam * xyz[1]
64 + sinPhi * xyz[2];
65}
66
67// North, East, Up Components -> Rectangular Coordinates
68////////////////////////////////////////////////////////////////////////////
69void neu2xyz(const double* Ell, const double* neu, double* xyz) {
70
71 double sinPhi = sin(Ell[0]);
72 double cosPhi = cos(Ell[0]);
73 double sinLam = sin(Ell[1]);
74 double cosLam = cos(Ell[1]);
75
76 xyz[0] = - sinPhi*cosLam * neu[0]
77 - sinLam * neu[1]
78 + cosPhi*cosLam * neu[2];
79
80 xyz[1] = - sinPhi*sinLam * neu[0]
81 + cosLam * neu[1]
82 + cosPhi*sinLam * neu[2];
83
84 xyz[2] = + cosPhi * neu[0]
85 + sinPhi * neu[2];
86}
87
Note: See TracBrowser for help on using the repository browser.