diff options
Diffstat (limited to 'src/lib/mathlib/math/Vector.hpp')
| -rw-r--r-- | src/lib/mathlib/math/Vector.hpp | 214 |
1 files changed, 197 insertions, 17 deletions
diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 73de793d5..adb2293e1 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -1,6 +1,8 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone <will.perone@gmail.com> + * Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,26 +34,204 @@ ****************************************************************************/ /** - * @file Vector.h + * @file Vector.hpp * - * math vector + * Generic Vector */ -#pragma once +#ifndef VECTOR_HPP +#define VECTOR_HPP -#include <nuttx/config.h> - -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) -#include "arm/Vector.hpp" -#else -#include "generic/Vector.hpp" -#endif +#include <math.h> +#include "../CMSIS/Include/arm_math.h" namespace math { -class Vector; -int __EXPORT vectorTest(); -int __EXPORT vectorAddTest(); -int __EXPORT vectorSubTest(); -bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f); -} // math + +template <unsigned int N> +class Vector { +public: + float data[N]; + arm_matrix_instance_f32 arm_col; + + /** + * trivial ctor + */ + Vector<N>() { + arm_col = {N, 1, &data[0]}; + } + + /** + * setting ctor + */ + Vector<N>(const float *d) { + memcpy(data, d, sizeof(data)); + arm_col = {N, 1, &data[0]}; + } + + /** + * access to elements by index + */ + inline float &operator ()(unsigned int i) { + return data[i]; + } + + /** + * access to elements by index + */ + inline const float &operator ()(unsigned int i) const { + return data[i]; + } + + /** + * test for equality + */ + bool operator ==(const Vector<N> &v) { + for (unsigned int i = 0; i < N; i++) + if (data[i] != v(i)) + return false; + return true; + } + + /** + * test for inequality + */ + bool operator !=(const Vector<N> &v) { + for (unsigned int i = 0; i < N; i++) + if (data[i] != v(i)) + return true; + return false; + } + + /** + * set to value + */ + const Vector<N> &operator =(const Vector<N> &v) { + memcpy(data, v.data, sizeof(data)); + return *this; + } + + /** + * negation + */ + const Vector<N> operator -(void) const { + Vector<N> res; + for (unsigned int i = 0; i < N; i++) + res[i] = -data[i]; + return res; + } + + /** + * addition + */ + const Vector<N> operator +(const Vector<N> &v) const { + Vector<N> res; + for (unsigned int i = 0; i < N; i++) + res[i] = data[i] + v(i); + return res; + } + + /** + * subtraction + */ + const Vector<N> operator -(const Vector<N> &v) const { + Vector<N> res; + for (unsigned int i = 0; i < N; i++) + res[i] = data[i] - v(i); + return res; + } + + /** + * uniform scaling + */ + const Vector<N> operator *(const float num) const { + Vector<N> temp(*this); + return temp *= num; + } + + /** + * uniform scaling + */ + const Vector<N> operator /(const float num) const { + Vector<N> temp(*this); + return temp /= num; + } + + /** + * addition + */ + const Vector<N> &operator +=(const Vector<N> &v) { + for (unsigned int i = 0; i < N; i++) + data[i] += v(i); + return *this; + } + + /** + * subtraction + */ + const Vector<N> &operator -=(const Vector<N> &v) { + for (unsigned int i = 0; i < N; i++) + data[i] -= v(i); + return *this; + } + + /** + * uniform scaling + */ + const Vector<N> &operator *=(const float num) { + for (unsigned int i = 0; i < N; i++) + data[i] *= num; + return *this; + } + + /** + * uniform scaling + */ + const Vector<N> &operator /=(const float num) { + for (unsigned int i = 0; i < N; i++) + data[i] /= num; + return *this; + } + + /** + * dot product + */ + float operator *(const Vector<N> &v) const { + float res; + for (unsigned int i = 0; i < N; i++) + res += data[i] * v(i); + return res; + } + + /** + * gets the length of this vector squared + */ + float length_squared() const { + return (*this * *this); + } + + /** + * gets the length of this vector + */ + float length() const { + return sqrtf(*this * *this); + } + + /** + * normalizes this vector + */ + void normalize() { + *this /= length(); + } + + /** + * returns the normalized version of this vector + */ + Vector<N> normalized() const { + return *this / length(); + } +}; + +} + +#endif // VECTOR_HPP |