diff options
author | Roman Bapst <romanbapst@yahoo.de> | 2014-09-27 12:00:15 +0200 |
---|---|---|
committer | Roman Bapst <romanbapst@yahoo.de> | 2014-09-27 12:00:15 +0200 |
commit | 020a8292155cd2821cb66a9709a8c7c18524bdac (patch) | |
tree | 8fa47965c394adfb2bcc74956e472404ed6ed319 /src/lib/mathlib | |
parent | 43056258839c67590dec811e923553fad66a0f4b (diff) | |
download | px4-firmware-020a8292155cd2821cb66a9709a8c7c18524bdac.tar.gz px4-firmware-020a8292155cd2821cb66a9709a8c7c18524bdac.tar.bz2 px4-firmware-020a8292155cd2821cb66a9709a8c7c18524bdac.zip |
Adapted for shared library use with ROS
Diffstat (limited to 'src/lib/mathlib')
-rw-r--r-- | src/lib/mathlib/math/Matrix.hpp | 114 |
1 files changed, 68 insertions, 46 deletions
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 8fb3caa0d..34fc4604a 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -54,10 +54,9 @@ #define M_PI_2_F 1.570769 #endif -namespace math -{ +namespace math { -template <unsigned int M, unsigned int N> +template<unsigned int M, unsigned int N> class __EXPORT Matrix; // MxN matrix with float elements @@ -73,19 +72,19 @@ public: /** * struct for using arm_math functions */ - #ifdef CONFIG_ARCH_ARM +#ifdef CONFIG_ARCH_ARM arm_matrix_instance_f32 arm_mat; - #else +#else eigen_matrix_instance arm_mat; - #endif +#endif /** * trivial ctor * Initializes the elements to zero. */ - MatrixBase() : - data{}, - arm_mat{M, N, &data[0][0]} + MatrixBase() : + data {}, + arm_mat {M, N, &data[0][0]} { } @@ -95,19 +94,19 @@ public: * copyt ctor */ MatrixBase(const MatrixBase<M, N> &m) : - arm_mat{M, N, &data[0][0]} + arm_mat {M, N, &data[0][0]} { memcpy(data, m.data, sizeof(data)); } MatrixBase(const float *d) : - arm_mat{M, N, &data[0][0]} + arm_mat {M, N, &data[0][0]} { memcpy(data, d, sizeof(data)); } - MatrixBase(const float d[M][N]) : - arm_mat{M, N, &data[0][0]} + MatrixBase(const float d[M][N]) : + arm_mat {M, N, &data[0][0]} { memcpy(data, d, sizeof(data)); } @@ -159,9 +158,9 @@ public: */ bool operator ==(const Matrix<M, N> &m) const { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m.data[i][j]) - return false; + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m.data[i][j]) + return false; return true; } @@ -171,9 +170,9 @@ public: */ bool operator !=(const Matrix<M, N> &m) const { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m.data[i][j]) - return true; + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m.data[i][j]) + return true; return false; } @@ -193,8 +192,8 @@ public: Matrix<M, N> res; for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - res.data[i][j] = -data[i][j]; + for (unsigned int j = 0; j < M; j++) + res.data[i][j] = -data[i][j]; return res; } @@ -206,16 +205,16 @@ public: Matrix<M, N> res; for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - res.data[i][j] = data[i][j] + m.data[i][j]; + for (unsigned int j = 0; j < M; j++) + res.data[i][j] = data[i][j] + m.data[i][j]; return res; } Matrix<M, N> &operator +=(const Matrix<M, N> &m) { for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - data[i][j] += m.data[i][j]; + for (unsigned int j = 0; j < M; j++) + data[i][j] += m.data[i][j]; return *static_cast<Matrix<M, N>*>(this); } @@ -227,16 +226,16 @@ public: Matrix<M, N> res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - res.data[i][j] = data[i][j] - m.data[i][j]; + for (unsigned int j = 0; j < N; j++) + res.data[i][j] = data[i][j] - m.data[i][j]; return res; } Matrix<M, N> &operator -=(const Matrix<M, N> &m) { for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - data[i][j] -= m.data[i][j]; + for (unsigned int j = 0; j < M; j++) + data[i][j] -= m.data[i][j]; return *static_cast<Matrix<M, N>*>(this); } @@ -248,16 +247,17 @@ public: Matrix<M, N> res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - res.data[i][j] = data[i][j] * num; + for (unsigned int j = 0; j < N; j++) + res.data[i][j] = data[i][j] * num; return res; + } Matrix<M, N> &operator *=(const float num) { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - data[i][j] *= num; + for (unsigned int j = 0; j < N; j++) + data[i][j] *= num; return *static_cast<Matrix<M, N>*>(this); } @@ -266,16 +266,16 @@ public: Matrix<M, N> res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - res[i][j] = data[i][j] / num; + for (unsigned int j = 0; j < N; j++) + res[i][j] = data[i][j] / num; return res; } Matrix<M, N> &operator /=(const float num) { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - data[i][j] /= num; + for (unsigned int j = 0; j < N; j++) + data[i][j] /= num; return *static_cast<Matrix<M, N>*>(this); } @@ -285,27 +285,49 @@ public: */ template <unsigned int P> Matrix<M, P> operator *(const Matrix<N, P> &m) const { +#ifdef CONFIG_ARCH_ARM Matrix<M, P> res; arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); return res; +#else + Eigen::Matrix<float,M,N,Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float,M,N,Eigen::RowMajor> >(this->arm_mat.pData); + Eigen::Matrix<float,N,P,Eigen::RowMajor> Him = Eigen::Map<Eigen::Matrix<float,N,P,Eigen::RowMajor> >(m.arm_mat.pData); + Eigen::Matrix<float,M,P,Eigen::RowMajor> Product = Me * Him; + Matrix<M, P> res(Product.data()); + return res; +#endif } /** * transpose the matrix */ Matrix<N, M> transposed(void) const { +#ifdef CONFIG_ARCH_ARM Matrix<N, M> res; arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); return res; +#else + Eigen::Matrix<float,N,M,Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float,N,M,Eigen::RowMajor> >(this->arm_mat.pData); + Me.transposeInPlace(); + Matrix<N, M> res(Me.data()); + return res; +#endif } /** * invert the matrix */ Matrix<M, N> inversed(void) const { +#ifdef CONFIG_ARCH_ARM Matrix<M, N> res; arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); return res; +#else + Eigen::Matrix<float,M,N,Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float,M,N,Eigen::RowMajor> >(this->arm_mat.pData); + Eigen::Matrix<float,M,N,Eigen::RowMajor> MyInverse = Me.inverse();//not sure if A = A.inverse() is a good idea + Matrix<M, N> res(MyInverse.data()); + return res; +#endif } /** @@ -323,7 +345,7 @@ public: unsigned int n = (M < N) ? M : N; for (unsigned int i = 0; i < n; i++) - data[i][i] = 1; + data[i][i] = 1; } void print(void) { @@ -331,7 +353,7 @@ public: printf("[ "); for (unsigned int j = 0; j < N; j++) - printf("%.3f\t", data[i][j]); + printf("%.3f\t", data[i][j]); printf(" ]\n"); } @@ -364,16 +386,16 @@ public: * multiplication by a vector */ Vector<M> operator *(const Vector<N> &v) const { - #ifdef CONFIG_ARCH_ARM +#ifdef CONFIG_ARCH_ARM Vector<M> res; arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); - #else +#else //probably nicer if this could go into a function like "eigen_mat_mult" or so - Eigen::Matrix<float,M,N,Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float,M,N,Eigen::RowMajor> >(this->arm_mat.pData); - Eigen::VectorXf Vec = Eigen::Map<Eigen::VectorXf>(v.arm_col.pData,N); + Eigen::Matrix<float,M,N,Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float,M,N,Eigen::RowMajor> >(this->arm_mat.pData); + Eigen::VectorXf Vec = Eigen::Map<Eigen::VectorXf>(v.arm_col.pData,N); Eigen::VectorXf Product = Me * Vec; Vector<M> res(Product.data()); - #endif +#endif return res; } }; @@ -405,8 +427,8 @@ public: */ Vector<3> operator *(const Vector<3> &v) const { Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2], - data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], - data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); + data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], + data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); return res; } |