Vector3.hpp
2.98 KB
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
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
/**
* @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, size_t N>
class Matrix;
template <typename Type, size_t M>
class Vector;
template <typename Type>
class Dcm;
template <typename Type>
class Vector2;
template<typename Type>
class Vector3 : public Vector<Type, 3>
{
public:
using Matrix31 = Matrix<Type, 3, 1>;
Vector3() = default;
Vector3(const Matrix31 & other) :
Vector<Type, 3>(other)
{
}
explicit Vector3(const Type data_[3]) :
Vector<Type, 3>(data_)
{
}
Vector3(Type x, Type y, Type z) {
Vector3 &v(*this);
v(0) = x;
v(1) = y;
v(2) = z;
}
template<size_t P, size_t Q>
Vector3(const Slice<Type, 3, 1, P, Q>& slice_in) : Vector<Type, 3>(slice_in)
{
}
template<size_t P, size_t Q>
Vector3(const Slice<Type, 1, 3, P, Q>& slice_in) : Vector<Type, 3>(slice_in)
{
}
Vector3 cross(const Matrix31 & b) const {
const Vector3 &a(*this);
return {a(1)*b(2,0) - a(2)*b(1,0), -a(0)*b(2,0) + a(2)*b(0,0), a(0)*b(1,0) - a(1)*b(0,0)};
}
/**
* Override matrix ops so Vector3 type is returned
*/
inline Vector3 operator+(Vector3 other) const
{
return Matrix31::operator+(other);
}
inline Vector3 operator+(Type scalar) const
{
return Matrix31::operator+(scalar);
}
inline Vector3 operator-(Vector3 other) const
{
return Matrix31::operator-(other);
}
inline Vector3 operator-(Type scalar) const
{
return Matrix31::operator-(scalar);
}
inline Vector3 operator-() const
{
return Matrix31::operator-();
}
inline Vector3 operator*(Type scalar) const
{
return Matrix31::operator*(scalar);
}
inline Type operator*(Vector3 b) const
{
return Vector<Type, 3>::operator*(b);
}
inline Vector3 operator%(const Matrix31 & b) const {
return (*this).cross(b);
}
/**
* Override vector ops so Vector3 type is returned
*/
inline Vector3 unit() const {
return Vector3(Vector<Type, 3>::unit());
}
inline Vector3 normalized() const {
return unit();
}
const Slice<Type, 2, 1, 3, 1> xy() const
{
return Slice<Type, 2, 1, 3, 1>(0, 0, this);
}
Slice<Type, 2, 1, 3, 1> xy()
{
return Slice<Type, 2, 1, 3, 1>(0, 0, this);
}
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;
}
};
using Vector3f = Vector3<float>;
using Vector3d = Vector3<double>;
} // namespace matrix
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */