PX4 Firmware
PX4 Autopilot Software http://px4.io
Vector3.hpp
Go to the documentation of this file.
1 /**
2  * @file Vector3.hpp
3  *
4  * 3D vector class.
5  *
6  * @author James Goppert <james.goppert@gmail.com>
7  */
8 
9 #pragma once
10 
11 #include "math.hpp"
12 
13 namespace matrix
14 {
15 
16 template <typename Type, size_t M, size_t N>
17 class Matrix;
18 
19 template <typename Type, size_t M>
20 class Vector;
21 
22 template <typename Type>
23 class Dcm;
24 
25 template <typename Type>
26 class Vector2;
27 
28 template<typename Type>
29 class Vector3 : public Vector<Type, 3>
30 {
31 public:
32 
34 
35  Vector3() = default;
36 
37  Vector3(const Matrix31 & other) :
38  Vector<Type, 3>(other)
39  {
40  }
41 
42  explicit Vector3(const Type data_[3]) :
43  Vector<Type, 3>(data_)
44  {
45  }
46 
47  Vector3(Type x, Type y, Type z) {
48  Vector3 &v(*this);
49  v(0) = x;
50  v(1) = y;
51  v(2) = z;
52  }
53 
54  template<size_t P, size_t Q>
55  Vector3(const Slice<Type, 3, 1, P, Q>& slice_in) : Vector<Type, 3>(slice_in)
56  {
57  }
58 
59  Vector3 cross(const Matrix31 & b) const {
60  const Vector3 &a(*this);
61  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)};
62  }
63 
64  /**
65  * Override matrix ops so Vector3 type is returned
66  */
67 
68  inline Vector3 operator+(Vector3 other) const
69  {
70  return Matrix31::operator+(other);
71  }
72 
73  inline Vector3 operator-(Vector3 other) const
74  {
75  return Matrix31::operator-(other);
76  }
77 
78  inline Vector3 operator-() const
79  {
80  return Matrix31::operator-();
81  }
82 
83  inline Vector3 operator*(Type scalar) const
84  {
85  return Matrix31::operator*(scalar);
86  }
87 
88  inline Type operator*(Vector3 b) const
89  {
91  }
92 
93  inline Vector3 operator%(const Matrix31 & b) const {
94  return (*this).cross(b);
95  }
96 
97  /**
98  * Override vector ops so Vector3 type is returned
99  */
100  inline Vector3 unit() const {
101  return Vector3(Vector<Type, 3>::unit());
102  }
103 
104  inline Vector3 normalized() const {
105  return unit();
106  }
107 
109  {
110  return Slice<Type, 2, 1, 3, 1>(0, 0, this);
111  }
112 
114  {
115  return Slice<Type, 2, 1, 3, 1>(0, 0, this);
116  }
117 
118 
119  Dcm<Type> hat() const { // inverse to Dcm.vee() operation
120  const Vector3 &v(*this);
121  Dcm<Type> A;
122  A(0,0) = 0;
123  A(0,1) = -v(2);
124  A(0,2) = v(1);
125  A(1,0) = v(2);
126  A(1,1) = 0;
127  A(1,2) = -v(0);
128  A(2,0) = -v(1);
129  A(2,1) = v(0);
130  A(2,2) = 0;
131  return A;
132  }
133 
134 };
135 
137 
138 } // namespace matrix
139 
140 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
Vector3 operator*(Type scalar) const
Definition: Vector3.hpp:83
Vector3 cross(const Matrix31 &b) const
Definition: Vector3.hpp:59
Vector3()=default
Vector3 operator-(Vector3 other) const
Definition: Vector3.hpp:73
Vector3(const Slice< Type, 3, 1, P, Q > &slice_in)
Definition: Vector3.hpp:55
Vector3(Type x, Type y, Type z)
Definition: Vector3.hpp:47
Direction cosine matrix class.
Definition: AxisAngle.hpp:15
Type operator*(const MatrixM1 &b) const
Definition: Vector.hpp:64
Vector3 operator+(Vector3 other) const
Override matrix ops so Vector3 type is returned.
Definition: Vector3.hpp:68
Vector3 operator%(const Matrix31 &b) const
Definition: Vector3.hpp:93
Matrix< Type, M, N > operator-() const
Definition: Matrix.hpp:214
Vector3(const Type data_[3])
Definition: Vector3.hpp:42
Matrix< Type, 3, 1 > Matrix31
Definition: Vector3.hpp:33
Type operator*(Vector3 b) const
Definition: Vector3.hpp:88
Vector3(const Matrix31 &other)
Definition: Vector3.hpp:37
Matrix< Type, M, P > operator*(const Matrix< Type, N, P > &other) const
Matrix Operations.
Definition: Matrix.hpp:140
Vector3< float > Vector3f
Definition: Vector3.hpp:136
Vector3 normalized() const
Definition: Vector3.hpp:104
Slice< Type, 2, 1, 3, 1 > xy()
Definition: Vector3.hpp:113
Vector3 operator-() const
Definition: Vector3.hpp:78
const Slice< Type, 2, 1, 3, 1 > xy() const
Definition: Vector3.hpp:108
Vector3 unit() const
Override vector ops so Vector3 type is returned.
Definition: Vector3.hpp:100
Dcm< Type > hat() const
Definition: Vector3.hpp:119
Matrix< Type, M, N > operator+(const Matrix< Type, M, N > &other) const
Definition: Matrix.hpp:185