diff options
author | Roman Bapst <romanbapst@yahoo.de> | 2014-11-13 11:16:22 +0100 |
---|---|---|
committer | Roman Bapst <romanbapst@yahoo.de> | 2014-11-13 11:16:22 +0100 |
commit | 003b326c7731b0f3b9b139ae008d6e4d07cefab8 (patch) | |
tree | 7ce7cf0102fccd8a7e56ab2da8411523527d3902 /src/lib/mathlib/math | |
parent | 794a89e711a90a62510f6bdde8ee59511237f6ca (diff) | |
download | px4-firmware-003b326c7731b0f3b9b139ae008d6e4d07cefab8.tar.gz px4-firmware-003b326c7731b0f3b9b139ae008d6e4d07cefab8.tar.bz2 px4-firmware-003b326c7731b0f3b9b139ae008d6e4d07cefab8.zip |
applied fix_code_style.sh
Diffstat (limited to 'src/lib/mathlib/math')
-rw-r--r-- | src/lib/mathlib/math/Matrix.hpp | 113 |
1 files changed, 64 insertions, 49 deletions
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 9fc3c78c8..806f5933a 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -51,10 +51,11 @@ #else #include <math/eigen_math.h> #include <Eigen/Eigen> -#define M_PI_2_F 1.5707963267948966192 +#define M_PI_2_F 1.5707963267948966192f #endif -namespace math { +namespace math +{ template<unsigned int M, unsigned int N> class __EXPORT Matrix; @@ -83,9 +84,8 @@ public: * Initializes the elements to zero. */ MatrixBase() : - data {}, - arm_mat {M, N, &data[0][0]} - { + data {}, + arm_mat {M, N, &data[0][0]} { } virtual ~MatrixBase() {}; @@ -94,20 +94,17 @@ 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]} - { + arm_mat {M, N, &data[0][0]} { memcpy(data, d, sizeof(data)); } @@ -158,9 +155,10 @@ 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; } @@ -170,9 +168,10 @@ 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; } @@ -192,8 +191,9 @@ 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; } @@ -205,16 +205,18 @@ 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); } @@ -226,16 +228,18 @@ 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); } @@ -247,8 +251,9 @@ 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; @@ -256,8 +261,9 @@ public: 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 +272,18 @@ 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); } @@ -290,9 +298,11 @@ public: 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; + 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 @@ -307,7 +317,8 @@ public: 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); + 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; @@ -323,8 +334,9 @@ public: 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 + 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 @@ -344,16 +356,18 @@ public: memset(data, 0, sizeof(data)); unsigned int n = (M < N) ? M : N; - for (unsigned int i = 0; i < n; i++) - data[i][i] = 1; + for (unsigned int i = 0; i < n; i++) { + data[i][i] = 1; + } } void print(void) { for (unsigned int i = 0; i < M; i++) { printf("[ "); - for (unsigned int j = 0; j < N; j++) - printf("%.3f\t", data[i][j]); + for (unsigned int j = 0; j < N; j++) { + printf("%.3f\t", data[i][j]); + } printf(" ]\n"); } @@ -391,8 +405,9 @@ public: arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); #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 @@ -427,8 +442,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; } |