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.
- checksums.yaml +7 -0
- data/.rspec +3 -0
- data/CHANGELOG.md +5 -0
- data/CODE_OF_CONDUCT.md +84 -0
- data/Gemfile +10 -0
- data/README.md +86 -0
- data/Rakefile +86 -0
- data/bin/console +15 -0
- data/bin/setup +8 -0
- data/ext/gps_pvt/Coordinate/Coordinate_wrap.cxx +6613 -0
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +16019 -0
- data/ext/gps_pvt/SylphideMath/SylphideMath_wrap.cxx +21050 -0
- data/ext/gps_pvt/extconf.rb +70 -0
- data/ext/ninja-scan-light/tool/navigation/EGM.h +2971 -0
- data/ext/ninja-scan-light/tool/navigation/GPS.h +2432 -0
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver.h +479 -0
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +1081 -0
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_MultiFrequency.h +199 -0
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_RAIM.h +210 -0
- data/ext/ninja-scan-light/tool/navigation/MagneticField.h +928 -0
- data/ext/ninja-scan-light/tool/navigation/NTCM.h +211 -0
- data/ext/ninja-scan-light/tool/navigation/RINEX.h +1781 -0
- data/ext/ninja-scan-light/tool/navigation/WGS84.h +186 -0
- data/ext/ninja-scan-light/tool/navigation/coordinate.h +406 -0
- data/ext/ninja-scan-light/tool/param/bit_array.h +145 -0
- data/ext/ninja-scan-light/tool/param/complex.h +558 -0
- data/ext/ninja-scan-light/tool/param/matrix.h +4049 -0
- data/ext/ninja-scan-light/tool/param/matrix_fixed.h +665 -0
- data/ext/ninja-scan-light/tool/param/matrix_special.h +562 -0
- data/ext/ninja-scan-light/tool/param/quaternion.h +765 -0
- data/ext/ninja-scan-light/tool/param/vector3.h +651 -0
- data/ext/ninja-scan-light/tool/swig/Coordinate.i +177 -0
- data/ext/ninja-scan-light/tool/swig/GPS.i +1102 -0
- data/ext/ninja-scan-light/tool/swig/SylphideMath.i +1234 -0
- data/ext/ninja-scan-light/tool/swig/extconf.rb +5 -0
- data/ext/ninja-scan-light/tool/swig/makefile +53 -0
- data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb +417 -0
- data/ext/ninja-scan-light/tool/swig/spec/SylphideMath_spec.rb +489 -0
- data/gps_pvt.gemspec +57 -0
- data/lib/gps_pvt/receiver.rb +375 -0
- data/lib/gps_pvt/ubx.rb +148 -0
- data/lib/gps_pvt/version.rb +5 -0
- data/lib/gps_pvt.rb +9 -0
- data/sig/gps_pvt.rbs +4 -0
- 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__
|