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