22 #define ALLOW_DOUBLE_MATH_FUNCTIONS 33 double d = WGS84_E * sin(llh[0]);
36 ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
37 ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
38 ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
44 const double p = sqrt(ecef[0]*ecef[0] + ecef[1]*ecef[1]);
48 llh[1] = atan2(ecef[1], ecef[0]);
54 if (p <
WGS84_A *
double(1e-16)) {
55 llh[0] = copysign(
M_PI_2, ecef[2]);
56 llh[2] = fabs(ecef[2]) -
WGS84_B;
62 const double e_c = sqrt(1 - WGS84_E*WGS84_E);
63 const double Z = fabs(ecef[2]) * e_c /
WGS84_A;
74 double A_n, B_n, D_n, F_n;
78 for (
int i=0; i<10; i++)
82 A_n = sqrt(S*S + C*C);
83 D_n = Z*A_n*A_n*A_n + WGS84_E*WGS84_E*S*S*S;
84 F_n = P*A_n*A_n*A_n - WGS84_E*WGS84_E*C*C*C;
85 B_n = double(1.5) * WGS84_E*S*C*C*(A_n*(P*S - Z*C) - WGS84_E*S*C);
121 if (fabs(S - prev_S) <
double(1e-16) && fabs(C - prev_C) <
double(1e-16)) {
129 A_n = sqrt(S*S + C*C);
130 llh[0] = copysign(1.0, ecef[2]) * atan(S / (e_c*C));
131 llh[2] = (p*e_c*C + fabs(ecef[2])*S -
WGS84_A*e_c*A_n) / sqrt(e_c*e_c*C*C + S*S);
void wgsecef2llh(const Vector3d &ecef, Vector3d &llh)
bool is_zero(const T fVal1)
static const double WGS84_B
static const double WGS84_A
void wgsllh2ecef(const Vector3d &llh, Vector3d &ecef)