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