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