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,651 @@
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 __VECTOR3_H
33
+ #define __VECTOR3_H
34
+
35
+ /** @file
36
+ * @brief 3�����x�N�g�����C�u����
37
+ *
38
+ * 3�����x�N�g�����`�������C�u�����B
39
+ */
40
+
41
+ #include <cmath>
42
+ #include <cstring>
43
+ #include <stdexcept>
44
+ #include "param/matrix.h"
45
+
46
+ #if (__cplusplus < 201103L) && !defined(noexcept)
47
+ #define noexcept throw()
48
+ #endif
49
+
50
+ template <class FloatT>
51
+ struct Vector3DataProperty{
52
+ /**
53
+ * �C���f�b�N�X�̒�`
54
+ *
55
+ */
56
+ enum Index {
57
+ X_INDEX = 0, ///< X�v�f�̓C���f�b�N�X0
58
+ Y_INDEX, ///< Y�v�f�̓C���f�b�N�X1
59
+ Z_INDEX}; ///< Z�v�f�̓C���f�b�N�X2
60
+ #if defined(_MSC_VER) || defined(__TI_COMPILER_VERSION__)
61
+ static const unsigned int OUT_OF_INDEX = 3; ///< �ő�v�f��(3)
62
+ #else
63
+ static const unsigned int OUT_OF_INDEX; ///< �ő�v�f��(3)
64
+ #endif
65
+ };
66
+
67
+ #if !(defined(_MSC_VER) || defined(__TI_COMPILER_VERSION__))
68
+ template <class FloatT>
69
+ const unsigned int Vector3DataProperty<FloatT>::OUT_OF_INDEX
70
+ = Vector3DataProperty<FloatT>::Z_INDEX + 1; ///< �ő�v�f��(3)
71
+ #endif
72
+
73
+ template <class FloatT>
74
+ class Vector3Data : public Vector3DataProperty<FloatT> {
75
+ protected:
76
+ typedef Vector3DataProperty<FloatT> property_t;
77
+ typedef Vector3Data<FloatT> self_t;
78
+
79
+ private:
80
+ struct storage_t{
81
+ FloatT values[property_t::OUT_OF_INDEX]; ///<�v�f�ۑ��p
82
+ int ref; ///<�Q�ƃJ�E���^
83
+ storage_t() : ref(1) {}
84
+ storage_t(const FloatT &x, const FloatT &y, const FloatT &z)
85
+ : ref(1) {
86
+ values[property_t::X_INDEX] = x;
87
+ values[property_t::Y_INDEX] = y;
88
+ values[property_t::Z_INDEX] = z;
89
+ }
90
+ } *storage; ///< �X�g���[�W
91
+
92
+ Vector3Data(storage_t *_storage) : storage(_storage){
93
+
94
+ }
95
+
96
+ protected:
97
+ /**
98
+ * �R���X�g���N�^�B
99
+ */
100
+ Vector3Data() : storage(new storage_t()){
101
+
102
+ }
103
+
104
+ /**
105
+ * �R���X�g���N�^�B
106
+ * �w�肵���l�ŏ���������܂��B
107
+ *
108
+ * @param x X�v�f�̒l
109
+ * @param y Y�v�f�̒l
110
+ * @param z Z�v�f�̒l
111
+ */
112
+ Vector3Data(const FloatT &x, const FloatT &y, const FloatT &z)
113
+ : storage(new storage_t(x, y, z)){
114
+
115
+ }
116
+
117
+ /**
118
+ * �R���X�g���N�^�B
119
+ * �w�肵���l�ŏ���������܂��B
120
+ *
121
+ * @param values �v�f�̒l
122
+ */
123
+ Vector3Data(const FloatT (&values)[property_t::OUT_OF_INDEX])
124
+ : storage(new storage_t(values[0], values[1], values[2])){
125
+
126
+ }
127
+
128
+ /**
129
+ * �R�s�[�R���X�g���N�^
130
+ *
131
+ * �V�����[�R�s�[���s���܂��B
132
+ *
133
+ * @param v �R�s�[��
134
+ */
135
+ Vector3Data(const self_t &v){
136
+ if(storage = v.storage){(storage->ref)++;}
137
+ }
138
+
139
+ /**
140
+ * ������Z�q
141
+ *
142
+ * �V�����[�R�s�[�őΉ����Ă��܂��B
143
+ *
144
+ * @param v �R�s�[��
145
+ */
146
+ self_t &operator=(const self_t &v){
147
+ if(this == &v){return *this;}
148
+ if(storage && ((--(storage->ref)) <= 0)){delete storage;}
149
+ if(storage = v.storage){(storage->ref)++;}
150
+ return *this;
151
+ }
152
+
153
+ /**
154
+ * �f�B�[�v�R�s�[���s���܂��B
155
+ *
156
+ * @return �R�s�[
157
+ */
158
+ self_t deep_copy() const {
159
+ return self_t(storage->values[0], storage->values[1], storage->values[2]);
160
+ }
161
+ public:
162
+ /**
163
+ * �f�X�g���N�^�B
164
+ *
165
+ * �Q�ƃJ�E���^�����Z���܂��B
166
+ * �����Q�ƃJ�E���^��0�̏ꍇ�A�g�p���Ă�����������������܂��B
167
+ */
168
+ ~Vector3Data(){
169
+ if(storage && ((--(storage->ref)) <= 0)){
170
+ delete storage;
171
+ }
172
+ }
173
+
174
+ /**
175
+ * �v�f(�̎Q��)��Ԃ��܂��B
176
+ * �]���đ�����”\�ł��B
177
+ *
178
+ * @param index �v�f�ԍ��A0�`2:�v�fX�`Z
179
+ * @return (FloatT &) �v�f�ւ̎Q��
180
+ */
181
+ const FloatT &operator[](const unsigned int &index) const {
182
+ //if(index => OUT_OF_INDEX){return NULL;}
183
+ return (storage->values)[index];
184
+ }
185
+ FloatT &operator[](const unsigned int &index){
186
+ return const_cast<FloatT &>(static_cast<const self_t &>(*this)[index]);
187
+ }
188
+ };
189
+
190
+ template <class FloatT>
191
+ struct Vector3Data_TypeMapper {
192
+ typedef Vector3Data<FloatT> res_t;
193
+ };
194
+
195
+ /**
196
+ * @brief 3�����x�N�g��
197
+ *
198
+ * 3�����x�N�g���N���X�B
199
+ * �x�N�g�����g�̒�`����ς�O�ςȂǂ��܂߂��l�X�ȉ��Z�̒�`���s���Ă��܂��B
200
+ *
201
+ * �Ȃ��A�����I�ɎQ�ƃJ�E���^�𗘗p�������C�g�E�G�C�g�Ȏ����ɂȂ��Ă��邽�߁A
202
+ * �������≉�Z�񐔂��ߖ񂳂�邱�Ƃ��@�̂���܂��B
203
+ * ���̂��߂ɖ����I��copy()���\�b�h���g�p���Ȃ��ƃf�B�[�v�R�s�[������Ȃ��̂ŁA
204
+ * �ė��p���s���ۂ͒��ӂ��Ă��������B
205
+ *
206
+ * @param FloatT ���Z���x�Adouble�Ȃ�
207
+ */
208
+ template <class FloatT>
209
+ class Vector3 : public Vector3Data_TypeMapper<FloatT>::res_t {
210
+ protected:
211
+ typedef Vector3<FloatT> self_t;
212
+ typedef typename Vector3Data_TypeMapper<FloatT>::res_t super_t;
213
+
214
+ Vector3(const super_t &v) : super_t(v){}
215
+
216
+ public:
217
+ /**
218
+ * �R���X�g���N�^�B
219
+ * �S�v�f��0�ŏ���������܂��B
220
+ */
221
+ Vector3()
222
+ : super_t(FloatT(0), FloatT(0), FloatT(0)){}
223
+
224
+ /**
225
+ * �R���X�g���N�^�B
226
+ * �w�肵���l�ŏ���������܂��B
227
+ *
228
+ * @param x X�v�f�̒l
229
+ * @param y Y�v�f�̒l
230
+ * @param z Z�v�f�̒l
231
+ */
232
+ Vector3(const FloatT &x, const FloatT &y, const FloatT &z)
233
+ : super_t(x, y, z){}
234
+
235
+ /**
236
+ * �f�X�g���N�^�B
237
+ *
238
+ */
239
+ ~Vector3() noexcept {}
240
+
241
+ /**
242
+ * �R�s�[�R���X�g���N�^
243
+ *
244
+ * @param v �R�s�[��
245
+ */
246
+ Vector3(const self_t &v) : super_t(v) {
247
+
248
+ }
249
+
250
+ /**
251
+ * ������Z�q
252
+ *
253
+ * @param v �R�s�[��
254
+ */
255
+ self_t &operator=(const self_t &v){
256
+ super_t::operator=(v);
257
+ return *this;
258
+ }
259
+
260
+ /**
261
+ * �f�B�[�v�R�s�[���s���܂��B
262
+ *
263
+ * @return (Vector3) �R�s�[
264
+ */
265
+ self_t copy() const{
266
+ return self_t(super_t::deep_copy());
267
+ }
268
+
269
+ using super_t::X_INDEX;
270
+ using super_t::Y_INDEX;
271
+ using super_t::Z_INDEX;
272
+ using super_t::OUT_OF_INDEX;
273
+ using super_t::operator[];
274
+
275
+ /**
276
+ * �R���X�g���N�^�B
277
+ * �w�肵���l�ŏ���������܂��B
278
+ *
279
+ * @param values �v�f�̒l
280
+ */
281
+ Vector3(const FloatT (&values)[OUT_OF_INDEX]) : super_t(values){}
282
+
283
+ /**
284
+ * �v�f��ݒ肵�܂��B
285
+ * �v�f�ԍ��̒�`��operator[](const unsigned int &)�ɂ���Ē�`����Ă��܂��B
286
+ *
287
+ * @param index �v�f�ԍ�
288
+ * @param value �ݒ肷��l
289
+ * @see operator[](const unsigned int &)
290
+ */
291
+ void set(const unsigned &index, const FloatT &value){(*this)[index] = value;}
292
+ /**
293
+ * X�v�f��ݒ肵�܂��B
294
+ * @param x �ݒ肷��l
295
+ */
296
+ void setX(const FloatT &x) noexcept {set(X_INDEX, x);}
297
+ /**
298
+ * Y�v�f��ݒ肵�܂��B
299
+ * @param y �ݒ肷��l
300
+ */
301
+ void setY(const FloatT &y) noexcept {set(Y_INDEX, y);}
302
+ /**
303
+ * Z�v�f��ݒ肵�܂��B
304
+ * @param z �ݒ肷��l
305
+ */
306
+ void setZ(const FloatT &z) noexcept {set(Z_INDEX, z);}
307
+
308
+ /**
309
+ * ������Z�q
310
+ *
311
+ * @param values �v�f�̒l
312
+ */
313
+ self_t &operator=(const FloatT (&values)[OUT_OF_INDEX]) noexcept {
314
+ for(int i(0); i < OUT_OF_INDEX; ++i){set(i, values[i]);}
315
+ return *this;
316
+ }
317
+
318
+ /**
319
+ * �v�f���擾���܂��B
320
+ * �v�f�ԍ��̒�`��operator[](const unsigned int &) const�ɂ���Ē�`����Ă��܂��B
321
+ *
322
+ * @param index �v�f�ԍ�
323
+ * @return const FloatT & �v�f
324
+ * @see operator[](const unsigned int &) const
325
+ */
326
+ const FloatT &get(const unsigned &index) const {return (*this)[index];}
327
+ /**
328
+ * X�v�f���擾���܂��B
329
+ * @return FloatT X�v�f
330
+ */
331
+ const FloatT &getX() const noexcept {return get(X_INDEX);}
332
+ /**
333
+ * Y�v�f���擾���܂��B
334
+ * @return FloatT Y�v�f
335
+ */
336
+ const FloatT &getY() const noexcept {return get(Y_INDEX);}
337
+ /**
338
+ * Z�v�f���擾���܂��B
339
+ * @return FloatT Z�v�f
340
+ */
341
+ const FloatT &getZ() const noexcept {return get(Z_INDEX);}
342
+
343
+ #ifndef pow2
344
+ #define pow2(x) ((x) * (x))
345
+ #else
346
+ #define POW2_ALREADY_DEFINED
347
+ #endif
348
+ /**
349
+ * 3�����x�N�g��@f$ \vec{v} @f$�̃m����@f$ \left| \vec{v} \right| @f$��
350
+ * �������߂܂��B
351
+ * ��`��3�����x�N�g��
352
+ * @f$ \vec{v} \equiv \begin{pmatrix} x \\ y \\ z \end{pmatrix} @f$�ɑ΂�
353
+ * @f[
354
+ * \left| \vec{v} \right|^{2} \equiv x^{2} + y^{2} + z^{2}
355
+ * @f]
356
+ * �ł��B
357
+ *
358
+ * @return (T) ����
359
+ */
360
+ FloatT abs2() const noexcept {
361
+ FloatT result(0);
362
+ for(unsigned int i(0); i < OUT_OF_INDEX; i++){result += pow2((*this)[i]);}
363
+ return result;
364
+ }
365
+ #ifndef POW2_ALREADY_DEFINED
366
+ #undef pow2
367
+ #else
368
+ #undef POW2_ALREADY_DEFINED
369
+ #endif
370
+ /**
371
+ * 3�����x�N�g��@f$ \vec{v} @f$�̃m����@f$ \left| \vec{v} \right| @f$��
372
+ * �����߂܂��B
373
+ * ��`��abs2()���Q�Ƃ��Ă��������B
374
+ *
375
+ * @return (T) ����
376
+ * @see abs2()
377
+ */
378
+ FloatT abs() const{return sqrt(abs2());}
379
+
380
+ /**
381
+ * �P���}�C�i�X�I�y���[�^�B
382
+ *
383
+ * @return (Vector3<FloatT>) �S�v�f�Ƀ}�C�i�X���‚�������
384
+ */
385
+ self_t operator-() const{return (copy() *= -1);}
386
+
387
+ /**
388
+ * �X�J���[�ς����܂��B�j��I�ł��B
389
+ *
390
+ * @param t �X�J���[
391
+ * @return (Vector3<FloatT>) ����
392
+ */
393
+ self_t &operator*=(const FloatT &t) noexcept {
394
+ for(unsigned int i(0); i < OUT_OF_INDEX; i++){(*this)[i] *= t;}
395
+ return *this;
396
+ }
397
+ /**
398
+ * �X�J���[�ς����܂��B
399
+ *
400
+ * @param t �X�J���[
401
+ * @return (Vector3<FloatT>) ����
402
+ * @see oprtator*=(const FloatT &t)
403
+ */
404
+ self_t operator*(const FloatT &t) const{return copy() *= t;}
405
+
406
+ /**
407
+ * �X�J���[�������܂��B�j��I�ł��B
408
+ *
409
+ * @param t �X�J���[
410
+ * @return (Vector3<FloatT>) ����
411
+ */
412
+ self_t &operator/=(const FloatT &t){return (*this) *= (FloatT(1) / t);}
413
+ /**
414
+ * �X�J���[�������܂��B
415
+ *
416
+ * @param t �X�J���[
417
+ * @return (Vector3<FloatT>) ����
418
+ * @see oprtator/=(const FloatT &t)
419
+ */
420
+ self_t operator/(const FloatT &t) const{return copy() /= t;}
421
+
422
+ /**
423
+ * 3�����x�N�g���Ƃ̉��Z�����܂��B�j��I�ł��B
424
+ *
425
+ * @param v 3�����x�N�g��
426
+ * @return (Vector3<FloatT>) ����
427
+ */
428
+ self_t &operator+=(const self_t &v) noexcept {
429
+ for(unsigned int i(0); i < OUT_OF_INDEX; i++){(*this)[i] += v[i];}
430
+ return *this;
431
+ }
432
+ /**
433
+ * 3�����x�N�g���Ƃ̉��Z�����܂��B
434
+ *
435
+ * @param v 3�����x�N�g��
436
+ * @return (Vector3<FloatT>) ����
437
+ * @see operator+=(const Vector3<FloatT> &)
438
+ */
439
+ self_t operator+(const self_t &v) const{return copy() += v;}
440
+
441
+ /**
442
+ * 3�����x�N�g���Ƃ̌��Z�����܂��B�j��I�ł��B
443
+ *
444
+ * @param v 3�����x�N�g��
445
+ * @return (Vector3<FloatT>) ����
446
+ */
447
+ self_t &operator-=(const self_t &v) noexcept {
448
+ for(unsigned int i(0); i < OUT_OF_INDEX; i++){(*this)[i] -= v[i];}
449
+ return *this;
450
+ }
451
+ /**
452
+ * 3�����x�N�g���Ƃ̌��Z�����܂��B
453
+ *
454
+ * @param v 3�����x�N�g��
455
+ * @return (Vector3<FloatT>) ����
456
+ * @see operator-=(const Vector3<FloatT> &)
457
+ */
458
+ self_t operator-(const self_t &v) const{return copy() -= v;}
459
+
460
+ /**
461
+ * �O�ς����܂��B
462
+ *
463
+ * @param v 3�����x�N�g��
464
+ * @return (Vector3<FloatT>) ����
465
+ */
466
+ self_t operator*(const self_t &v) const{
467
+ self_t result;
468
+ result[0] = (*this)[1] * v[2] - (*this)[2] * v[1];
469
+ result[1] = (*this)[2] * v[0] - (*this)[0] * v[2];
470
+ result[2] = (*this)[0] * v[1] - (*this)[1] * v[0];
471
+ return result;
472
+ }
473
+
474
+ /**
475
+ * ���ς����܂��B
476
+ *
477
+ * @param v 3�����x�N�g��
478
+ * @return (Vector3<FloatT>) ����
479
+ */
480
+ FloatT innerp(const self_t &v) const noexcept {
481
+ FloatT result(0);
482
+ for(unsigned int i(0); i < OUT_OF_INDEX; i++){
483
+ result += (*this)[i] * v[i];
484
+ }
485
+ return result;
486
+ }
487
+
488
+ /**
489
+ * �O�ς����܂��B�j��I�ł��B
490
+ *
491
+ * @param v 3�����x�N�g��
492
+ * @return (Vector3<FloatT>) ����
493
+ * @see operator*(const Vector3<FloatT> &) const
494
+ */
495
+ self_t &operator*=(const self_t &v){
496
+ FloatT temp0((*this)[0]), temp1((*this)[1]);
497
+ (*this)[0] = (*this)[1] * v[2] - (*this)[2] * v[1];
498
+ (*this)[1] = (*this)[2] * v[0] - temp0 * v[2];
499
+ (*this)[2] = temp0 * v[1] - temp1 * v[0];
500
+ return *this;
501
+ }
502
+
503
+ /**
504
+ * 3�����x�N�g�������₷���`�ŏo�͂��܂��B
505
+ *
506
+ * @param out �o�̓X�g���[��
507
+ * @param v �o�͑Ώ�
508
+ * @return (ostream) �o�̓X�g���[��
509
+ */
510
+ friend std::ostream &operator<<(std::ostream &out, const self_t &v){
511
+ out << "{";
512
+ for(unsigned int i(0); i < OUT_OF_INDEX; i++){
513
+ out << (i == 0 ? "" : ",") << v[i];
514
+ }
515
+ out << "}";
516
+ return out;
517
+ }
518
+
519
+ /**
520
+ * @f$ 3 \times 1 @f$�s�񂠂邢��@f$ 1 \times 3 @f$�s���Vector3�^�ɕϊ����܂��B
521
+ *
522
+ * @param matrix �s��
523
+ * @throw std::logic_error �s��̃T�C�Y���������Ȃ��Ƃ�
524
+ */
525
+ Vector3(const Matrix<FloatT> &matrix) : super_t() {
526
+ if((matrix.rows() == OUT_OF_INDEX) && (matrix.columns() == 1)){
527
+ for(unsigned int i = 0; i < OUT_OF_INDEX; i++){
528
+ (*this)[i] = matrix(i, 0);
529
+ }
530
+ }else if((matrix.rows() == 1) && (matrix.columns() == OUT_OF_INDEX)){
531
+ for(unsigned int i = 0; i < OUT_OF_INDEX; i++){
532
+ (*this)[i] = matrix(0, i);
533
+ }
534
+ }else{
535
+ throw std::logic_error("Operatiorn void!! ; Need Matrix(3, 1) or Matrix(1, 3)");
536
+ }
537
+ }
538
+
539
+ friend self_t operator*(const Matrix<FloatT> &matrix, const self_t &vec){
540
+ if((matrix.rows() != OUT_OF_INDEX) || (matrix.columns() != OUT_OF_INDEX)){
541
+ throw std::logic_error("Operatiorn void!! ; Need Matrix(3, 3)");
542
+ }
543
+ self_t res;
544
+ for(unsigned int i(0); i < OUT_OF_INDEX; i++){
545
+ res[i] = FloatT(0);
546
+ for(unsigned int j(0); j < OUT_OF_INDEX; j++){
547
+ res[i] += matrix(i, j) * vec[j];
548
+ }
549
+ }
550
+ return res;
551
+ }
552
+
553
+ /**
554
+ * @f$ 3 \times 1 @f$�s��ɕϊ����܂��B
555
+ *
556
+ * @return (Matrix<FloatT>) �s��
557
+ */
558
+ Matrix<FloatT> toMatrix() const{
559
+ Matrix<FloatT> matrix(OUT_OF_INDEX, 1);
560
+ for(unsigned int i(0); i < OUT_OF_INDEX; i++){matrix(i, 0) = (*this)[i];}
561
+ return matrix;
562
+ }
563
+
564
+ /**
565
+ * �O�ς��s��̐ς̌`�ɕϊ����܂��B���Ȃ킿�A
566
+ * @f$ \vec{v}_{\mathrm{this}} * \vec{v}_{\mathrm{another}}
567
+ * = \begin{bmatrix}
568
+ * 0 & -v_{Z} & v_{Y} \\
569
+ * v_{Z} & 0 & -v_{X} \\
570
+ * -v_{Y} & v_{X} & 0
571
+ * \end{bmatrix}_{v_\mathrm{this}}
572
+ * v_{\mathrm{another}} @f$
573
+ * �ƂȂ�܂��B���Ȃ݂�
574
+ * @f$ \vec{v}_{\mathrm{another}} * \vec{v}_{\mathrm{this}}
575
+ * = \begin{bmatrix}
576
+ * 0 & v_{Z} & -v_{Y} \\
577
+ * -v_{Z} & 0 & v_{X} \\
578
+ * v_{Y} & -v_{X} & 0
579
+ * \end{bmatrix}_{v_\mathrm{this}}
580
+ * v_{\mathrm{another}}
581
+ * = - \left[ \; \right]_{v_\mathrm{this}} v_{\mathrm{another}} @f$
582
+ * �ł��B
583
+ *
584
+ */
585
+ Matrix<FloatT> skewMatrix() const{
586
+ Matrix<FloatT> matrix(OUT_OF_INDEX, OUT_OF_INDEX);
587
+ {
588
+ matrix(0, 1) = -(*this)[2];
589
+ matrix(0, 2) = (*this)[1];
590
+ matrix(1, 0) = (*this)[2];
591
+ matrix(1, 2) = -(*this)[0];
592
+ matrix(2, 0) = -(*this)[1];
593
+ matrix(2, 1) = (*this)[0];
594
+ }
595
+ return matrix;
596
+ }
597
+ };
598
+
599
+ /**
600
+ * Vector3 data type without fly weight design pattern for performance tuning
601
+ *
602
+ * To use this, the following example may be helpful:
603
+ * template <>
604
+ * struct Vector3Data_TypeMapper<double> {
605
+ * typedef Vector3Data_NoFlyWeight<double> res_t;
606
+ * };
607
+ */
608
+ template <class FloatT>
609
+ class Vector3Data_NoFlyWeight : public Vector3DataProperty<FloatT> {
610
+ protected:
611
+ typedef Vector3DataProperty<FloatT> property_t;
612
+ typedef Vector3Data_NoFlyWeight<FloatT> self_t;
613
+ private:
614
+ FloatT values[property_t::OUT_OF_INDEX];
615
+ protected:
616
+ Vector3Data_NoFlyWeight() noexcept {}
617
+ Vector3Data_NoFlyWeight(const FloatT &x, const FloatT &y, const FloatT &z) noexcept {
618
+ values[property_t::X_INDEX] = x;
619
+ values[property_t::Y_INDEX] = y;
620
+ values[property_t::Z_INDEX] = z;
621
+ }
622
+ Vector3Data_NoFlyWeight(const FloatT (&v)[property_t::OUT_OF_INDEX]) noexcept {
623
+ std::memcpy(values, &v, sizeof(values));
624
+ }
625
+ Vector3Data_NoFlyWeight(const self_t &v) noexcept {
626
+ std::memcpy(values, v.values, sizeof(values));
627
+ }
628
+ self_t &operator=(const self_t &v) noexcept {
629
+ std::memcpy(values, v.values, sizeof(values));
630
+ return *this;
631
+ }
632
+ self_t deep_copy() const {
633
+ self_t copied;
634
+ std::memcpy(copied.values, values, sizeof(values));
635
+ return copied;
636
+ }
637
+ public:
638
+ ~Vector3Data_NoFlyWeight() noexcept {}
639
+ const FloatT &operator[](const unsigned int &index) const {
640
+ return values[index];
641
+ }
642
+ FloatT &operator[](const unsigned int &index){
643
+ return const_cast<FloatT &>(static_cast<const self_t &>(*this)[index]);
644
+ }
645
+ };
646
+
647
+ #if (__cplusplus < 201103L) && defined(noexcept)
648
+ #undef noexcept
649
+ #endif
650
+
651
+ #endif /* __VECTOR3_H */