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