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,765 @@
|
|
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 __QUATERNION_H
|
33
|
+
#define __QUATERNION_H
|
34
|
+
|
35
|
+
/** @file
|
36
|
+
* @brief �N�H�[�^�j�I��(Quaternion�A4����)���C�u����
|
37
|
+
*
|
38
|
+
* �N�H�[�^�j�I�����`�������C�u�����B
|
39
|
+
*/
|
40
|
+
|
41
|
+
#include <cmath>
|
42
|
+
#include <stdexcept>
|
43
|
+
#include "param/matrix.h"
|
44
|
+
|
45
|
+
#include "param/vector3.h"
|
46
|
+
|
47
|
+
#if (__cplusplus < 201103L) && !defined(noexcept)
|
48
|
+
#define noexcept throw()
|
49
|
+
#endif
|
50
|
+
|
51
|
+
template <class FloatT>
|
52
|
+
struct QuaternionDataProperty{
|
53
|
+
|
54
|
+
#if defined(_MSC_VER) || defined(__TI_COMPILER_VERSION__)
|
55
|
+
static const unsigned int OUT_OF_INDEX = 4; ///< �ő�v�f��(4)
|
56
|
+
#else
|
57
|
+
static const unsigned int OUT_OF_INDEX; ///< �ő�v�f��
|
58
|
+
#endif
|
59
|
+
};
|
60
|
+
|
61
|
+
#if !(defined(_MSC_VER) || defined(__TI_COMPILER_VERSION__))
|
62
|
+
template <class FloatT>
|
63
|
+
const unsigned int QuaternionDataProperty<FloatT>::OUT_OF_INDEX
|
64
|
+
= 4; ///< �ő�v�f��
|
65
|
+
#endif
|
66
|
+
|
67
|
+
template <class FloatT>
|
68
|
+
class QuaternionData : public QuaternionDataProperty<FloatT> {
|
69
|
+
protected:
|
70
|
+
typedef QuaternionDataProperty<FloatT> property_t;
|
71
|
+
typedef QuaternionData<FloatT> self_t;
|
72
|
+
private:
|
73
|
+
struct storage_t{
|
74
|
+
FloatT scalar; ///< �X�J���[�v�f
|
75
|
+
Vector3<FloatT> vector; ///< �x�N�g���v�f
|
76
|
+
int ref; ///< �Q�ƃJ�E���^
|
77
|
+
storage_t() : ref(1) {}
|
78
|
+
storage_t(const FloatT &q0, const Vector3<FloatT> &v)
|
79
|
+
: scalar(q0), vector(v), ref(1) {}
|
80
|
+
storage_t(
|
81
|
+
const FloatT &q0, const FloatT &q1,
|
82
|
+
const FloatT &q2, const FloatT &q3)
|
83
|
+
: scalar(q0), vector(q1, q2, q3), ref(1) {}
|
84
|
+
} *storage; ///< �X�g���[�W
|
85
|
+
protected:
|
86
|
+
/**
|
87
|
+
* �R���X�g���N�^
|
88
|
+
*
|
89
|
+
*/
|
90
|
+
QuaternionData() : storage(new storage_t()){}
|
91
|
+
|
92
|
+
/**
|
93
|
+
* �R���X�g���N�^
|
94
|
+
*
|
95
|
+
* @param q0 �X�J���[�v�f
|
96
|
+
* @param v 3�����x�N�g���v�f
|
97
|
+
*/
|
98
|
+
QuaternionData(const FloatT &q0, const Vector3<FloatT> &v)
|
99
|
+
: storage(new storage_t(q0, v)){}
|
100
|
+
|
101
|
+
/**
|
102
|
+
* �R���X�g���N�^
|
103
|
+
*
|
104
|
+
* @param q0 �X�J���[�v�f
|
105
|
+
* @param q1 3�����x�N�g���v�f��(X)
|
106
|
+
* @param q2 3�����x�N�g���v�f��(Y)
|
107
|
+
* @param q3 3�����x�N�g���v�f��(Z)
|
108
|
+
*/
|
109
|
+
QuaternionData(
|
110
|
+
const FloatT &q0, const FloatT &q1,
|
111
|
+
const FloatT &q2, const FloatT &q3)
|
112
|
+
: storage(new storage_t(q0, q1, q2, q3)) {}
|
113
|
+
|
114
|
+
/**
|
115
|
+
* �R���X�g���N�^�B
|
116
|
+
* �w�肵���l�ŏ���������܂��B
|
117
|
+
*
|
118
|
+
* @param values �v�f�̒l
|
119
|
+
*/
|
120
|
+
QuaternionData(const FloatT (&values)[property_t::OUT_OF_INDEX])
|
121
|
+
: storage(new storage_t(values[0], values[1], values[2], values[3])){
|
122
|
+
|
123
|
+
}
|
124
|
+
|
125
|
+
/**
|
126
|
+
* �R�s�[�R���X�g���N�^
|
127
|
+
*
|
128
|
+
* �V�����[�R�s�[���s���܂��B
|
129
|
+
*
|
130
|
+
* @param q �R�s�[��
|
131
|
+
*/
|
132
|
+
QuaternionData(const self_t &q){
|
133
|
+
if(storage = q.storage){(storage->ref)++;}
|
134
|
+
}
|
135
|
+
|
136
|
+
/**
|
137
|
+
* ������Z�q
|
138
|
+
*
|
139
|
+
* �V�����[�R�s�[�őΉ����Ă��܂��B
|
140
|
+
*
|
141
|
+
* @param q �R�s�[��
|
142
|
+
*/
|
143
|
+
self_t &operator=(const self_t &q){
|
144
|
+
if(this == &q){return *this;}
|
145
|
+
if(storage && ((--(storage->ref)) <= 0)){delete storage;}
|
146
|
+
if(storage = q.storage){(storage->ref)++;}
|
147
|
+
return *this;
|
148
|
+
}
|
149
|
+
|
150
|
+
/**
|
151
|
+
* �f�B�[�v�R�s�[���s���܂��B
|
152
|
+
*
|
153
|
+
* @return (Quarterion) �R�s�[
|
154
|
+
*/
|
155
|
+
self_t deep_copy() const{
|
156
|
+
return self_t(
|
157
|
+
storage->scalar,
|
158
|
+
storage->vector.copy());
|
159
|
+
}
|
160
|
+
|
161
|
+
public:
|
162
|
+
/**
|
163
|
+
* �f�X�g���N�^
|
164
|
+
*
|
165
|
+
* �Q�ƃJ�E���^�����Z���܂��B
|
166
|
+
* �����Q�ƃJ�E���^��0�̏ꍇ�A�g�p���Ă�����������������܂��B
|
167
|
+
*/
|
168
|
+
~QuaternionData(){
|
169
|
+
if(storage && ((--(storage->ref)) <= 0)){
|
170
|
+
delete storage;
|
171
|
+
}
|
172
|
+
}
|
173
|
+
|
174
|
+
/**
|
175
|
+
* �X�J���[�v�f��Ԃ��܂��B
|
176
|
+
*
|
177
|
+
* @return (T) �X�J���[�v�f
|
178
|
+
*/
|
179
|
+
const FloatT &scalar() const noexcept {return storage->scalar;}
|
180
|
+
FloatT &scalar() noexcept {
|
181
|
+
return const_cast<FloatT &>(static_cast<const self_t &>(*this).scalar());
|
182
|
+
}
|
183
|
+
/**
|
184
|
+
* �x�N�g���v�f��Ԃ��܂��B
|
185
|
+
*
|
186
|
+
* @return (Vector<FloatT>) �x�N�g���v�f
|
187
|
+
*/
|
188
|
+
const Vector3<FloatT> &vector() const noexcept {return storage->vector;}
|
189
|
+
Vector3<FloatT> &vector() noexcept {
|
190
|
+
return const_cast<Vector3<FloatT> &>(static_cast<const self_t &>(*this).vector());
|
191
|
+
}
|
192
|
+
|
193
|
+
/**
|
194
|
+
* �v�f(�̎Q��)��Ԃ��܂��B
|
195
|
+
* �]���đ�����\�ł��B
|
196
|
+
*
|
197
|
+
* @param index �v�f�ԍ��A0:�X�J���[�v�f,1�`3:3�����x�N�g���v�fX�`Z
|
198
|
+
* @return (FloatT &) �v�f�ւ̎Q��
|
199
|
+
*/
|
200
|
+
const FloatT &operator[](const unsigned int &index) const {
|
201
|
+
if(index == 0){return storage->scalar;}
|
202
|
+
else{return (storage->vector)[index - 1];}
|
203
|
+
}
|
204
|
+
FloatT &operator[](const unsigned int &index){
|
205
|
+
return const_cast<FloatT &>(static_cast<const self_t &>(*this)[index]);
|
206
|
+
}
|
207
|
+
};
|
208
|
+
|
209
|
+
template <class FloatT>
|
210
|
+
struct QuaternionData_TypeMapper {
|
211
|
+
typedef QuaternionData<FloatT> res_t;
|
212
|
+
};
|
213
|
+
|
214
|
+
/**
|
215
|
+
* @brief �N�H�[�^�j�I��
|
216
|
+
*
|
217
|
+
* �N�H�[�^�j�I��@f$ \Tilde{q} \f$�̓X�J���[�v�f@f$ q_{0} \f$��
|
218
|
+
* 3�����x�N�g���v�f@f$ \vec{q} \f$����Ȃ�
|
219
|
+
* @f[
|
220
|
+
* \Tilde{q} \equiv \begin{Bmatrix} q_{0} \\ \vec{q} \end{Bmatrix}
|
221
|
+
* @f]
|
222
|
+
* �ƕ\������܂��B
|
223
|
+
*
|
224
|
+
* ���ɑS�v�f�̓��a��1�ƂȂ�P�ʃN�H�[�^�j�I����p����ƁA
|
225
|
+
* 3������ł̎p���p��\���ۂ� * �I�C���[�p�Ő�����
|
226
|
+
* @f$ \tan{\frac{\pi}{2}} @f$�Ƃ��������ٓ_��������邱�Ƃ��ł��A
|
227
|
+
* ���炩�ȉ��Z���s�����Ƃ��\�ł��B
|
228
|
+
*
|
229
|
+
* �Ȃ��A�����I�ɎQ�ƃJ�E���^�𗘗p�������C�g�E�G�C�g�Ȏ����ɂȂ��Ă��邽�߁A
|
230
|
+
* �������≉�Z���ߖ�邱�Ƃ��@�̂���܂��B
|
231
|
+
* ���̂��߂ɖ����I��copy()���\�b�h���g�p���Ȃ��ƃf�B�[�v�R�s�[������Ȃ��̂ŁA
|
232
|
+
* �ė��p���s���ۂ͒��ӂ��Ă��������B
|
233
|
+
*
|
234
|
+
* @param FloatT ���Z���x�Adouble�Ȃ�
|
235
|
+
* @see Vector3<FloatT>
|
236
|
+
*/
|
237
|
+
template <class FloatT>
|
238
|
+
class Quaternion : public QuaternionData_TypeMapper<FloatT>::res_t {
|
239
|
+
protected:
|
240
|
+
typedef Quaternion<FloatT> self_t;
|
241
|
+
typedef typename QuaternionData_TypeMapper<FloatT>::res_t super_t;
|
242
|
+
|
243
|
+
Quaternion(const super_t &q) : super_t(q) {}
|
244
|
+
|
245
|
+
public:
|
246
|
+
using super_t::OUT_OF_INDEX;
|
247
|
+
using super_t::operator[];
|
248
|
+
using super_t::scalar;
|
249
|
+
using super_t::vector;
|
250
|
+
|
251
|
+
/**
|
252
|
+
* �R���X�g���N�^
|
253
|
+
* �S�v�f��0�ŏ��������܂��B
|
254
|
+
*/
|
255
|
+
Quaternion()
|
256
|
+
: super_t(FloatT(0), FloatT(0), FloatT(0), FloatT(0)) {}
|
257
|
+
|
258
|
+
/**
|
259
|
+
* �R���X�g���N�^
|
260
|
+
*
|
261
|
+
* @param q0 �X�J���[�v�f
|
262
|
+
* @param v 3�����x�N�g���v�f
|
263
|
+
*/
|
264
|
+
Quaternion(const FloatT &q0, const Vector3<FloatT> &v)
|
265
|
+
: super_t(q0, v){}
|
266
|
+
|
267
|
+
/**
|
268
|
+
* �R���X�g���N�^
|
269
|
+
*
|
270
|
+
* @param q0 �X�J���[�v�f
|
271
|
+
* @param q1 3�����x�N�g���v�f��(X)
|
272
|
+
* @param q2 3�����x�N�g���v�f��(Y)
|
273
|
+
* @param q3 3�����x�N�g���v�f��(Z)
|
274
|
+
*/
|
275
|
+
Quaternion(
|
276
|
+
const FloatT &q0, const FloatT &q1,
|
277
|
+
const FloatT &q2, const FloatT &q3)
|
278
|
+
: super_t(q0, q1, q2, q3) {}
|
279
|
+
|
280
|
+
/**
|
281
|
+
* �R���X�g���N�^�B
|
282
|
+
* �w�肵���l�ŏ���������܂��B
|
283
|
+
*
|
284
|
+
* @param values �v�f�̒l
|
285
|
+
*/
|
286
|
+
Quaternion(const FloatT (&values)[OUT_OF_INDEX]) : super_t(values){}
|
287
|
+
|
288
|
+
/**
|
289
|
+
* �f�X�g���N�^
|
290
|
+
*
|
291
|
+
*/
|
292
|
+
~Quaternion() noexcept {}
|
293
|
+
|
294
|
+
/**
|
295
|
+
* �R�s�[�R���X�g���N�^
|
296
|
+
*
|
297
|
+
* @param q �R�s�[��
|
298
|
+
*/
|
299
|
+
Quaternion(const self_t &q) : super_t(q) {
|
300
|
+
|
301
|
+
}
|
302
|
+
|
303
|
+
/**
|
304
|
+
* ������Z�q
|
305
|
+
*
|
306
|
+
* @param q �R�s�[��
|
307
|
+
*/
|
308
|
+
self_t &operator=(const self_t &q){
|
309
|
+
super_t::operator=(q);
|
310
|
+
return *this;
|
311
|
+
}
|
312
|
+
|
313
|
+
/**
|
314
|
+
* �f�B�[�v�R�s�[���s���܂��B
|
315
|
+
*
|
316
|
+
* @return (Quarterion) �R�s�[
|
317
|
+
*/
|
318
|
+
self_t copy() const{
|
319
|
+
return self_t(super_t::deep_copy());
|
320
|
+
}
|
321
|
+
|
322
|
+
/**
|
323
|
+
* �v�f��ݒ肵�܂��B
|
324
|
+
* �v�f�ԍ��̒�`��operator[](unsigned int)�ɂ���Ē�`����Ă��܂��B
|
325
|
+
*
|
326
|
+
* @param index �v�f�ԍ�
|
327
|
+
* @param value �ݒ肷��l
|
328
|
+
* @see operator[](unsigned int)
|
329
|
+
*/
|
330
|
+
void set(const unsigned int &index, const FloatT &value){(*this)[index] = value;}
|
331
|
+
|
332
|
+
/**
|
333
|
+
* ������Z�q
|
334
|
+
*
|
335
|
+
* @param values �v�f�̒l
|
336
|
+
*/
|
337
|
+
self_t &operator=(const FloatT (&values)[OUT_OF_INDEX]) noexcept {
|
338
|
+
for(int i(0); i < OUT_OF_INDEX; ++i){set(i, values[i]);}
|
339
|
+
return *this;
|
340
|
+
}
|
341
|
+
|
342
|
+
/**
|
343
|
+
* �v�f���擾���܂��B
|
344
|
+
* �v�f�ԍ��̒�`��operator[](unsigned int) const�ɂ���Ē�`����Ă��܂��B
|
345
|
+
*
|
346
|
+
* @param index �v�f�ԍ�
|
347
|
+
* @return FloatT �v�f
|
348
|
+
* @see operator[](unsigned int) const
|
349
|
+
*/
|
350
|
+
const FloatT &get(const unsigned int &index) const{return (*this)[index];}
|
351
|
+
|
352
|
+
/**
|
353
|
+
* �����N�H�[�^�j�I�������߂܂��B
|
354
|
+
* �����N�H�[�^�j�I��@f$ \Tilde{q}^{*} \f$��
|
355
|
+
* @f[
|
356
|
+
* \Tilde{q}^{*} \equiv \begin{Bmatrix} q_{0} \\ - \vec{q} \end{Bmatrix}
|
357
|
+
* @f]
|
358
|
+
* �Œ�`����܂��B
|
359
|
+
* �Ȃ��A���߂�ۂɃf�B�[�v�R�s�[������Ă���̂ŁA�Ȍ�̕ԋp�l�ɑ��鑀��ɂ��āA
|
360
|
+
* ���̃N�H�[�^�j�I���͔�j��ƂȂ�܂��B
|
361
|
+
*
|
362
|
+
* @return (Quarterion<FloatT>) �����N�H�[�^�j�I��
|
363
|
+
*/
|
364
|
+
self_t conj() const{
|
365
|
+
return self_t(scalar(), -vector());
|
366
|
+
}
|
367
|
+
|
368
|
+
#ifndef pow2
|
369
|
+
#define pow2(x) ((x) * (x))
|
370
|
+
#else
|
371
|
+
#define POW2_ALREADY_DEFINED
|
372
|
+
#endif
|
373
|
+
/**
|
374
|
+
* �v�f�̓��a@f$ \left| \Tilde{q} \right|^{2} @f$�����߂܂��B
|
375
|
+
* @f[
|
376
|
+
* \left| \Tilde{q} \right|^{2} \equiv q_{0} {}^{2} + \left| \vec{q} \right|^{2}
|
377
|
+
* @f]
|
378
|
+
* �Œ�`����܂��B
|
379
|
+
*
|
380
|
+
* @return (FloatT) ����
|
381
|
+
*/
|
382
|
+
FloatT abs2() const noexcept {
|
383
|
+
return pow2(scalar()) + vector().abs2();
|
384
|
+
}
|
385
|
+
#ifndef POW2_ALREADY_DEFINED
|
386
|
+
#undef pow2
|
387
|
+
#else
|
388
|
+
#undef POW2_ALREADY_DEFINED
|
389
|
+
#endif
|
390
|
+
/**
|
391
|
+
* �v�f�̓��a�̕�����(�m����)�����߂܂��B
|
392
|
+
*
|
393
|
+
* @return (FloatT) ����
|
394
|
+
* @see abs2()
|
395
|
+
*/
|
396
|
+
FloatT abs() const {return std::sqrt(abs2());}
|
397
|
+
|
398
|
+
/**
|
399
|
+
* �X�J���[�Ƃ̐ώZ���s���܂��B�j��I�ł��B
|
400
|
+
*
|
401
|
+
* @return (Quaternion<FloatT>) ����
|
402
|
+
*/
|
403
|
+
self_t &operator*=(const FloatT &t) noexcept {
|
404
|
+
scalar() *= t;
|
405
|
+
vector() *= t;
|
406
|
+
return *this;
|
407
|
+
}
|
408
|
+
|
409
|
+
/**
|
410
|
+
* �X�J���[�Ƃ̐ώZ���s���܂��B
|
411
|
+
*
|
412
|
+
* @return (Quaternion<FloatT>) ����
|
413
|
+
*/
|
414
|
+
self_t operator*(const FloatT &t) const{return copy() *= t;}
|
415
|
+
|
416
|
+
/**
|
417
|
+
* �X�J���[�Ƃ̏��Z���s���܂��B�j��I�ł��B
|
418
|
+
*
|
419
|
+
* @return (Quaternion<FloatT>) ����
|
420
|
+
*/
|
421
|
+
self_t &operator/=(const FloatT &t){return (*this) *= (FloatT(1) / t);}
|
422
|
+
|
423
|
+
/**
|
424
|
+
* �X�J���[�Ƃ̏��Z���s���܂��B
|
425
|
+
*
|
426
|
+
* @return (Quaternion<FloatT>) ����
|
427
|
+
*/
|
428
|
+
self_t operator/(const FloatT &t) const{return copy() /= t;}
|
429
|
+
|
430
|
+
/**
|
431
|
+
* �m������1�ɂȂ�悤�ɐ��K�����܂��B
|
432
|
+
*
|
433
|
+
* @return (Quaternion<FloatT>) ����
|
434
|
+
* @see abs()
|
435
|
+
*/
|
436
|
+
self_t regularize() const{return (*this) / abs();}
|
437
|
+
|
438
|
+
/**
|
439
|
+
* �N�H�[�^�j�I���Ƃ̐ώZ���s���܂��B
|
440
|
+
* �ώZ @f$ \Tilde{q}_{a} \Tilde{q}_{b} @f$��
|
441
|
+
* @f[
|
442
|
+
* \Tilde{q}_{a} \Tilde{q}_{b}
|
443
|
+
* \equiv \begin{Bmatrix} q_{0a} \\ \vec{q}_{a} \end{Bmatrix}
|
444
|
+
* \begin{Bmatrix} q_{0b} \\ \vec{q}_{b} \end{Bmatrix}
|
445
|
+
* \equiv \begin{Bmatrix}
|
446
|
+
* q_{0a} q_{0b} - \vec{q}_{a} \cdot \vec{q}_{b} \\
|
447
|
+
* q_{0a} \vec{q}_{b} + q_{0b} \vec{q}_{a} + \vec{q}_{a} \times \vec{q}_{b}
|
448
|
+
* \end{Bmatrix}
|
449
|
+
* @f]
|
450
|
+
* �Œ�`����܂��B
|
451
|
+
*
|
452
|
+
* @return (Quaternion<FloatT>) ����
|
453
|
+
*/
|
454
|
+
self_t operator*(const self_t &q) const{
|
455
|
+
self_t result((
|
456
|
+
scalar() * q.scalar())
|
457
|
+
- (vector().innerp(q.vector())),
|
458
|
+
(q.vector() * scalar())
|
459
|
+
+= (vector() * q.scalar())
|
460
|
+
+= (vector() * q.vector()));
|
461
|
+
return result;
|
462
|
+
}
|
463
|
+
|
464
|
+
/**
|
465
|
+
* �N�H�[�^�j�I���Ƃ̐ώZ���s���܂��B�j��I�ł��B
|
466
|
+
* ��`��operator*(const Quaternion &) const���Q�Ƃ��Ă��������B
|
467
|
+
*
|
468
|
+
* @return (Quaternion<FloatT>) ����
|
469
|
+
* @see operator*(const Quaternion<FloatT> &) const
|
470
|
+
*/
|
471
|
+
self_t &operator*=(const self_t &q){return (*this) = (*this) * q;}
|
472
|
+
|
473
|
+
/**
|
474
|
+
* �N�H�[�^�j�I���Ƃ̉��Z���s���܂��B�j��I�ł��B
|
475
|
+
* ���Z @f$ \Tilde{q}_{a} + \Tilde{q}_{b} @f$��
|
476
|
+
* @f[
|
477
|
+
* \Tilde{q}_{a} + \Tilde{q}_{b}
|
478
|
+
* \equiv \begin{Bmatrix} q_{0a} \\ \vec{q}_{a} \end{Bmatrix}
|
479
|
+
* + \begin{Bmatrix} q_{0b} \\ \vec{q}_{b} \end{Bmatrix}
|
480
|
+
* \equiv \begin{Bmatrix}
|
481
|
+
* q_{0a} + q_{0b} \\
|
482
|
+
* \vec{q}_{a} + \vec{q}_{b}
|
483
|
+
* \end{Bmatrix}
|
484
|
+
* @f]
|
485
|
+
* �Œ�`����܂��B
|
486
|
+
*
|
487
|
+
* @return (Quaternion<FloatT>) ����
|
488
|
+
*/
|
489
|
+
self_t &operator+=(const self_t &q) noexcept {
|
490
|
+
for(unsigned int i(0); i < OUT_OF_INDEX; i++){(*this)[i] += q[i];}
|
491
|
+
return *this;
|
492
|
+
}
|
493
|
+
/**
|
494
|
+
* �N�H�[�^�j�I���Ƃ̉��Z���s���܂��B
|
495
|
+
* ��`��operator+=(const Quaternion<FloatT> &) const���Q�Ƃ��Ă��������B
|
496
|
+
*
|
497
|
+
* @return (Quaternion<FloatT>) ����
|
498
|
+
* @see operator+=(const Quaternion<FloatT> &)
|
499
|
+
*/
|
500
|
+
self_t operator+(const self_t &q) const {return copy() += q;}
|
501
|
+
|
502
|
+
/**
|
503
|
+
* �N�H�[�^�j�I���Ƃ̌��Z���s���܂��B�j��I�ł��B
|
504
|
+
* ���Z @f$ \Tilde{q}_{a} - \Tilde{q}_{b} @f$��
|
505
|
+
* @f[
|
506
|
+
* \Tilde{q}_{a} - \Tilde{q}_{b}
|
507
|
+
* \equiv \begin{Bmatrix} q_{0a} \\ \vec{q}_{a} \end{Bmatrix}
|
508
|
+
* - \begin{Bmatrix} q_{0b} \\ \vec{q}_{b} \end{Bmatrix}
|
509
|
+
* \equiv \begin{Bmatrix}
|
510
|
+
* q_{0a} - q_{0b} \\
|
511
|
+
* \vec{q}_{a} - \vec{q}_{b}
|
512
|
+
* \end{Bmatrix}
|
513
|
+
* @f]
|
514
|
+
* �Œ�`����܂��B
|
515
|
+
*
|
516
|
+
* @return (Quaternion<FloatT>) ����
|
517
|
+
*/
|
518
|
+
self_t &operator-=(const self_t &q) noexcept {
|
519
|
+
for(unsigned int i(0); i < OUT_OF_INDEX; i++){(*this)[i] -= q[i];}
|
520
|
+
return *this;
|
521
|
+
}
|
522
|
+
/**
|
523
|
+
* �N�H�[�^�j�I���Ƃ̌��Z���s���܂��B
|
524
|
+
* ��`��operator-=(const Quaternion<FloatT> &)���Q�Ƃ��Ă��������B
|
525
|
+
*
|
526
|
+
* @return (Quaternion<FloatT>) ����
|
527
|
+
* @see operator-=(const Quaternion<FloatT> &)
|
528
|
+
*/
|
529
|
+
self_t operator-(const self_t &q) const{return copy() -= q;}
|
530
|
+
|
531
|
+
/**
|
532
|
+
* 3�����x�N�g���Ƃ̐ώZ���s���܂��B�j��I�ł��B
|
533
|
+
* �ώZ @f$ \Tilde{q} \vec{v} @f$��
|
534
|
+
* @f[
|
535
|
+
* \Tilde{q} \vec{v}
|
536
|
+
* \equiv \begin{Bmatrix} q_{0} \\ \vec{q} \end{Bmatrix}
|
537
|
+
* \begin{Bmatrix} 0 \\ \vec{v} \end{Bmatrix}
|
538
|
+
* \equiv \begin{Bmatrix}
|
539
|
+
* - \vec{q} \cdot \vec{v} \\
|
540
|
+
* q_{0} \vec{v} + \vec{q} \times \vec{v}
|
541
|
+
* \end{Bmatrix}
|
542
|
+
* @f]
|
543
|
+
* �Œ�`����܂��B
|
544
|
+
*
|
545
|
+
* @return (Quaternion<FloatT>) ����
|
546
|
+
*/
|
547
|
+
self_t &operator*=(const Vector3<FloatT> &v) noexcept {
|
548
|
+
FloatT temp_scalar(scalar());
|
549
|
+
scalar() = -(vector().innerp(v));
|
550
|
+
(vector() *= v) += v * temp_scalar;
|
551
|
+
return (*this);
|
552
|
+
}
|
553
|
+
/**
|
554
|
+
* 3�����x�N�g���Ƃ̐ώZ���s���܂��B
|
555
|
+
* ��`��operator*=(const Vector3<FloatT> &)���Q�Ƃ��Ă��������B
|
556
|
+
*
|
557
|
+
* @return (Quaternion<FloatT>) ����
|
558
|
+
* @see operator*=(const Vector3<FloatT> &)
|
559
|
+
*/
|
560
|
+
self_t operator*(const Vector3<FloatT> &v) const{return copy() *= v;}
|
561
|
+
|
562
|
+
/**
|
563
|
+
* ��]�p�̔��������߂܂��B
|
564
|
+
*
|
565
|
+
* @return (T) ����
|
566
|
+
*/
|
567
|
+
FloatT getTheta_2() const{return std::acos(regularize()[0]);}
|
568
|
+
/**
|
569
|
+
* ��]�p�����߂܂��B
|
570
|
+
*
|
571
|
+
* @return (T) ����
|
572
|
+
* @see getTheta_2()
|
573
|
+
*/
|
574
|
+
FloatT getTheta() const{return getTheta_2() * 2;}
|
575
|
+
/**
|
576
|
+
* ��]���̍��W�����߂܂��B
|
577
|
+
*
|
578
|
+
* @return (Vector3<FloatT>) ����
|
579
|
+
*/
|
580
|
+
Vector3<FloatT> getAxis() const{
|
581
|
+
Vector3<FloatT> axis;
|
582
|
+
self_t r(regularize());
|
583
|
+
FloatT theta_2(r.getTheta_2());
|
584
|
+
for(unsigned int i(0); i < Vector3<FloatT>::OUT_OF_INDEX; i++){
|
585
|
+
axis[i] = (*this)[i + 1] / std::sin(theta_2);
|
586
|
+
}
|
587
|
+
return axis;
|
588
|
+
}
|
589
|
+
|
590
|
+
#ifndef pow2
|
591
|
+
#define pow2(x) ((x) * (x))
|
592
|
+
#else
|
593
|
+
#define POW2_ALREADY_DEFINED
|
594
|
+
#endif
|
595
|
+
/**
|
596
|
+
* @f$ 3 \times 3 @f$ ��Direction Cosine Matrix(DCM)�ɕϊ����܂��B
|
597
|
+
*
|
598
|
+
* @return (Matrix<FloatT>) DCM
|
599
|
+
*/
|
600
|
+
Matrix<FloatT> getDCM() const{
|
601
|
+
self_t r = regularize();
|
602
|
+
Matrix<FloatT> dcm = Matrix<FloatT>(3, 3);
|
603
|
+
{
|
604
|
+
//dcm(0, 0) = pow2(r[0]) + pow2(r[1]) - pow2(r[2]) - pow2(r[3]);
|
605
|
+
dcm(0, 0) = FloatT(1) - (pow2(r[2]) + pow2(r[3])) * 2;
|
606
|
+
dcm(0, 1) = ((r[1] * r[2]) + (r[0] * r[3])) * 2;
|
607
|
+
dcm(0, 2) = ((r[1] * r[3]) - (r[0] * r[2])) * 2;
|
608
|
+
|
609
|
+
dcm(1, 0) = ((r[1] * r[2]) - (r[0] * r[3])) * 2;
|
610
|
+
//dcm(1, 1) = pow2(r[0]) - pow2(r[1]) + pow2(r[2]) - pow2(r[3]);
|
611
|
+
dcm(1, 1) = FloatT(1) - (pow2(r[1]) + pow2(r[3])) * 2;
|
612
|
+
dcm(1, 2) = ((r[2] * r[3]) + (r[0] * r[1])) * 2;
|
613
|
+
|
614
|
+
dcm(2, 0) = ((r[1] * r[3]) + (r[0] * r[2])) * 2;
|
615
|
+
dcm(2, 1) = ((r[2] * r[3]) - (r[0] * r[1])) * 2;
|
616
|
+
//dcm(2, 2) = pow2(r[0]) - pow2(r[1]) - pow2(r[2]) + pow2(r[3]);
|
617
|
+
dcm(2, 2) = FloatT(1) - (pow2(r[1]) + pow2(r[2])) * 2;
|
618
|
+
}
|
619
|
+
return dcm;
|
620
|
+
}
|
621
|
+
#ifndef POW2_ALREADY_DEFINED
|
622
|
+
#undef pow2
|
623
|
+
#else
|
624
|
+
#undef POW2_ALREADY_DEFINED
|
625
|
+
#endif
|
626
|
+
|
627
|
+
/**
|
628
|
+
* Quaternion�����₷���`�ŏo�͂��܂��B
|
629
|
+
*
|
630
|
+
* @param out �o�̓X�g���[��
|
631
|
+
* @param q �o�͑Ώ�
|
632
|
+
* @return (ostream) �o�̓X�g���[��
|
633
|
+
*/
|
634
|
+
friend std::ostream &operator<<(std::ostream &out, const self_t &q){
|
635
|
+
for(unsigned int i(0); i < OUT_OF_INDEX; i++){
|
636
|
+
out << (i == 0 ? "{" : ",") << q[i];
|
637
|
+
}
|
638
|
+
out << "}";
|
639
|
+
return out;
|
640
|
+
}
|
641
|
+
|
642
|
+
/**
|
643
|
+
* @f$ 4 \times 1 @f$�s�邢��@f$ 1 \times 4 @f$�s��A
|
644
|
+
* ���邢��DCM�̏�������@f$ 3 \times 3 @f$�s���
|
645
|
+
* Quaternion�^�ɕϊ����܂��B
|
646
|
+
*
|
647
|
+
* @param mat �s��
|
648
|
+
* @throws std::logic_error �s��̃T�C�Y���������Ȃ��Ƃ�
|
649
|
+
*/
|
650
|
+
Quaternion(const Matrix<FloatT> &mat) : super_t() {
|
651
|
+
|
652
|
+
if(mat.rows() == OUT_OF_INDEX && mat.columns() == 1){
|
653
|
+
for(int i(0); i < OUT_OF_INDEX; i++){(*this)[i]= mat(i, 0);}
|
654
|
+
}else if(mat.rows() == 1 && mat.columns() == OUT_OF_INDEX){
|
655
|
+
for(int i(0); i < OUT_OF_INDEX; i++){(*this)[i]= mat(0, i);}
|
656
|
+
}else if((mat.rows() == 3) && (mat.columns() == 3)){
|
657
|
+
|
658
|
+
// TODO: DCM�̏��������Ă��邩�A���ׂ邱��
|
659
|
+
|
660
|
+
// �Ίp�v�f�𑫂��� 3 q_0^2 - (q_1^2 + q_2^2 + q_3^2) == 4 q_0^2 - 1 �ł���
|
661
|
+
// q_0 >= 0�ł��邩��q_0����Ԃ͂��߂ɂƂ���
|
662
|
+
(*this)[0] = std::sqrt(((mat(0, 0) + mat(1, 1) + mat(2, 2)) + 1) / 4);
|
663
|
+
|
664
|
+
if((*this)[0] > 1E-10){
|
665
|
+
// ��Ίp�v�f����c������߂�
|
666
|
+
(*this)[1] = (mat(1, 2) - mat(2, 1)) / 4 / (*this)[0];
|
667
|
+
(*this)[2] = (mat(2, 0) - mat(0, 2)) / 4 / (*this)[0];
|
668
|
+
(*this)[3] = (mat(0, 1) - mat(1, 0)) / 4 / (*this)[0];
|
669
|
+
}else{ // 0�ɋ߂��Ƃ���܂�悭�Ȃ�
|
670
|
+
// �Ίp�v�f(0)+(1)-(2) => (q_0^2 + q_1^2 + q_2^2) - 3 q_3^2 == 1 - 4 q_3^2 �ł���
|
671
|
+
// ���̂Ƃ�q_3�𐳂Ƃ���
|
672
|
+
(*this)[3] = std::sqrt(((mat(0, 0) + mat(1, 1) - mat(2, 2)) - 1) / -4);
|
673
|
+
if((*this)[3] > 1E-10){
|
674
|
+
(*this)[1] = (mat(2, 0) + mat(0, 2)) / 4 / (*this)[3];
|
675
|
+
(*this)[2] = (mat(2, 1) + mat(1, 2)) / 4 / (*this)[3];
|
676
|
+
}else{ // ���܂菬�����Ɩ��
|
677
|
+
// �Ίp�v�f���狁�߂�
|
678
|
+
// �Ίp�v�f-(0)+(1)+(2) => (q_0^2 + q_2^2 + q_3^2) - 3 q_1^2 == 1 - 4 q_1^2
|
679
|
+
// �Ίp�v�f(0)-(1)+(2) => (q_0^2 + q_1^2 + q_3^2) - 3 q_2^2 == 1 - 4 q_2^2
|
680
|
+
// ���̂Ƃ�q_2�𐳂Ƃ��Aq_1�̕����͔�Ίp�v�f���狁�߂�
|
681
|
+
(*this)[1] = std::sqrt(((-mat(0, 0) + mat(1, 1) + mat(2, 2)) - 1) / -4);
|
682
|
+
(*this)[2] = std::sqrt(((mat(0, 0) - mat(1, 1) + mat(2, 2)) - 1) / -4);
|
683
|
+
if(mat(0, 1) + mat(1, 0) < 0){(*this)[1] *= -1;}
|
684
|
+
}
|
685
|
+
}
|
686
|
+
}else{
|
687
|
+
throw std::logic_error("Operatiorn void!! ; Need Matrix(4, 1) or Matrix(1, 4) or DCM_Matrix(3, 3)");
|
688
|
+
}
|
689
|
+
}
|
690
|
+
|
691
|
+
/**
|
692
|
+
* @f$ 4 \times 1 @f$�s��ɕϊ����܂��B
|
693
|
+
*
|
694
|
+
* @return (Matrix<FloatT>) �s��
|
695
|
+
*/
|
696
|
+
Matrix<FloatT> toMatrix() const {
|
697
|
+
Matrix<FloatT> matrix = Matrix<FloatT>(OUT_OF_INDEX, 1);
|
698
|
+
for(int i(0); i < OUT_OF_INDEX; i++){matrix(i, 0) = (*this)[i];}
|
699
|
+
return matrix;
|
700
|
+
}
|
701
|
+
};
|
702
|
+
|
703
|
+
/**
|
704
|
+
* Quaternion data type without fly weight design pattern for performance tuning
|
705
|
+
*
|
706
|
+
* To use this, the following example may be helpful:
|
707
|
+
* template <>
|
708
|
+
* struct QuaternionData_TypeMapper<double> {
|
709
|
+
* typedef QuaternionData_NoFlyWeight<double> res_t;
|
710
|
+
* };
|
711
|
+
*/
|
712
|
+
template <class FloatT>
|
713
|
+
class QuaternionData_NoFlyWeight : public QuaternionDataProperty<FloatT> {
|
714
|
+
protected:
|
715
|
+
typedef QuaternionDataProperty<FloatT> property_t;
|
716
|
+
typedef QuaternionData_NoFlyWeight<FloatT> self_t;
|
717
|
+
private:
|
718
|
+
FloatT _scalar;
|
719
|
+
Vector3<FloatT> _vector;
|
720
|
+
protected:
|
721
|
+
QuaternionData_NoFlyWeight(){}
|
722
|
+
QuaternionData_NoFlyWeight(const FloatT &q0, const Vector3<FloatT> &v)
|
723
|
+
: _scalar(q0), _vector(v){}
|
724
|
+
QuaternionData_NoFlyWeight(
|
725
|
+
const FloatT &q0, const FloatT &q1,
|
726
|
+
const FloatT &q2, const FloatT &q3) noexcept
|
727
|
+
: _scalar(q0), _vector(q1, q2, q3) {}
|
728
|
+
QuaternionData_NoFlyWeight(const FloatT (&v)[property_t::OUT_OF_INDEX]) noexcept
|
729
|
+
: _scalar(v[0]), _vector((const FloatT (&)[property_t::OUT_OF_INDEX - 1])*(&v[1])) {}
|
730
|
+
QuaternionData_NoFlyWeight(const self_t &q) noexcept
|
731
|
+
: _scalar(q._scalar), _vector(q._vector){
|
732
|
+
}
|
733
|
+
self_t &operator=(const self_t &q) noexcept {
|
734
|
+
_scalar = q._scalar;
|
735
|
+
_vector = q._vector;
|
736
|
+
return *this;
|
737
|
+
}
|
738
|
+
self_t deep_copy() const{
|
739
|
+
return self_t(_scalar, _vector.copy());
|
740
|
+
}
|
741
|
+
public:
|
742
|
+
~QuaternionData_NoFlyWeight() noexcept {}
|
743
|
+
const FloatT &scalar() const noexcept {return _scalar;}
|
744
|
+
FloatT &scalar() noexcept {
|
745
|
+
return const_cast<FloatT &>(static_cast<const self_t &>(*this).scalar());
|
746
|
+
}
|
747
|
+
const Vector3<FloatT> &vector() const noexcept {return _vector;}
|
748
|
+
Vector3<FloatT> &vector() noexcept {
|
749
|
+
return const_cast<Vector3<FloatT> &>(static_cast<const self_t &>(*this).vector());
|
750
|
+
}
|
751
|
+
const FloatT &operator[](const unsigned &index) const {
|
752
|
+
if(index == 0){return _scalar;}
|
753
|
+
else{return _vector[index - 1];}
|
754
|
+
}
|
755
|
+
FloatT &operator[](const unsigned &index){
|
756
|
+
return const_cast<FloatT &>(static_cast<const self_t &>(*this)[index]);
|
757
|
+
}
|
758
|
+
};
|
759
|
+
|
760
|
+
#if (__cplusplus < 201103L) && defined(noexcept)
|
761
|
+
#undef noexcept
|
762
|
+
#endif
|
763
|
+
|
764
|
+
#endif /* __QUATERNION_H */
|
765
|
+
|