gps_pvt 0.1.1

Sign up to get free protection for your applications and to get access to all the features.
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 */