From 020a8292155cd2821cb66a9709a8c7c18524bdac Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Sat, 27 Sep 2014 12:00:15 +0200 Subject: Adapted for shared library use with ROS --- src/lib/mathlib/math/Matrix.hpp | 114 ++++++++++++++++++++++++---------------- 1 file changed, 68 insertions(+), 46 deletions(-) (limited to 'src') 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 +template 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) : - 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) 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) 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 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 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 &operator +=(const Matrix &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*>(this); } @@ -227,16 +226,16 @@ public: Matrix 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 &operator -=(const Matrix &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*>(this); } @@ -248,16 +247,17 @@ public: Matrix 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 &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*>(this); } @@ -266,16 +266,16 @@ public: Matrix 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 &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*>(this); } @@ -285,27 +285,49 @@ public: */ template Matrix operator *(const Matrix &m) const { +#ifdef CONFIG_ARCH_ARM Matrix res; arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); return res; +#else + Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); + Eigen::Matrix Him = Eigen::Map >(m.arm_mat.pData); + Eigen::Matrix Product = Me * Him; + Matrix res(Product.data()); + return res; +#endif } /** * transpose the matrix */ Matrix transposed(void) const { +#ifdef CONFIG_ARCH_ARM Matrix res; arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); return res; +#else + Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); + Me.transposeInPlace(); + Matrix res(Me.data()); + return res; +#endif } /** * invert the matrix */ Matrix inversed(void) const { +#ifdef CONFIG_ARCH_ARM Matrix res; arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); return res; +#else + Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); + Eigen::Matrix MyInverse = Me.inverse();//not sure if A = A.inverse() is a good idea + Matrix 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 operator *(const Vector &v) const { - #ifdef CONFIG_ARCH_ARM +#ifdef CONFIG_ARCH_ARM Vector 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 Me = Eigen::Map >(this->arm_mat.pData); - Eigen::VectorXf Vec = Eigen::Map(v.arm_col.pData,N); + Eigen::Matrix Me = Eigen::Map >(this->arm_mat.pData); + Eigen::VectorXf Vec = Eigen::Map(v.arm_col.pData,N); Eigen::VectorXf Product = Me * Vec; Vector 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; } -- cgit v1.2.3