1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
| /**
| * @file Vector3.hpp
| *
| * 3D vector class.
| *
| * @author James Goppert <james.goppert@gmail.com>
| */
|
| #pragma once
|
| #include "math.hpp"
|
| namespace matrix
| {
|
| template <typename Type, size_t M>
| class Vector;
|
| template<typename Type>
| class Dcm;
|
| template<typename Type>
| class Vector3 : public Vector<Type, 3>
| {
| public:
|
| typedef Matrix<Type, 3, 1> Matrix31;
|
| virtual ~Vector3() {};
|
| Vector3() :
| Vector<Type, 3>()
| {
| }
|
| Vector3(const Matrix31 & other) :
| Vector<Type, 3>(other)
| {
| }
|
| Vector3(const Type *data_) :
| Vector<Type, 3>(data_)
| {
| }
|
| Vector3(Type x, Type y, Type z) : Vector<Type, 3>()
| {
| Vector3 &v(*this);
| v(0) = x;
| v(1) = y;
| v(2) = z;
| }
|
| Vector3 cross(const Matrix31 & b) const {
| const Vector3 &a(*this);
| Vector3 c;
| c(0) = a(1)*b(2,0) - a(2)*b(1,0);
| c(1) = -a(0)*b(2,0) + a(2)*b(0,0);
| c(2) = a(0)*b(1,0) - a(1)*b(0,0);
| return c;
| }
|
| Vector3 operator%(const Matrix31 & b) const {
| return (*this).cross(b);
| }
|
| Dcm<Type> hat() const { // inverse to Dcm.vee() operation
| const Vector3 &v(*this);
| Dcm<Type> A;
| A(0,0) = 0;
| A(0,1) = -v(2);
| A(0,2) = v(1);
| A(1,0) = v(2);
| A(1,1) = 0;
| A(1,2) = -v(0);
| A(2,0) = -v(1);
| A(2,1) = v(0);
| A(2,2) = 0;
| return A;
| }
|
| };
|
| typedef Vector3<float> Vector3f;
|
| } // namespace matrix
|
| /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
|
