diff options
Diffstat (limited to 'include/math/vector.h')
| -rw-r--r-- | include/math/vector.h | 184 |
1 files changed, 0 insertions, 184 deletions
diff --git a/include/math/vector.h b/include/math/vector.h deleted file mode 100644 index 10345c3..0000000 --- a/include/math/vector.h +++ /dev/null @@ -1,184 +0,0 @@ -// Inertial Measurement Unit Maths Library -// -// Copyright 2013-2021 Sam Cowen <[email protected]> -// Bug fixes and cleanups by Gé Vissers ([email protected]) -// -// Permission is hereby granted, free of charge, to any person obtaining a -// copy of this software and associated documentation files (the "Software"), -// to deal in the Software without restriction, including without limitation -// the rights to use, copy, modify, merge, publish, distribute, sublicense, -// and/or sell copies of the Software, and to permit persons to whom the -// Software is furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS -// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER -// DEALINGS IN THE SOFTWARE. - -#ifndef IMUMATH_VECTOR_HPP -#define IMUMATH_VECTOR_HPP - -#include <math.h> -#include <stdint.h> -#include <string.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() const { - double res = 0; - for (int i = 0; i < N; i++) - res += p_vec[i] * p_vec[i]; - - return sqrt(res); - } - - void normalize() { - double mag = magnitude(); - if (isnan(mag) || mag == 0.0) - return; - - for (int i = 0; i < N; i++) - p_vec[i] /= mag; - } - - double dot(const Vector &v) const { - double ret = 0; - for (int i = 0; i < N; i++) - ret += p_vec[i] * v.p_vec[i]; - - return 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. - // Only a definition for N==3 is given below this class, using - // cross() with another value for N will result in a link error. - Vector cross(const Vector &v) const; - - 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=(const 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+(const 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-(const 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]; -}; - -template <> inline Vector<3> Vector<3>::cross(const Vector &v) const { - return Vector(p_vec[1] * v.p_vec[2] - p_vec[2] * v.p_vec[1], - p_vec[2] * v.p_vec[0] - p_vec[0] * v.p_vec[2], - p_vec[0] * v.p_vec[1] - p_vec[1] * v.p_vec[0]); -} - -} // namespace imu - -#endif
\ No newline at end of file |