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,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
+