[5437] | 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 |
|
---|