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