aboutsummaryrefslogtreecommitdiff
path: root/src/lib
diff options
context:
space:
mode:
authorRoman Bapst <romanbapst@yahoo.de>2014-11-13 11:16:22 +0100
committerRoman Bapst <romanbapst@yahoo.de>2014-11-13 11:16:22 +0100
commit003b326c7731b0f3b9b139ae008d6e4d07cefab8 (patch)
tree7ce7cf0102fccd8a7e56ab2da8411523527d3902 /src/lib
parent794a89e711a90a62510f6bdde8ee59511237f6ca (diff)
downloadpx4-firmware-003b326c7731b0f3b9b139ae008d6e4d07cefab8.tar.gz
px4-firmware-003b326c7731b0f3b9b139ae008d6e4d07cefab8.tar.bz2
px4-firmware-003b326c7731b0f3b9b139ae008d6e4d07cefab8.zip
applied fix_code_style.sh
Diffstat (limited to 'src/lib')
-rw-r--r--src/lib/mathlib/math/Matrix.hpp113
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;
}