gps_pvt 0.1.1

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
Files changed (45) hide show
  1. checksums.yaml +7 -0
  2. data/.rspec +3 -0
  3. data/CHANGELOG.md +5 -0
  4. data/CODE_OF_CONDUCT.md +84 -0
  5. data/Gemfile +10 -0
  6. data/README.md +86 -0
  7. data/Rakefile +86 -0
  8. data/bin/console +15 -0
  9. data/bin/setup +8 -0
  10. data/ext/gps_pvt/Coordinate/Coordinate_wrap.cxx +6613 -0
  11. data/ext/gps_pvt/GPS/GPS_wrap.cxx +16019 -0
  12. data/ext/gps_pvt/SylphideMath/SylphideMath_wrap.cxx +21050 -0
  13. data/ext/gps_pvt/extconf.rb +70 -0
  14. data/ext/ninja-scan-light/tool/navigation/EGM.h +2971 -0
  15. data/ext/ninja-scan-light/tool/navigation/GPS.h +2432 -0
  16. data/ext/ninja-scan-light/tool/navigation/GPS_Solver.h +479 -0
  17. data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +1081 -0
  18. data/ext/ninja-scan-light/tool/navigation/GPS_Solver_MultiFrequency.h +199 -0
  19. data/ext/ninja-scan-light/tool/navigation/GPS_Solver_RAIM.h +210 -0
  20. data/ext/ninja-scan-light/tool/navigation/MagneticField.h +928 -0
  21. data/ext/ninja-scan-light/tool/navigation/NTCM.h +211 -0
  22. data/ext/ninja-scan-light/tool/navigation/RINEX.h +1781 -0
  23. data/ext/ninja-scan-light/tool/navigation/WGS84.h +186 -0
  24. data/ext/ninja-scan-light/tool/navigation/coordinate.h +406 -0
  25. data/ext/ninja-scan-light/tool/param/bit_array.h +145 -0
  26. data/ext/ninja-scan-light/tool/param/complex.h +558 -0
  27. data/ext/ninja-scan-light/tool/param/matrix.h +4049 -0
  28. data/ext/ninja-scan-light/tool/param/matrix_fixed.h +665 -0
  29. data/ext/ninja-scan-light/tool/param/matrix_special.h +562 -0
  30. data/ext/ninja-scan-light/tool/param/quaternion.h +765 -0
  31. data/ext/ninja-scan-light/tool/param/vector3.h +651 -0
  32. data/ext/ninja-scan-light/tool/swig/Coordinate.i +177 -0
  33. data/ext/ninja-scan-light/tool/swig/GPS.i +1102 -0
  34. data/ext/ninja-scan-light/tool/swig/SylphideMath.i +1234 -0
  35. data/ext/ninja-scan-light/tool/swig/extconf.rb +5 -0
  36. data/ext/ninja-scan-light/tool/swig/makefile +53 -0
  37. data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb +417 -0
  38. data/ext/ninja-scan-light/tool/swig/spec/SylphideMath_spec.rb +489 -0
  39. data/gps_pvt.gemspec +57 -0
  40. data/lib/gps_pvt/receiver.rb +375 -0
  41. data/lib/gps_pvt/ubx.rb +148 -0
  42. data/lib/gps_pvt/version.rb +5 -0
  43. data/lib/gps_pvt.rb +9 -0
  44. data/sig/gps_pvt.rbs +4 -0
  45. metadata +117 -0
