Adafruit driver converted to Mbed OS 6.x.
Dependents: Adafruit-BNO055-test
vector.h
- Committer:
- simonscott
- Date:
- 2015-09-16
- Revision:
- 0:22c544c8741a
File content as of revision 0:22c544c8741a:
/* Inertial Measurement Unit Maths Library Copyright (C) 2013-2014 Samuel Cowen www.camelsoftware.com This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <http://www.gnu.org/licenses/>. */ #ifndef IMUMATH_VECTOR_HPP #define IMUMATH_VECTOR_HPP #include <stdlib.h> #include <string.h> #include <stdint.h> #include <math.h> namespace imu { template <uint8_t N> class Vector { public: Vector() { memset(p_vec, 0, sizeof(double)*N); } Vector(double a) { memset(p_vec, 0, sizeof(double)*N); p_vec[0] = a; } Vector(double a, double b) { memset(p_vec, 0, sizeof(double)*N); p_vec[0] = a; p_vec[1] = b; } Vector(double a, double b, double c) { memset(p_vec, 0, sizeof(double)*N); p_vec[0] = a; p_vec[1] = b; p_vec[2] = c; } Vector(double a, double b, double c, double d) { memset(p_vec, 0, sizeof(double)*N); p_vec[0] = a; p_vec[1] = b; p_vec[2] = c; p_vec[3] = d; } Vector(const Vector<N> &v) { for (int x = 0; x < N; x++) p_vec[x] = v.p_vec[x]; } ~Vector() { } uint8_t n() { return N; } double magnitude() { double res = 0; int i; for(i = 0; i < N; i++) res += (p_vec[i] * p_vec[i]); if(isnan(res)) return 0; if((fabs(res-1)) >= 0.000001) // Avoid a sqrt if possible. return sqrt(res); return 1; } void normalize() { double mag = magnitude(); if(abs(mag) <= 0.0001) return; int i; for(i = 0; i < N; i++) p_vec[i] = p_vec[i]/mag; } double dot(Vector v) { double ret = 0; int i; for(i = 0; i < N; i++) ret += p_vec[i] * v.p_vec[i]; return ret; } Vector cross(Vector v) { Vector ret; // The cross product is only valid for vectors with 3 dimensions, // with the exception of higher dimensional stuff that is beyond the intended scope of this library if(N != 3) return ret; ret.p_vec[0] = (p_vec[1] * v.p_vec[2]) - (p_vec[2] * v.p_vec[1]); ret.p_vec[1] = (p_vec[2] * v.p_vec[0]) - (p_vec[0] * v.p_vec[2]); ret.p_vec[2] = (p_vec[0] * v.p_vec[1]) - (p_vec[1] * v.p_vec[0]); return ret; } Vector scale(double scalar) const { Vector ret; for(int i = 0; i < N; i++) ret.p_vec[i] = p_vec[i] * scalar; return ret; } Vector invert() const { Vector ret; for(int i = 0; i < N; i++) ret.p_vec[i] = -p_vec[i]; return ret; } Vector operator = (Vector v) { for (int x = 0; x < N; x++ ) p_vec[x] = v.p_vec[x]; return *this; } double& operator [](int n) { return p_vec[n]; } double operator [](int n) const { return p_vec[n]; } double& operator ()(int n) { return p_vec[n]; } double operator ()(int n) const { return p_vec[n]; } Vector operator + (Vector v) const { Vector ret; for(int i = 0; i < N; i++) ret.p_vec[i] = p_vec[i] + v.p_vec[i]; return ret; } Vector operator - (Vector v) const { Vector ret; for(int i = 0; i < N; i++) ret.p_vec[i] = p_vec[i] - v.p_vec[i]; return ret; } Vector operator * (double scalar) const { return scale(scalar); } Vector operator / (double scalar) const { Vector ret; for(int i = 0; i < N; i++) ret.p_vec[i] = p_vec[i] / scalar; return ret; } void toDegrees() { for(int i = 0; i < N; i++) p_vec[i] *= 57.2957795131; //180/pi } void toRadians() { for(int i = 0; i < N; i++) p_vec[i] *= 0.01745329251; //pi/180 } double& x() { return p_vec[0]; } double& y() { return p_vec[1]; } double& z() { return p_vec[2]; } double x() const { return p_vec[0]; } double y() const { return p_vec[1]; } double z() const { return p_vec[2]; } private: double p_vec[N]; }; }; #endif