cgv
Loading...
Searching...
No Matches
geo_transform.cxx
1#include "geo_transform.h"
2#include <cmath>
3
4namespace cgv {
5 namespace math {
6
7 // One millimeter tolerance.
8 static const double POSITION_TOLERANCE = 0.001;
9
10 typedef fvec<float, 3> vec3;
11 typedef fvec<double, 2> dvec2;
12 typedef fvec<double, 3> dvec3;
13
14 dvec3 ECFE_from_geodetic(const dvec3& geodetic, double a, double e)
15 {
16 const double& phi = geodetic.x();
17 const double& lambda = geodetic.y();
18 const double& h = geodetic.z();
19
20 double phi_rad = cgv::math::deg2rad(phi);
21 double lambda_rad = cgv::math::deg2rad(lambda);
22
23 // some precalculations
24 double e_2 = e * e;
25 double sin_phi = sin(phi_rad);
26 double cos_phi = cos(phi_rad);
27 double sin_lambda = sin(lambda_rad);
28 double cos_lambda = cos(lambda_rad);
29 double N = a / sqrt(1 - e_2 * sin_phi * sin_phi);
30
31 //... finally
32 return dvec3(
33 (h + N) * cos_phi * cos_lambda,
34 (h + N) * cos_phi * sin_lambda,
35 (h + (1 - e_2) * N) * sin_phi);
36 }
37
38
39 template <class reference_ellipsoid>
40 void cartesian_to_geodetic_iterative(const double X, const double Y, const double Z,
41 double& phi, double& lambda, double& h)
42 {
43 }
44
45 dvec3 geodetic_from_ECFE(const dvec3& ECFE, double a, double e)
46 {
47 // From: GPStk
48 // Iterative routine to convert cartesian (ECEF) to geodetic coordinates,
49 // (Geoid specified by semi-major axis and eccentricity squared).
50 // @param xyz (input): X,Y,Z in meters
51 // @param llh (output): geodetic lat(deg N), lon(deg E),
52 // height above ellipsoid (meters)
53 const double& X = ECFE[0];
54 const double& Y = ECFE[1];
55 const double& Z = ECFE[2];
56
57 dvec3 geodetic;
58 double& phi = geodetic[0];
59 double& lambda = geodetic[1];
60 double& h = geodetic[2];
61
62 double eccSq = e*e;
63 double p, slat, N, htold, latold;
64 p = sqrt(X * X + Y * Y);
65 if (p < POSITION_TOLERANCE / 5)
66 { // pole or origin
67 phi = (Z > 0 ? 90.0 : -90.0);
68 lambda = 0; // lon undefined, really
69 h = ::fabs(Z) - a * sqrt(1.0 - eccSq);
70 return geodetic;
71 }
72 phi = ::atan2(Z, p * (1.0 - eccSq));
73 h = 0;
74 for (int i = 0; i < 5; i++) {
75 slat = ::sin(phi);
76 N = a / sqrt(1.0 - eccSq * slat * slat);
77 htold = h;
78 h = p / ::cos(phi) - N;
79 latold = phi;
80 phi = ::atan2(Z, p * (1.0 - eccSq * (N / (N + h))));
81 if (::fabs(phi - latold) < 1.0e-9 && fabs(h - htold) < 1.0e-9 * a) break;
82 }
83 lambda = ::atan2(Y, X);
84 if (lambda < 0.0) lambda += 2.0 * cgv::math::constants::phi;
85 phi = cgv::math::rad2deg(phi);
86 lambda = cgv::math::rad2deg(lambda);
87 return geodetic;
88 }
89
90 dvec3 geodetic_from_ECFE_approx(const dvec3& ECFE, double a, double e)
91 {
92 // converts from ECEF-r to ECEF-g in a closed form
93 // input: [X,Y,Z] in meters
94 // output: [phi,lambda,h], lat. (deg N), long. (deg E), height above ellipsoid (in meters) void cartesian_to_geodetic(const double X, const double Y, const double Z, double& phi, double& lambda, double& h)
95 const double& X = ECFE[0];
96 const double& Y = ECFE[1];
97 const double& Z = ECFE[2];
98
99 dvec3 geodetic;
100 double& phi = geodetic[0];
101 double& lambda = geodetic[1];
102 double& h = geodetic[2];
103
104 phi = 0;
105 lambda = 0;
106 h = 0;
107
108 double a_2 = a * a;
109 double e_2 = e * e;
110 double e_4 = e_2 * e_2;
111 double X_2 = X * X;
112 double Y_2 = Y * Y;
113 double Z_2 = Z * Z;
114
115 if (sqrt(X_2 + Y_2 + Z_2) > (a * e_2 / (sqrt(1.0 - e_2)))) {
116 if (!((std::abs(X) < POSITION_TOLERANCE) && (std::abs(Y) < POSITION_TOLERANCE))) // <- TODO: adjust tolerance?
117 {
118 // some intermediate results...
119 double p = (X_2 + Y_2) / a_2;
120 double q = ((1.0 - e_2) / a_2) * Z_2;
121 double r = (p + q - e_4) / 6.0;
122 double s = e_4 * ((p * q) / (4.0 * r * r * r));
123 double t = std::pow(1.0 + s + sqrt(s * (2.0 + s)), 1.0 / 3.0); // <- TODO : could negative values occur under the cube root?
124 double u = r * (1.0 + t + 1.0 / t);
125 double v = sqrt(u * u + e_4 * q);
126 double w = e_2 * (u + v - q) / (2 * v);
127 double k = sqrt(u + v + w * w) - w;
128 double D = (k * sqrt(X_2 + Y_2)) / (k + e_2);
129
130 double D_2 = D * D;
131
132 // ... finally
133
134 // HEIGHT
135 h = ((k + e_2 - 1) / k) * sqrt(D_2 + Z_2);
136
137 // LATITUDE
138 phi = 2 * atan(Z / (D + sqrt(D_2 + Z_2)));
139 phi = cgv::math::rad2deg(phi);
140
141 // LONGITUDE
142 if (Y >= 0.0)
143 {
144 lambda = 0.5 * cgv::math::constants::pi - 2.0 * atan(X / (sqrt(X_2 + Y_2) + Y));
145 }
146 else
147 {
148 lambda = -0.5 * cgv::math::constants::pi + 2.0 * atan(X / (sqrt(X_2 + Y_2) + Y));
149 }
150 lambda = cgv::math::rad2deg(lambda);
151 }
152 else
153 std::cout << "\nError! Input coordinates not valid";
154 }
155 return geodetic;
156 }
157
158 dvec3 ENU_from_geodetic(const dvec3& position_geodetic, const dvec3& origin_geodetic, double a, double e)
159 {
160 const double& phi_geo = position_geodetic[0];
161 const double& lambda_geo = position_geodetic[1];
162 const double& h_geo = position_geodetic[2];
163 const double& phi_origin = origin_geodetic[0];
164 const double& lambda_origin = origin_geodetic[1];
165 const double& h_origin = origin_geodetic[2];
166 // some precalculations
167 double phi_rad = cgv::math::deg2rad(phi_origin);
168 double lambda_rad = cgv::math::deg2rad(lambda_origin);
169 double sin_phi = sin(phi_rad);
170 double cos_phi = cos(phi_rad);
171 double sin_lambda = sin(lambda_rad);
172 double cos_lambda = cos(lambda_rad);
173
174 // calculate ECEF-r coordinates
175 dvec3 position_ECFE = ECFE_from_geodetic(position_geodetic, a, e);
176 dvec3 origin_ECFE = ECFE_from_geodetic(origin_geodetic, a, e);
177 const double& X_geo = position_ECFE[0];
178 const double& Y_geo = position_ECFE[1];
179 const double& Z_geo = position_ECFE[2];
180 const double& X_origin = origin_ECFE[0];
181 const double& Y_origin = origin_ECFE[1];
182 const double& Z_origin = origin_ECFE[2];
183
184 // we translate our coordinate system into the given origin and rotate around z-axis and x-axis
185 // we define a transformation from LTS-r to ECEF-r as follows: x_geo = origin + R * x_lts with R = R_z(phi) * swap(x,y) * R_x(lambda) * swap(y, z)
186 // [R_x/z = cartesian rotation around x-/z-axis]
187 // hence our method has to return the inverse transformation: x_lts = R^T * (x_geo - origin) = swap(y, z) * R_x(lambda) * swap(x,y) * R_z(phi) * (x_geo - origin)
188 double x = X_geo - X_origin;
189 double y = Y_geo - Y_origin;
190 double z = Z_geo - Z_origin;
191
192 return dvec3(
193 -sin_lambda * x + cos_lambda * y,
194 -sin_phi * cos_lambda * x - sin_phi * sin_lambda * y + cos_phi * z,
195 cos_phi * cos_lambda * x + cos_phi * sin_lambda * y + sin_phi * z);
196 }
197 }
198}
cgv
the cgv namespace
Definition print.h:11
cgv::dvec3
cgv::math::fvec< double, 3 > dvec3
declare type of 3d double precision floating point vectors
Definition fvec.h:668
cgv::vec3
cgv::math::fvec< float, 3 > vec3
declare type of 3d single precision floating point vectors
Definition fvec.h:661
cgv::dvec2
cgv::math::fvec< double, 2 > dvec2
declare type of 2d double precision floating point vectors
Definition fvec.h:666