PX4 Firmware
PX4 Autopilot Software http://px4.io
Vector.hpp
Go to the documentation of this file.
1 /**
2  * @file Vector.hpp
3  *
4  * 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 : public Matrix<Type, M, 1>
21 {
22 public:
24 
25  Vector() = default;
26 
27  Vector(const MatrixM1 & other) :
28  MatrixM1(other)
29  {
30  }
31 
32  explicit Vector(const Type data_[M]) :
33  MatrixM1(data_)
34  {
35  }
36 
37  template<size_t P, size_t Q>
38  Vector(const Slice<Type, M, 1, P, Q>& slice_in) :
39  Matrix<Type, M, 1>(slice_in)
40  {
41  }
42 
43  inline Type operator()(size_t i) const
44  {
45  const MatrixM1 &v = *this;
46  return v(i, 0);
47  }
48 
49  inline Type &operator()(size_t i)
50  {
51  MatrixM1 &v = *this;
52  return v(i, 0);
53  }
54 
55  Type dot(const MatrixM1 & b) const {
56  const Vector &a(*this);
57  Type r(0);
58  for (size_t i = 0; i<M; i++) {
59  r += a(i)*b(i,0);
60  }
61  return r;
62  }
63 
64  inline Type operator*(const MatrixM1 & b) const {
65  const Vector &a(*this);
66  return a.dot(b);
67  }
68 
69  inline Vector operator*(Type b) const {
70  return Vector(MatrixM1::operator*(b));
71  }
72 
73  Type norm() const {
74  const Vector &a(*this);
75  return Type(matrix::sqrt(a.dot(a)));
76  }
77 
78  Type norm_squared() const {
79  const Vector &a(*this);
80  return a.dot(a);
81  }
82 
83  inline Type length() const {
84  return norm();
85  }
86 
87  inline void normalize() {
88  (*this) /= norm();
89  }
90 
91  Vector unit() const {
92  return (*this) / norm();
93  }
94 
95  Vector unit_or_zero(const Type eps = Type(1e-5)) const {
96  const Type n = norm();
97  if (n > eps) {
98  return (*this) / n;
99  }
100  return Vector();
101  }
102 
103  inline Vector normalized() const {
104  return unit();
105  }
106 
107  bool longerThan(Type testVal) const {
108  return norm_squared() > testVal*testVal;
109  }
110 
111  Vector sqrt() const {
112  const Vector &a(*this);
113  Vector r;
114  for (size_t i = 0; i<M; i++) {
115  r(i) = Type(matrix::sqrt(a(i)));
116  }
117  return r;
118  }
119 };
120 
121 } // namespace matrix
122 
123 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
Type operator()(size_t i) const
Definition: Vector.hpp:43
Type operator*(const MatrixM1 &b) const
Definition: Vector.hpp:64
void normalize()
Definition: Vector.hpp:87
Type norm_squared() const
Definition: Vector.hpp:78
Vector normalized() const
Definition: Vector.hpp:103
Vector()=default
Type dot(const MatrixM1 &b) const
Definition: Vector.hpp:55
Vector unit() const
Definition: Vector.hpp:91
Type length() const
Definition: Vector.hpp:83
Vector operator*(Type b) const
Definition: Vector.hpp:69
bool longerThan(Type testVal) const
Definition: Vector.hpp:107
Type & operator()(size_t i)
Definition: Vector.hpp:49
Vector(const MatrixM1 &other)
Definition: Vector.hpp:27
Vector(const Type data_[M])
Definition: Vector.hpp:32
Type norm() const
Definition: Vector.hpp:73
Vector unit_or_zero(const Type eps=Type(1e-5)) const
Definition: Vector.hpp:95
Vector(const Slice< Type, M, 1, P, Q > &slice_in)
Definition: Vector.hpp:38
Vector sqrt() const
Definition: Vector.hpp:111
Dual< Scalar, N > sqrt(const Dual< Scalar, N > &a)
Definition: Dual.hpp:188
Matrix< Type, M, 1 > MatrixM1
Definition: Vector.hpp:23