Debian Codegraph @

src/atlas/util/Rotation.cc

raw
1 /*
2 * (C) Copyright 2013 ECMWF.
3 *
4 * This software is licensed under the terms of the Apache Licence Version 2.0
5 * which can be obtained at http://www.apache.org/licenses/LICENSE-2.0.
6 * In applying this licence, ECMWF does not waive the privileges and immunities
7 * granted to it by virtue of its status as an intergovernmental organisation
8 * nor does it submit to any jurisdiction.
9 */
10
11 #include "atlas/util/Rotation.h"
12
13 #include <cmath>
14 #include <iostream>
15
16 #include "eckit/config/Parametrisation.h"
17 #include "eckit/types/FloatCompare.h"
18
19 #include "atlas/util/Constants.h"
20 #include "atlas/util/CoordinateEnums.h"
21 #include "atlas/util/UnitSphere.h"
22
23 namespace atlas {
24 namespace util {
25
26 namespace {
27
28 PointLonLat wrap_latitude(const PointLonLat& p) {
29 double lon = p.lon();
30 double lat = p.lat();
31
32 while (lat > 90.) {
33 lon += 180.;
34 lat = 180. - lat;
35 }
36
37 while (lat < -90.) {
38 lon -= 180.;
39 lat = -180. - lat;
40 }
41
42 return {lon, lat};
43 }
44
45 double wrap_angle(double a) {
46 PointLonLat angle{a, 0};
47 angle.normalise();
48 return angle.lon();
49 }
50
51 } // namespace
52
53 void Rotation::print(std::ostream& out) const {
54 out << "north_pole:" << npole_ << ", south_pole:" << spole_ << ", rotation_angle:" << angle_;
55 }
56 std::ostream& operator<<(std::ostream& out, const Rotation& r) {
57 r.print(out);
58 return out;
59 }
60
61 PointLonLat Rotation::rotate(const PointLonLat& p) const {
62 PointLonLat rotated(p);
63 rotate(rotated.data());
64 return rotated;
65 }
66
67 PointLonLat Rotation::unrotate(const PointLonLat& p) const {
68 PointLonLat unrotated(p);
69 unrotate(unrotated.data());
70 return unrotated;
71 }
72
73 void Rotation::precompute() {
74 const double theta = -(90. + spole_.lat()) * Constants::degreesToRadians();
75 const double phi = -spole_.lon() * Constants::degreesToRadians();
76
77 const double sin_theta = std::sin(theta);
78 const double cos_theta = std::cos(theta);
79 const double sin_phi = std::sin(phi);
80 const double cos_phi = std::cos(phi);
81
82 auto eq = [](double a, double b) { return eckit::types::is_approximately_equal(a, b, 1.e-12); };
83
84 rotated_ = !eq(angle_, 0) || !eq(sin_theta, 0) || !eq(cos_theta, 1) || !eq(sin_phi, 0) || !eq(cos_phi, 1);
85 rotation_angle_only_ = eq(sin_theta, 0) && eq(cos_theta, 1) && rotated_;
86
87 if (!rotated_) {
88 rotate_ = unrotate_ = RotationMatrix{{{1., 0., 0.}, {0., 1., 0.}, {0., 0., 1.}}};
89 return;
90 }
91
92 // Pt = Rot(z) * Rot(y) * P, rotate about y axes then z
93 // Since we're undoing the rotation described in the definition
94 // of the coordinate system,
95 // we first rotate by ϑ = -(90 + spole_.lat()) around the y axis
96 // (along the rotated Greenwich meridian)
97 // and then by φ = -spole_.lon() degrees around the z axis):
98 // (xt) ( cos(φ), sin(φ), 0) ( cos(ϑ), 0, sin(ϑ)) (x)
99 // (yt) = (-sin(φ), cos(φ), 0).( 0 , 1, 0 ).(y)
100 // (zt) ( 0 , 0 , 1) ( -sin(ϑ), 0, cos(ϑ)) (z)
101
102 // Expanded
103 // xt = cos(ϑ) cos(φ) x + sin(φ) y + sin(ϑ) cos(φ) z
104 // yt = -cos(ϑ) sin(φ) x + cos(φ) y - sin(ϑ) sin(φ) z
105 // zt = -sin(ϑ) x + cos(ϑ) z
106
107 rotate_ = RotationMatrix{{{cos_theta * cos_phi, sin_phi, sin_theta * cos_phi},
108 {-cos_theta * sin_phi, cos_phi, -sin_theta * sin_phi},
109 {-sin_theta, 0., cos_theta}}};
110
111 // Assume right hand rule, rotate about z axes and then y
112 // P = Rot(y) * Rot(z) * Pt
113 // x ( cos(ϑ), 0, -sin(ϑ)) ( cos(φ), -sin(φ), 0) (xt)
114 // y = ( 0 , 1, 0 ) ( sin(φ), cos(φ), 0) (yt)
115 // z ( sin(ϑ), 0, cos(ϑ)) ( 0 , 0 , 1) (zt)
116
117 // Expanded
118 // x ( cos(ϑ)cos(φ) , -cos(ϑ)sin(φ) , -sin(ϑ)) (xt)
119 // y = ( sin(φ) , cos(φ) , 0 ).(yt)
120 // z ( sin(ϑ) cos(φ), -sin(ϑ) sin(φ), cos(ϑ)) (zt)
121
122 unrotate_ = RotationMatrix{{{cos_theta * cos_phi, -cos_theta * sin_phi, -sin_theta},
123 {sin_phi, cos_phi, 0.},
124 {sin_theta * cos_phi, -sin_theta * sin_phi, cos_theta}}};
125 }
126
127 Rotation::Rotation(const PointLonLat& south_pole, double rotation_angle) {
128 spole_ = south_pole;
129 npole_ = PointLonLat(spole_.lon() - 180., -spole_.lat());
130 if (npole_.lon() < 0.) {
131 npole_.lon() += 360.;
132 }
133 angle_ = wrap_angle(rotation_angle);
134
135 precompute();
136 }
137
138 Rotation::Rotation(const eckit::Parametrisation& p) {
139 // get rotation angle
140 p.get("rotation_angle", angle_);
141 angle_ = wrap_angle(angle_);
142
143 // get pole
144 std::vector<double> pole(2);
145 if (p.get("north_pole", pole)) {
146 npole_ = PointLonLat(pole.data());
147 spole_ = PointLonLat(npole_.lon() - 180., -npole_.lat());
148 if (spole_.lon() < 0.) {
149 spole_.lon() += 360.;
150 }
151 }
152 else if (p.get("south_pole", pole)) {
153 spole_ = PointLonLat(pole.data());
154 npole_ = PointLonLat(spole_.lon() - 180., -spole_.lat());
155 if (npole_.lon() < 0.) {
156 npole_.lon() += 360.;
157 }
158 }
159
160 precompute();
161 }
162
163 using RotationMatrix = std::array<std::array<double, 3>, 3>;
164
165 inline PointXYZ rotate_geocentric(const PointXYZ& p, const RotationMatrix& R) {
166 return PointXYZ(R[XX][XX] * p.x() + R[XX][YY] * p.y() + R[XX][ZZ] * p.z(),
167 R[YY][XX] * p.x() + R[YY][YY] * p.y() + R[YY][ZZ] * p.z(),
168 R[ZZ][XX] * p.x() + R[ZZ][YY] * p.y() + R[ZZ][ZZ] * p.z());
169 }
170
171 void Rotation::rotate(double crd[]) const {
172 if (!rotated_) {
173 return;
174 }
175
176 crd[LON] -= angle_;
177
178 if (!rotation_angle_only_) {
179 const PointLonLat L(wrap_latitude({crd[LON], crd[LAT]}));
180 PointXYZ P;
181 UnitSphere::convertSphericalToCartesian(L, P);
182
183 const PointXYZ Pt = rotate_geocentric(P, rotate_);
184 PointLonLat Lt;
185 UnitSphere::convertCartesianToSpherical(Pt, Lt);
186
187 crd[LON] = Lt.lon();
188 crd[LAT] = Lt.lat();
189 }
190 }
191
192 void Rotation::unrotate(double crd[]) const {
193 if (!rotated_) {
194 return;
195 }
196
197 if (!rotation_angle_only_) {
198 const PointLonLat Lt(crd);
199 PointXYZ Pt;
200 UnitSphere::convertSphericalToCartesian(Lt, Pt);
201
202 const PointXYZ P = rotate_geocentric(Pt, unrotate_);
203 PointLonLat L;
204 UnitSphere::convertCartesianToSpherical(P, L);
205
206 crd[LON] = L.lon();
207 crd[LAT] = L.lat();
208 }
209
210 crd[LON] += angle_;
211 }
212
213 } // namespace util
214 } // namespace atlas