adafruit_bno055 / utility / vector.h @ 3e12eaa8
History | View | Annotate | Download (4.802 KB)
1 | 4bc1c0c1 | Kevin Townsend | /*
|
---|---|---|---|
2 | Inertial Measurement Unit Maths Library
|
||
3 | Copyright (C) 2013-2014 Samuel Cowen
|
||
4 | d964148c | Tony DiCola | www.camelsoftware.com
|
5 | 4bc1c0c1 | Kevin Townsend | |
6 | This program is free software: you can redistribute it and/or modify
|
||
7 | it under the terms of the GNU General Public License as published by
|
||
8 | the Free Software Foundation, either version 3 of the License, or
|
||
9 | (at your option) any later version.
|
||
10 | |||
11 | This program is distributed in the hope that it will be useful,
|
||
12 | but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||
14 | GNU General Public License for more details.
|
||
15 | |||
16 | You should have received a copy of the GNU General Public License
|
||
17 | along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||
18 | */
|
||
19 | |||
20 | #ifndef IMUMATH_VECTOR_HPP
|
||
21 | #define IMUMATH_VECTOR_HPP
|
||
22 | |||
23 | #include <string.h> |
||
24 | #include <stdint.h> |
||
25 | #include <math.h> |
||
26 | |||
27 | |||
28 | namespace imu |
||
29 | { |
||
30 | |||
31 | template <uint8_t N> class Vector |
||
32 | { |
||
33 | public:
|
||
34 | d964148c | Tony DiCola | Vector() |
35 | { |
||
36 | 4bc1c0c1 | Kevin Townsend | memset(p_vec, 0, sizeof(double)*N); |
37 | d964148c | Tony DiCola | } |
38 | 4bc1c0c1 | Kevin Townsend | |
39 | d964148c | Tony DiCola | Vector(double a)
|
40 | { |
||
41 | 4bc1c0c1 | Kevin Townsend | memset(p_vec, 0, sizeof(double)*N); |
42 | d964148c | Tony DiCola | p_vec[0] = a;
|
43 | } |
||
44 | 4bc1c0c1 | Kevin Townsend | |
45 | d964148c | Tony DiCola | Vector(double a, double b) |
46 | { |
||
47 | 4bc1c0c1 | Kevin Townsend | memset(p_vec, 0, sizeof(double)*N); |
48 | d964148c | Tony DiCola | p_vec[0] = a;
|
49 | p_vec[1] = b;
|
||
50 | } |
||
51 | 4bc1c0c1 | Kevin Townsend | |
52 | d964148c | Tony DiCola | Vector(double a, double b, double c) |
53 | { |
||
54 | 4bc1c0c1 | Kevin Townsend | memset(p_vec, 0, sizeof(double)*N); |
55 | d964148c | Tony DiCola | p_vec[0] = a;
|
56 | p_vec[1] = b;
|
||
57 | p_vec[2] = c;
|
||
58 | } |
||
59 | 4bc1c0c1 | Kevin Townsend | |
60 | Vector(double a, double b, double c, double d) |
||
61 | { |
||
62 | memset(p_vec, 0, sizeof(double)*N); |
||
63 | p_vec[0] = a;
|
||
64 | d964148c | Tony DiCola | p_vec[1] = b;
|
65 | p_vec[2] = c;
|
||
66 | p_vec[3] = d;
|
||
67 | 4bc1c0c1 | Kevin Townsend | } |
68 | |||
69 | Vector(const Vector<N> &v)
|
||
70 | { |
||
71 | b2d499c7 | Paul Du Bois (laptop) | for (int x = 0; x < N; x++) |
72 | 4bc1c0c1 | Kevin Townsend | p_vec[x] = v.p_vec[x]; |
73 | } |
||
74 | |||
75 | ~Vector() |
||
76 | { |
||
77 | } |
||
78 | |||
79 | uint8_t n() { return N; }
|
||
80 | |||
81 | d7b28532 | Gé Vissers | double magnitude() const |
82 | 4bc1c0c1 | Kevin Townsend | { |
83 | double res = 0; |
||
84 | 651c5f56 | Gé Vissers | for (int i = 0; i < N; i++) |
85 | res += p_vec[i] * p_vec[i]; |
||
86 | 4bc1c0c1 | Kevin Townsend | |
87 | if(isnan(res))
|
||
88 | return 0; |
||
89 | 3e12eaa8 | Gé Vissers | return sqrt(res);
|
90 | 4bc1c0c1 | Kevin Townsend | } |
91 | |||
92 | void normalize()
|
||
93 | { |
||
94 | double mag = magnitude();
|
||
95 | a61c970f | Gé Vissers | if (fabs(mag) <= 0.0001) |
96 | 4bc1c0c1 | Kevin Townsend | return;
|
97 | |||
98 | int i;
|
||
99 | for(i = 0; i < N; i++) |
||
100 | p_vec[i] = p_vec[i]/mag; |
||
101 | } |
||
102 | |||
103 | d7b28532 | Gé Vissers | double dot(const Vector& v) const |
104 | 4bc1c0c1 | Kevin Townsend | { |
105 | double ret = 0; |
||
106 | 651c5f56 | Gé Vissers | for (int i = 0; i < N; i++) |
107 | 4bc1c0c1 | Kevin Townsend | ret += p_vec[i] * v.p_vec[i]; |
108 | |||
109 | return ret;
|
||
110 | } |
||
111 | |||
112 | fd9de024 | Gé Vissers | // The cross product is only valid for vectors with 3 dimensions,
|
113 | // with the exception of higher dimensional stuff that is beyond
|
||
114 | // the intended scope of this library.
|
||
115 | // Only a definition for N==3 is given below this class, using
|
||
116 | // cross() with another value for N will result in a link error.
|
||
117 | Vector cross(const Vector& v) const; |
||
118 | 4bc1c0c1 | Kevin Townsend | |
119 | 0695bf91 | Paul Du Bois (laptop) | Vector scale(double scalar) const |
120 | 4bc1c0c1 | Kevin Townsend | { |
121 | Vector ret; |
||
122 | for(int i = 0; i < N; i++) |
||
123 | ret.p_vec[i] = p_vec[i] * scalar; |
||
124 | return ret;
|
||
125 | } |
||
126 | |||
127 | 0695bf91 | Paul Du Bois (laptop) | Vector invert() const
|
128 | 4bc1c0c1 | Kevin Townsend | { |
129 | Vector ret; |
||
130 | for(int i = 0; i < N; i++) |
||
131 | ret.p_vec[i] = -p_vec[i]; |
||
132 | return ret;
|
||
133 | } |
||
134 | |||
135 | d7b28532 | Gé Vissers | Vector& operator=(const Vector& v)
|
136 | 4bc1c0c1 | Kevin Townsend | { |
137 | for (int x = 0; x < N; x++ ) |
||
138 | p_vec[x] = v.p_vec[x]; |
||
139 | d964148c | Tony DiCola | return *this;
|
140 | 4bc1c0c1 | Kevin Townsend | } |
141 | |||
142 | double& operator [](int n) |
||
143 | { |
||
144 | return p_vec[n];
|
||
145 | } |
||
146 | |||
147 | 0695bf91 | Paul Du Bois (laptop) | double operator [](int n) const |
148 | { |
||
149 | return p_vec[n];
|
||
150 | } |
||
151 | |||
152 | 4bc1c0c1 | Kevin Townsend | double& operator ()(int n) |
153 | { |
||
154 | return p_vec[n];
|
||
155 | } |
||
156 | |||
157 | 0695bf91 | Paul Du Bois (laptop) | double operator ()(int n) const |
158 | { |
||
159 | return p_vec[n];
|
||
160 | } |
||
161 | |||
162 | d7b28532 | Gé Vissers | Vector operator+(const Vector& v) const |
163 | 4bc1c0c1 | Kevin Townsend | { |
164 | Vector ret; |
||
165 | for(int i = 0; i < N; i++) |
||
166 | ret.p_vec[i] = p_vec[i] + v.p_vec[i]; |
||
167 | return ret;
|
||
168 | } |
||
169 | |||
170 | d7b28532 | Gé Vissers | Vector operator-(const Vector& v) const |
171 | 4bc1c0c1 | Kevin Townsend | { |
172 | Vector ret; |
||
173 | for(int i = 0; i < N; i++) |
||
174 | ret.p_vec[i] = p_vec[i] - v.p_vec[i]; |
||
175 | return ret;
|
||
176 | } |
||
177 | |||
178 | 0695bf91 | Paul Du Bois (laptop) | Vector operator * (double scalar) const |
179 | 4bc1c0c1 | Kevin Townsend | { |
180 | return scale(scalar);
|
||
181 | } |
||
182 | |||
183 | 0695bf91 | Paul Du Bois (laptop) | Vector operator / (double scalar) const |
184 | 4bc1c0c1 | Kevin Townsend | { |
185 | Vector ret; |
||
186 | for(int i = 0; i < N; i++) |
||
187 | ret.p_vec[i] = p_vec[i] / scalar; |
||
188 | return ret;
|
||
189 | } |
||
190 | |||
191 | void toDegrees()
|
||
192 | { |
||
193 | for(int i = 0; i < N; i++) |
||
194 | p_vec[i] *= 57.2957795131; //180/pi |
||
195 | } |
||
196 | |||
197 | void toRadians()
|
||
198 | { |
||
199 | for(int i = 0; i < N; i++) |
||
200 | p_vec[i] *= 0.01745329251; //pi/180 |
||
201 | } |
||
202 | |||
203 | double& x() { return p_vec[0]; } |
||
204 | double& y() { return p_vec[1]; } |
||
205 | double& z() { return p_vec[2]; } |
||
206 | 0695bf91 | Paul Du Bois (laptop) | double x() const { return p_vec[0]; } |
207 | double y() const { return p_vec[1]; } |
||
208 | double z() const { return p_vec[2]; } |
||
209 | 4bc1c0c1 | Kevin Townsend | |
210 | |||
211 | private:
|
||
212 | 651c5f56 | Gé Vissers | double p_vec[N];
|
213 | 4bc1c0c1 | Kevin Townsend | }; |
214 | |||
215 | |||
216 | fd9de024 | Gé Vissers | template <> |
217 | inline Vector<3> Vector<3>::cross(const Vector& v) const |
||
218 | { |
||
219 | return Vector(
|
||
220 | p_vec[1] * v.p_vec[2] - p_vec[2] * v.p_vec[1], |
||
221 | p_vec[2] * v.p_vec[0] - p_vec[0] * v.p_vec[2], |
||
222 | p_vec[0] * v.p_vec[1] - p_vec[1] * v.p_vec[0] |
||
223 | ); |
||
224 | } |
||
225 | |||
226 | } // namespace
|
||
227 | 4bc1c0c1 | Kevin Townsend | |
228 | #endif |