Toyomasa Watarai / Adafruit_BNO055

Adafruit driver converted to Mbed OS 6.x.

Dependents:   Adafruit-BNO055-test

vector.h@0:22c544c8741a, 2015-09-16 (annotated)

Committer:
simonscott
Date:
Wed Sep 16 19:48:33 2015 +0000
Revision:
0:22c544c8741a
First version

Who changed what in which revision?

UserRevisionLine numberNew 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