@@ -0,0 +1,186 @@
1
+ /*
2
+ * Copyright (c) 2015, M.Naruoka (fenrir)
3
+ * All rights reserved.
4
+ *
5
+ * Redistribution and use in source and binary forms, with or without modification,
6
+ * are permitted provided that the following conditions are met:
7
+ *
8
+ * - Redistributions of source code must retain the above copyright notice,
9
+ * this list of conditions and the following disclaimer.
10
+ * - Redistributions in binary form must reproduce the above copyright notice,
11
+ * this list of conditions and the following disclaimer in the documentation
12
+ * and/or other materials provided with the distribution.
13
+ * - Neither the name of the naruoka.org nor the names of its contributors
14
+ * may be used to endorse or promote products derived from this software
15
+ * without specific prior written permission.
16
+ *
17
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
19
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS
21
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
22
+ * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
23
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
24
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
25
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
26
+ * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
28
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29
+ *
30
+ */
31
+
32
+ #ifndef __WGS84_H__
33
+ #define __WGS84_H__
34
+
35
+ #include <cmath>
36
+
37
+ /** @file
38
+ * @brief World Geodetic System 1984 (WGS84) (�n�����f��WGS84)
39
+ *
40
+ * �n�����f���ł���WGS84��\������N���X�B
41
+ * �ɗ����a����яd�͂̌v�Z���ł��܂��B
42
+ *
43
+ * CAUTION: Part of parameters are refined.
44
+ *
45
+ * @see NIMA TR 8350.2 Third Edition
46
+ * Department of Defense World Geodetic System 1984
47
+ * Its Definition and Relationships with Local Geodetic Systems
48
+ */
49
+
50
+ template <class FloatT>
51
+ class WGS84Generic{
52
+ public:
53
+ // Defining parameters
54
+ static const FloatT R_e; ///< 3.2.1 (a) Semi-major Axis �ԓ����a [m]
55
+ static const FloatT F_e; ///< 3.2.2 (f) flattening �n���̝G����
56
+ static const FloatT Omega_Earth; ///< 3.2.4 (omega) Angular Velocity of the Earth �n�����]���x [rad/s]
57
+ static const FloatT Omega_Earth_IAU; ///< 3.2.4 (omega') Angular Velocity of the Earth �n�����]���x; IAU, GRS67 version [rad/s]
58
+ static const FloatT mu_Earth; ///< 3.2.3.2 (GM_orig) Earth�fs Gravitational Constant �n���d�͒萔; original [m^3/s^2]
59
+ static const FloatT mu_Earth_refined; ///< 3.2.3 (GM) Earth�fs Gravitational Constant �n���d�͒萔; refined [m^3/s^2]
60
+
61
+ // Derived parameters (refined)
62
+ static const FloatT epsilon_Earth; ///< Table 3.3 (e) First Eccentricity ���ΐS��
63
+ static const FloatT g_WGS0; ///< Table 3.4 (gamma_e) Theoretical (Normal) Gravity at the Equator on the Ellipsoid �ԓ���d�� [m/s^2]
64
+ static const FloatT g_WGS1; ///< Table 3.4 (k) Theoretical (Normal) Gravity Formula Constant �d�͌����萔
65
+ static const FloatT m_derived; ///< Table 3.4 (m) omega^2 * a^2 * b / GM
66
+
67
+ #ifndef pow2
68
+ #define pow2(x) ((x) * (x))
69
+ #else
70
+ #define ALREADY_POW2_DEFINED
71
+ #endif
72
+ /**
73
+ * Calculate curvature radius (north-south)
74
+ * ��k����(���Ȃ킿�o����)�̋ɗ����a�����߂܂��B
75
+ *
76
+ * @param latitude (geodetic) latitude (���n)�ܓx [rad]
77
+ * @return (FloatT) radius ��k�����̋ɗ����a[m]
78
+ */
79
+ static FloatT R_meridian(const FloatT &latitude){
80
+ return R_e * (1. - pow2(epsilon_Earth))
81
+ / std::pow((1. - pow2(epsilon_Earth) * pow2(std::sin(latitude))), 1.5);
82
+ }
83
+
84
+ /**
85
+ * Calculate curvature radius (east-west)
86
+ * ���������̋ɗ����a(�K�ѐ��ȗ����a)�����߂܂��B
87
+ * If the radius of circle of equal latitude, then multiple returned value by cos(geodetic_latitude)
88
+ * ���ܓx�ʂł̔��a���K�v�ȏꍇ�͕ԋp�l��cos(�n���o�x)�������邱�ƁB
89
+ *
90
+ * @param latitude (geodetic) latitude (���n)�ܓx [rad]
91
+ * @return (FloatT) radius ���������̋ɗ����a[m]
92
+ */
93
+ static FloatT R_normal(const FloatT &latitude){
94
+ return R_e / std::sqrt(1. - pow2(epsilon_Earth) * pow2(std::sin(latitude)));
95
+ }
96
+
97
+ /**
98
+ * Calculate gravity
99
+ * �d�͂����߂܂�
100
+ *
101
+ * @param latitude (geodetic) latitude (���n)�ܓx [rad]
102
+ * @param altitude (���x)[m]
103
+ * @return gravity �d��[m/s^2]
104
+ */
105
+ static FloatT gravity(const FloatT &latitude, const FloatT &altitude = 0){
106
+ FloatT slat2(pow2(std::sin(latitude)));
107
+ FloatT g0(g_WGS0 * (1. + g_WGS1 * slat2) / std::sqrt(1. - pow2(epsilon_Earth) * slat2));
108
+ if(altitude == 0){
109
+ return g0;
110
+ }else{
111
+ // @see Eq. (4-3)
112
+ return g0 * (1.0 \
113
+ - (2.0 / R_e * (1.0 + F_e + m_derived - 2.0 * F_e * slat2) * altitude) \
114
+ + (3.0 / pow2(R_e) * pow2(altitude)));
115
+ }
116
+ }
117
+
118
+ struct xz_t {
119
+ FloatT x, z;
120
+ FloatT geocentric_latitude() const {
121
+ return std::atan2(z, x);
122
+ }
123
+ FloatT geodetic_latitude() const {
124
+ return std::atan2(z / (1. - pow2(epsilon_Earth)), x);
125
+ }
126
+ FloatT distance2() const {
127
+ return (x * x) + (z * z);
128
+ }
129
+ FloatT distance() const {
130
+ return std::sqrt(distance2());
131
+ }
132
+ };
133
+
134
+ static xz_t xz(const FloatT &phi_geodetic, const FloatT &height = 0){
135
+ FloatT cphi(std::cos(phi_geodetic)), sphi(std::sin(phi_geodetic));
136
+ FloatT cphi2(pow2(cphi)), sphi2(pow2(sphi)), ba2(1.0 - pow2(epsilon_Earth)), denom(cphi2 + (ba2 * sphi2));
137
+ FloatT x2(pow2(R_e) * cphi2 / denom), z2(pow2(R_e) * pow2(ba2) * sphi2 / denom);
138
+ FloatT x(std::sqrt(x2)), z(std::sqrt(z2) * (phi_geodetic >= 0 ? 1 : -1));
139
+ xz_t res = {x + cphi * height, z + sphi * height};
140
+ return res;
141
+ }
142
+
143
+ static FloatT geocentric_latitude(const FloatT &phi_geodetic, const FloatT &height = 0){
144
+ return xz(phi_geodetic, height).geocentric_latitude();
145
+ }
146
+
147
+ static FloatT geodetic_latitude(const FloatT &phi_geocentric){
148
+ return std::atan2(std::sin(phi_geocentric) / (1.0 - pow2(epsilon_Earth)), std::cos(phi_geocentric));
149
+ }
150
+
151
+ static xz_t xz_geocentric(const FloatT &phi_geocentric, const FloatT &height = 0){
152
+ return xz(geodetic_latitude(phi_geocentric), height);
153
+ }
154
+ #ifdef ALREADY_POW2_DEFINED
155
+ #undef ALREADY_POW2_DEFINED
156
+ #else
157
+ #undef pow2
158
+ #endif
159
+ };
160
+
161
+ template <class FloatT>
162
+ const FloatT WGS84Generic<FloatT>::R_e = 6378137.0;
163
+ template <class FloatT>
164
+ const FloatT WGS84Generic<FloatT>::F_e = (1.0 / 298.257223563);
165
+ template <class FloatT>
166
+ const FloatT WGS84Generic<FloatT>::Omega_Earth = 7292115.0E-11;
167
+ template <class FloatT>
168
+ const FloatT WGS84Generic<FloatT>::Omega_Earth_IAU = 7292115.1467E-11;
169
+ template <class FloatT>
170
+ const FloatT WGS84Generic<FloatT>::mu_Earth = 3986005.0E8;
171
+ template <class FloatT>
172
+ const FloatT WGS84Generic<FloatT>::mu_Earth_refined = 3986004.418E8;
173
+
174
+ template <class FloatT>
175
+ const FloatT WGS84Generic<FloatT>::epsilon_Earth = 8.1819190842622E-2;
176
+ template <class FloatT>
177
+ const FloatT WGS84Generic<FloatT>::g_WGS0 = 9.7803253359;
178
+ template <class FloatT>
179
+ const FloatT WGS84Generic<FloatT>::g_WGS1 = 0.00193185265241;
180
+ template <class FloatT>
181
+ const FloatT WGS84Generic<FloatT>::m_derived = 0.00344978650684;
182
+
183
+
184
+ typedef WGS84Generic<double> WGS84;
185
+
186
+ #endif /* __WGS84_H__ */
@@ -0,0 +1,406 @@
1
+ /*
2
+ * Copyright (c) 2016, M.Naruoka (fenrir)
3
+ * All rights reserved.
4
+ *
5
+ * Redistribution and use in source and binary forms, with or without modification,
6
+ * are permitted provided that the following conditions are met:
7
+ *
8
+ * - Redistributions of source code must retain the above copyright notice,
9
+ * this list of conditions and the following disclaimer.
10
+ * - Redistributions in binary form must reproduce the above copyright notice,
11
+ * this list of conditions and the following disclaimer in the documentation
12
+ * and/or other materials provided with the distribution.
13
+ * - Neither the name of the naruoka.org nor the names of its contributors
14
+ * may be used to endorse or promote products derived from this software
15
+ * without specific prior written permission.
16
+ *
17
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
19
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS
21
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
22
+ * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
23
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
24
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
25
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
26
+ * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
28
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29
+ *
30
+ */
31
+
32
+ #ifndef __COORDINATE_H__
33
+ #define __COORDINATE_H__
34
+
35
+ /** @file
36
+ * @brief coordinate system definition
37
+ */
38
+
39
+ #include <cmath>
40
+
41
+ #ifdef pow2
42
+ #define POW2_ALREADY_DEFINED
43
+ #else
44
+ #define pow2(x) ((x)*(x))
45
+ #endif
46
+
47
+ #ifdef pow3
48
+ #define POW3_ALREADY_DEFINED
49
+ #else
50
+ #define pow3(x) ((x)*(x)*(x))
51
+ #endif
52
+
53
+ #include "param/vector3.h"
54
+ #include "param/matrix.h"
55
+
56
+ #include "WGS84.h"
57
+
58
+ template <class FloatT = double>
59
+ class System_3D {
60
+ public:
61
+ typedef System_3D self_t;
62
+ protected:
63
+ FloatT v[3];
64
+ template <class T>
65
+ static const T &const_ref(T *ptr){return static_cast<const T &>(*ptr);}
66
+ template <class T>
67
+ static T &non_const_ref(const T &ref){return const_cast<T &>(ref);}
68
+ public:
69
+ static const unsigned int value_boundary = 3;
70
+ System_3D(){
71
+ for(unsigned i(0); i < value_boundary; i++){
72
+ v[i] = FloatT(0);
73
+ }
74
+ }
75
+ System_3D(const FloatT &v0, const FloatT &v1, const FloatT &v2){
76
+ v[0] = v0;
77
+ v[1] = v1;
78
+ v[2] = v2;
79
+ }
80
+ System_3D(const self_t &orig){
81
+ for(unsigned i(0); i < value_boundary; i++){
82
+ v[i] = orig.v[i];
83
+ }
84
+ }
85
+ System_3D(const Vector3<FloatT> &vec){
86
+ for(unsigned i(0); i < value_boundary; i++){
87
+ v[i] = vec.get(i);
88
+ }
89
+ }
90
+ template <class Array2D_Type, class ViewType>
91
+ System_3D(const Matrix<FloatT, Array2D_Type, ViewType> &matrix){
92
+ if(matrix.rows() < matrix.columns()){
93
+ for(unsigned i(0); i < value_boundary; i++){
94
+ v[i] = matrix(0, i);
95
+ }
96
+ }else{
97
+ for(unsigned i(0); i < value_boundary; i++){
98
+ v[i] = matrix(i, 0);
99
+ }
100
+ }
101
+ }
102
+ ~System_3D(){}
103
+ System_3D &operator=(const System_3D &another){
104
+ if(this != &another){
105
+ for(unsigned i(0); i < value_boundary; i++){
106
+ v[i] = another.v[i];
107
+ }
108
+ }
109
+ return *this;
110
+ }
111
+
112
+ const FloatT &operator[](const int &i) const {return v[i];}
113
+ FloatT &operator[](const int &i){
114
+ return non_const_ref(const_ref(this)[i]);
115
+ }
116
+
117
+ operator Vector3<FloatT>() const {
118
+ return Vector3<FloatT>(v);
119
+ }
120
+
121
+ friend std::ostream &operator<<(std::ostream &out, const self_t &self){
122
+ out << self[0] << ","
123
+ << self[1] << ","
124
+ << self[2];
125
+ return out;
126
+ }
127
+ friend std::istream &operator>>(std::istream &in, self_t &self){
128
+ in >> self[0];
129
+ in >> self[1];
130
+ in >> self[2];
131
+ return in;
132
+ }
133
+ };
134
+
135
+ template <class FloatT, class Earth> class System_XYZ;
136
+ template <class FloatT, class Earth> class System_LLH;
137
+
138
+ template <class FloatT = double, class Earth = WGS84>
139
+ class System_XYZ : public System_3D<FloatT> {
140
+ public:
141
+ typedef System_XYZ<FloatT, Earth> self_t;
142
+ protected:
143
+ typedef System_3D<FloatT> super_t;
144
+ using super_t::const_ref;
145
+ using super_t::non_const_ref;
146
+ public:
147
+ System_XYZ() : super_t() {}
148
+ System_XYZ(const FloatT &x, const FloatT &y, const FloatT &z)
149
+ : super_t(x, y, z) {}
150
+ System_XYZ(const self_t &xyz)
151
+ : super_t(xyz) {}
152
+ System_XYZ(const Vector3<FloatT> &vec)
153
+ : super_t(vec) {}
154
+ template <class Array2D_Type, class ViewType>
155
+ System_XYZ(const Matrix<FloatT, Array2D_Type, ViewType> &matrix)
156
+ : super_t(matrix) {}
157
+ ~System_XYZ(){}
158
+
159
+ const FloatT &x() const {return super_t::operator[](0);}
160
+ const FloatT &y() const {return super_t::operator[](1);}
161
+ const FloatT &z() const {return super_t::operator[](2);}
162
+
163
+ FloatT &x(){return non_const_ref(const_ref(this).x());}
164
+ FloatT &y(){return non_const_ref(const_ref(this).y());}
165
+ FloatT &z(){return non_const_ref(const_ref(this).z());}
166
+
167
+ self_t &operator+=(const self_t &another){
168
+ x() += another.x();
169
+ y() += another.y();
170
+ z() += another.z();
171
+ return *this;
172
+ }
173
+ self_t operator+(const self_t &another) const {
174
+ self_t copy(*this);
175
+ return copy += another;
176
+ }
177
+ self_t &operator-=(const self_t &another){
178
+ x() -= another.x();
179
+ y() -= another.y();
180
+ z() -= another.z();
181
+ return *this;
182
+ }
183
+ self_t operator-(const self_t &another) const {
184
+ self_t copy(*this);
185
+ return copy -= another;
186
+ }
187
+
188
+ FloatT dist() const {
189
+
190
+ return FloatT(std::sqrt(
191
+ pow2(x()) + pow2(y()) + pow2(z())));
192
+ }
193
+
194
+ FloatT dist(const self_t &another) const {
195
+ return (*this - another).dist();
196
+ }
197
+
198
+ static const FloatT f0, a0, b0, e0;
199
+
200
+ /**
201
+ * Convert to LatLongAlt
202
+ *
203
+ */
204
+ System_LLH<FloatT, Earth> llh() const {
205
+ if((x() == 0) && (y() == 0) && (z() == 0)){
206
+ return System_LLH<FloatT, Earth>(0, 0, -a0);
207
+ }
208
+
209
+ static const FloatT h(pow2(a0) - pow2(b0));
210
+ FloatT p(std::sqrt(pow2(x()) + pow2(y())));
211
+ //cout << "p => " << p << endl;
212
+ FloatT t(std::atan2(z()*a0, p*b0));
213
+ FloatT sint(std::sin(t)), cost(std::cos(t));
214
+
215
+ FloatT _lat(std::atan2(z() + (h / b0 * pow3(sint)), p - (h / a0 * pow3(cost))));
216
+ FloatT n(a0 / std::sqrt(1.0 - pow2(e0) * pow2(std::sin(_lat))));
217
+
218
+ return System_LLH<FloatT, Earth>(
219
+ _lat,
220
+ std::atan2(y(), x()),
221
+ (p / std::cos(_lat) - n));
222
+ }
223
+ };
224
+
225
+ template <class FloatT, class Earth>
226
+ const FloatT System_XYZ<FloatT, Earth>::f0 = Earth::F_e;
227
+ template <class FloatT, class Earth>
228
+ const FloatT System_XYZ<FloatT, Earth>::a0 = Earth::R_e;
229
+ template <class FloatT, class Earth>
230
+ const FloatT System_XYZ<FloatT, Earth>::b0 = Earth::R_e * (1.0 - Earth::F_e);
231
+ template <class FloatT, class Earth>
232
+ const FloatT System_XYZ<FloatT, Earth>::e0 = std::sqrt(Earth::F_e * (2.0 - Earth::F_e));
233
+
234
+ template <class FloatT = double, class Earth = WGS84>
235
+ class System_LLH : public System_3D<FloatT> {
236
+ public:
237
+ typedef System_LLH<FloatT, Earth> self_t;
238
+ typedef System_XYZ<FloatT, Earth> xyz_t;
239
+ protected:
240
+ typedef System_3D<FloatT> super_t;
241
+ using super_t::const_ref;
242
+ using super_t::non_const_ref;
243
+ public:
244
+ System_LLH() : super_t() {}
245
+ System_LLH(const FloatT &latitude, const FloatT &longitude, const FloatT &height)
246
+ : super_t(latitude, longitude, height) {}
247
+ System_LLH(const self_t &llh)
248
+ : super_t(llh) {}
249
+ ~System_LLH(){}
250
+
251
+ const FloatT &latitude() const {return super_t::operator[](0);}
252
+ const FloatT &longitude() const {return super_t::operator[](1);}
253
+ const FloatT &height() const {return super_t::operator[](2);}
254
+
255
+ FloatT &latitude(){return non_const_ref(const_ref(this).latitude());}
256
+ FloatT &longitude(){return non_const_ref(const_ref(this).longitude());}
257
+ FloatT &height(){return non_const_ref(const_ref(this).height());}
258
+
259
+ static void rotation_ecef2enu(
260
+ const FloatT &clat, const FloatT &clng, const FloatT &slat, const FloatT &slng,
261
+ FloatT (&res)[3][3]){
262
+ res[0][0] = - slng; res[0][1] = clng; res[0][2] = 0;
263
+ res[1][0] = -slat * clng; res[1][1] = -slat * slng; res[1][2] = clat;
264
+ res[2][0] = clat * clng; res[2][1] = clat * slng; res[2][2] = slat;
265
+ // identical to Eq.(6) https://gssc.esa.int/navipedia/index.php/Transformations_between_ECEF_and_ENU_coordinates
266
+ }
267
+
268
+ void rotation_ecef2enu(FloatT (&res)[3][3]) const {
269
+ rotation_ecef2enu(
270
+ std::cos(latitude()), std::cos(longitude()), std::sin(latitude()), std::sin(longitude()),
271
+ res);
272
+ }
273
+
274
+ static void rotation_ecef2ned(
275
+ const FloatT &clat, const FloatT &clng, const FloatT &slat, const FloatT &slng,
276
+ FloatT (&res)[3][3]){
277
+ res[0][0] = -slat * clng; res[0][1] = -slat * slng; res[0][2] = clat; // swap row[0] and row[1] of ENU
278
+ res[1][0] = - slng; res[1][1] = clng; res[1][2] = 0;
279
+ res[2][0] = -clat * clng; res[2][1] = -clat * slng; res[2][2] = -slat; // invert polarity of row[2] of ENU
280
+ }
281
+
282
+ void rotation_ecef2ned(FloatT (&res)[3][3]) const {
283
+ rotation_ecef2ned(
284
+ std::cos(latitude()), std::cos(longitude()), std::sin(latitude()), std::sin(longitude()),
285
+ res);
286
+ }
287
+
288
+ /**
289
+ * Convert to ECEF (Earth-centered Earth-Fixed)
290
+ *
291
+ */
292
+ xyz_t xyz() const {
293
+ FloatT
294
+ clat(std::cos(latitude())), clng(std::cos(longitude())),
295
+ slat(std::sin(latitude())), slng(std::sin(longitude()));
296
+ FloatT n(xyz_t::a0 / std::sqrt(1.0 - pow2(xyz_t::e0) * pow2(slat)));
297
+
298
+ return System_XYZ<FloatT, Earth>(
299
+ (n + height()) * clat * clng,
300
+ (n + height()) * clat * slng,
301
+ (n * (1.0 -pow2(xyz_t::e0)) + height()) * slat);
302
+ }
303
+
304
+ friend std::ostream &operator<<(std::ostream &out, const self_t &self){
305
+ out << (self.latitude() / M_PI * 180) << ","
306
+ << (self.longitude() / M_PI * 180) << ","
307
+ << self.height();
308
+ return out;
309
+ }
310
+ friend std::istream &operator>>(std::istream &in, self_t &self){
311
+ FloatT _lat, _lng;
312
+ in >> _lat;
313
+ in >> _lng;
314
+ in >> self[2];
315
+ self.latitude() = _lat / 180 * M_PI;
316
+ self.longitude() = _lng / 180 * M_PI;
317
+ return in;
318
+ }
319
+ };
320
+
321
+ template <class FloatT = double, class Earth = WGS84>
322
+ class System_ENU : public System_3D<FloatT> {
323
+ public:
324
+ typedef System_ENU<FloatT, Earth> self_t;
325
+ typedef System_XYZ<FloatT, Earth> xyz_t;
326
+ typedef System_LLH<FloatT, Earth> llh_t;
327
+ protected:
328
+ typedef System_3D<FloatT> super_t;
329
+ using super_t::const_ref;
330
+ using super_t::non_const_ref;
331
+ public:
332
+ System_ENU() : super_t() {}
333
+ System_ENU(const FloatT &east, const FloatT &north, const FloatT &up)
334
+ : super_t(east, north, up) {}
335
+ System_ENU(const self_t &enu)
336
+ : super_t(enu) {}
337
+ System_ENU(const Vector3<FloatT> &vec)
338
+ : super_t(vec) {}
339
+ template <class Array2D_Type, class ViewType>
340
+ System_ENU(const Matrix<FloatT, Array2D_Type, ViewType> &mat)
341
+ : super_t(mat) {}
342
+ ~System_ENU() {}
343
+
344
+ const FloatT &east() const {return super_t::operator[](0);} ///< ������[m]
345
+ const FloatT &north() const {return super_t::operator[](1);} ///< �k����[m]
346
+ const FloatT &up() const {return super_t::operator[](2);} ///< �㐬��[m]
347
+
348
+ FloatT &east(){return non_const_ref(const_ref(this).east());}
349
+ FloatT &north(){return non_const_ref(const_ref(this).north());}
350
+ FloatT &up(){return non_const_ref(const_ref(this).up());}
351
+
352
+ FloatT horizontal() const {return std::sqrt(std::pow(east(), 2) + std::pow(north(), 2));}
353
+ FloatT vertical() const {return std::abs(up());}
354
+
355
+ static self_t relative_rel(const xyz_t &rel_pos, const llh_t &base_llh){
356
+ FloatT s1(std::sin(base_llh.longitude())),
357
+ c1(std::cos(base_llh.longitude())),
358
+ s2(std::sin(base_llh.latitude())),
359
+ c2(std::cos(base_llh.latitude()));
360
+
361
+ return self_t(
362
+ -rel_pos.x() * s1 + rel_pos.y() * c1,
363
+ -rel_pos.x() * c1 * s2 - rel_pos.y() * s1 * s2 + rel_pos.z() * c2,
364
+ rel_pos.x() * c1 * c2 + rel_pos.y() * s1 * c2 + rel_pos.z() * s2);
365
+ }
366
+
367
+ static self_t relative_rel(const xyz_t &rel_pos, const xyz_t &base){
368
+ return relative_rel(rel_pos, base.llh());
369
+ }
370
+
371
+ static self_t relative(const xyz_t &pos, const xyz_t &base){
372
+ return relative_rel(pos - base, base);
373
+ }
374
+
375
+ xyz_t absolute(const xyz_t &base) const {
376
+ llh_t base_llh(base.llh());
377
+ FloatT s1(std::sin(base_llh.longitude())), c1(std::cos(base_llh.longitude()));
378
+ FloatT s2(std::sin(base_llh.latitude())), c2(std::cos(base_llh.latitude()));
379
+
380
+ return xyz_t(
381
+ -east() * s1 - north() * c1 * s2 + up() * c1 * c2 + base.x(),
382
+ east() * c1 - north() * s1 * s2 + up() * s1 * c2 + base.y(),
383
+ north() * c2 + up() * s2 + base.z());
384
+ }
385
+
386
+ FloatT elevation() const {
387
+ return FloatT(std::atan2(up(), std::sqrt(pow2(east()) + pow2(north()))));
388
+ }
389
+ FloatT azimuth() const {
390
+ return FloatT(std::atan2(east(), north()));
391
+ }
392
+ };
393
+
394
+ #ifdef POW2_ALREADY_DEFINED
395
+ #undef POW2_ALREADY_DEFINED
396
+ #else
397
+ #undef pow2
398
+ #endif
399
+
400
+ #ifdef POW3_ALREADY_DEFINED
401
+ #undef POW3_ALREADY_DEFINED
402
+ #else
403
+ #undef pow3
404
+ #endif
405
+
406
+ #endif // __COORDINATE_H__