fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 21:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

mathlib: code style fixed

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-25 13:04:52 +04:00
parent 8ed193d115
commit bbeb97df67
3 changed files with 175 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mathlib/math/Matrix.hpp
+78 -55
View File
@@ -54,7 +54,8 @@ class __EXPORT Matrix;
// MxN matrix with float elements // MxN matrix with float elements
template <unsigned int M, unsigned int N> template <unsigned int M, unsigned int N>
class __EXPORT MatrixBase { class __EXPORT MatrixBase
{
public: public:
/** /**
* matrix data[row][col] * matrix data[row][col]
@@ -90,14 +91,14 @@ public:
/** /**
* access by index * access by index
*/ */
float &operator ()(const unsigned int row, const unsigned int col) { float &operator()(const unsigned int row, const unsigned int col) {
return data[row][col]; return data[row][col];
} }
/** /**
* access by index * access by index
*/ */
float operator ()(const unsigned int row, const unsigned int col) const { float operator()(const unsigned int row, const unsigned int col) const {
return data[row][col]; return data[row][col];
} }
@@ -119,10 +120,11 @@ public:
* test for equality * test for equality
*/ */
bool operator ==(const Matrix<M, N> &m) const { bool operator ==(const Matrix<M, N> &m) const {
for (unsigned int i = 0; i < M; i++) for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++) for (unsigned int j = 0; j < N; j++)
if (data[i][j] != m.data[i][j]) if (data[i][j] != m.data[i][j])
return false; return false;
return true; return true;
} }
@@ -130,10 +132,11 @@ public:
* test for inequality * test for inequality
*/ */
bool operator !=(const Matrix<M, N> &m) const { bool operator !=(const Matrix<M, N> &m) const {
for (unsigned int i = 0; i < M; i++) for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++) for (unsigned int j = 0; j < N; j++)
if (data[i][j] != m.data[i][j]) if (data[i][j] != m.data[i][j])
return true; return true;
return false; return false;
} }
@@ -150,9 +153,11 @@ public:
*/ */
Matrix<M, N> operator -(void) const { Matrix<M, N> operator -(void) const {
Matrix<M, N> res; Matrix<M, N> res;
for (unsigned int i = 0; i < N; i++)
for (unsigned int j = 0; j < M; j++) for (unsigned int i = 0; i < N; i++)
res.data[i][j] = -data[i][j]; for (unsigned int j = 0; j < M; j++)
res.data[i][j] = -data[i][j];
return res; return res;
} }
@@ -161,16 +166,19 @@ public:
*/ */
Matrix<M, N> operator +(const Matrix<M, N> &m) const { Matrix<M, N> operator +(const Matrix<M, N> &m) const {
Matrix<M, N> res; Matrix<M, N> res;
for (unsigned int i = 0; i < N; i++)
for (unsigned int j = 0; j < M; j++) for (unsigned int i = 0; i < N; i++)
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; return res;
} }
Matrix<M, N> &operator +=(const Matrix<M, N> &m) { Matrix<M, N> &operator +=(const Matrix<M, N> &m) {
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++)
for (unsigned int j = 0; j < M; j++) for (unsigned int j = 0; j < M; j++)
data[i][j] += m.data[i][j]; data[i][j] += m.data[i][j];
return *static_cast<Matrix<M, N>*>(this); return *static_cast<Matrix<M, N>*>(this);
} }
@@ -179,16 +187,19 @@ public:
*/ */
Matrix<M, N> operator -(const Matrix<M, N> &m) const { Matrix<M, N> operator -(const Matrix<M, N> &m) const {
Matrix<M, N> res; Matrix<M, N> res;
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++) for (unsigned int i = 0; i < M; i++)
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; return res;
} }
Matrix<M, N> &operator -=(const Matrix<M, N> &m) { Matrix<M, N> &operator -=(const Matrix<M, N> &m) {
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++)
for (unsigned int j = 0; j < M; j++) for (unsigned int j = 0; j < M; j++)
data[i][j] -= m.data[i][j]; data[i][j] -= m.data[i][j];
return *static_cast<Matrix<M, N>*>(this); return *static_cast<Matrix<M, N>*>(this);
} }
@@ -197,31 +208,37 @@ public:
*/ */
Matrix<M, N> operator *(const float num) const { Matrix<M, N> operator *(const float num) const {
Matrix<M, N> res; Matrix<M, N> res;
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++) for (unsigned int i = 0; i < M; i++)
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; return res;
} }
Matrix<M, N> &operator *=(const float num) { Matrix<M, N> &operator *=(const float num) {
for (unsigned int i = 0; i < M; i++) for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++) for (unsigned int j = 0; j < N; j++)
data[i][j] *= num; data[i][j] *= num;
return *static_cast<Matrix<M, N>*>(this); return *static_cast<Matrix<M, N>*>(this);
} }
Matrix<M, N> operator /(const float num) const { Matrix<M, N> operator /(const float num) const {
Matrix<M, N> res; Matrix<M, N> res;
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++) for (unsigned int i = 0; i < M; i++)
res[i][j] = data[i][j] / num; for (unsigned int j = 0; j < N; j++)
res[i][j] = data[i][j] / num;
return res; return res;
} }
Matrix<M, N> &operator /=(const float num) { Matrix<M, N> &operator /=(const float num) {
for (unsigned int i = 0; i < M; i++) for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++) for (unsigned int j = 0; j < N; j++)
data[i][j] /= num; data[i][j] /= num;
return *static_cast<Matrix<M, N>*>(this); return *static_cast<Matrix<M, N>*>(this);
} }
@@ -239,18 +256,18 @@ public:
* transpose the matrix * transpose the matrix
*/ */
Matrix<N, M> transposed(void) const { Matrix<N, M> transposed(void) const {
Matrix<N, M> res; Matrix<N, M> res;
arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); arm_mat_trans_f32(&this->arm_mat, &res.arm_mat);
return res; return res;
} }
/** /**
* invert the matrix * invert the matrix
*/ */
Matrix<M, N> inversed(void) const { Matrix<M, N> inversed(void) const {
Matrix<M, N> res; Matrix<M, N> res;
arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
return res; return res;
} }
/** /**
@@ -266,22 +283,26 @@ public:
void identity(void) { void identity(void) {
memset(data, 0, sizeof(data)); memset(data, 0, sizeof(data));
unsigned int n = (M < N) ? M : N; unsigned int n = (M < N) ? M : N;
for (unsigned int i = 0; i < n; i++) for (unsigned int i = 0; i < n; i++)
data[i][i] = 1; data[i][i] = 1;
} }
void print(void) { void print(void) {
for (unsigned int i = 0; i < M; i++) { for (unsigned int i = 0; i < M; i++) {
printf("[ "); printf("[ ");
for (unsigned int j = 0; j < N; j++)
for (unsigned int j = 0; j < N; j++)
printf("%.3f\t", data[i][j]); printf("%.3f\t", data[i][j]);
printf(" ]\n"); printf(" ]\n");
} }
} }
}; };
template <unsigned int M, unsigned int N> template <unsigned int M, unsigned int N>
class __EXPORT Matrix : public MatrixBase<M, N> { class __EXPORT Matrix : public MatrixBase<M, N>
{
public: public:
using MatrixBase<M, N>::operator *; using MatrixBase<M, N>::operator *;
@@ -303,14 +324,15 @@ public:
* multiplication by a vector * multiplication by a vector
*/ */
Vector<M> operator *(const Vector<N> &v) const { Vector<M> operator *(const Vector<N> &v) const {
Vector<M> res; Vector<M> res;
arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
return res; return res;
} }
}; };
template <> template <>
class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> { class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3>
{
public: public:
using MatrixBase<3, 3>::operator *; using MatrixBase<3, 3>::operator *;
@@ -332,10 +354,10 @@ public:
* multiplication by a vector * multiplication by a vector
*/ */
Vector<3> operator *(const Vector<3> &v) const { 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], 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[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[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]);
return res; return res;
} }
/** /**
@@ -380,6 +402,7 @@ public:
euler.data[0] = atan2f(data[2][1], data[2][2]); euler.data[0] = atan2f(data[2][1], data[2][2]);
euler.data[2] = atan2f(data[1][0], data[0][0]); euler.data[2] = atan2f(data[1][0], data[0][0]);
} }
return euler; return euler;
} }
}; };
src/lib/mathlib/math/Quaternion.hpp
+15 -14
View File
@@ -51,7 +51,8 @@
namespace math namespace math
{ {
class __EXPORT Quaternion : public Vector<4> { class __EXPORT Quaternion : public Vector<4>
{
public: public:
/** /**
* trivial ctor * trivial ctor
@@ -85,25 +86,25 @@ public:
*/ */
const Quaternion operator *(const Quaternion &q) const { const Quaternion operator *(const Quaternion &q) const {
return Quaternion( return Quaternion(
data[0] * q.data[0] - data[1] * q.data[1] - data[2] * q.data[2] - data[3] * q.data[3], data[0] * q.data[0] - data[1] * q.data[1] - data[2] * q.data[2] - data[3] * q.data[3],
data[0] * q.data[1] + data[1] * q.data[0] + data[2] * q.data[3] - data[3] * q.data[2], data[0] * q.data[1] + data[1] * q.data[0] + data[2] * q.data[3] - data[3] * q.data[2],
data[0] * q.data[2] - data[1] * q.data[3] + data[2] * q.data[0] + data[3] * q.data[1], data[0] * q.data[2] - data[1] * q.data[3] + data[2] * q.data[0] + data[3] * q.data[1],
data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]); data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]);
} }
/** /**
* derivative * derivative
*/ */
const Quaternion derivative(const Vector<3> &w) { const Quaternion derivative(const Vector<3> &w) {
float dataQ[] = { float dataQ[] = {
data[0], -data[1], -data[2], -data[3], data[0], -data[1], -data[2], -data[3],
data[1], data[0], -data[3], data[2], data[1], data[0], -data[3], data[2],
data[2], data[3], data[0], -data[1], data[2], data[3], data[0], -data[1],
data[3], -data[2], data[1], data[0] data[3], -data[2], data[1], data[0]
}; };
Matrix<4,4> Q(dataQ); Matrix<4, 4> Q(dataQ);
Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]); Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]);
return Q * v * 0.5f; return Q * v * 0.5f;
} }
/** /**
src/lib/mathlib/math/Vector.hpp
+82 -51
View File
@@ -54,7 +54,8 @@ template <unsigned int N>
class __EXPORT Vector; class __EXPORT Vector;
template <unsigned int N> template <unsigned int N>
class __EXPORT VectorBase { class __EXPORT VectorBase
{
public: public:
/** /**
* vector data * vector data
@@ -93,14 +94,14 @@ public:
/** /**
* access to elements by index * access to elements by index
*/ */
float &operator ()(const unsigned int i) { float &operator()(const unsigned int i) {
return data[i]; return data[i];
} }
/** /**
* access to elements by index * access to elements by index
*/ */
float operator ()(const unsigned int i) const { float operator()(const unsigned int i) const {
return data[i]; return data[i];
} }
@@ -115,20 +116,22 @@ public:
* test for equality * test for equality
*/ */
bool operator ==(const Vector<N> &v) const { bool operator ==(const Vector<N> &v) const {
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++)
if (data[i] != v.data[i]) if (data[i] != v.data[i])
return false; return false;
return true;
return true;
} }
/** /**
* test for inequality * test for inequality
*/ */
bool operator !=(const Vector<N> &v) const { bool operator !=(const Vector<N> &v) const {
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++)
if (data[i] != v.data[i]) if (data[i] != v.data[i])
return true; return true;
return false;
return false;
} }
/** /**
@@ -144,9 +147,11 @@ public:
*/ */
const Vector<N> operator -(void) const { const Vector<N> operator -(void) const {
Vector<N> res; Vector<N> res;
for (unsigned int i = 0; i < N; i++)
res.data[i] = -data[i]; for (unsigned int i = 0; i < N; i++)
return res; res.data[i] = -data[i];
return res;
} }
/** /**
@@ -154,9 +159,11 @@ public:
*/ */
const Vector<N> operator +(const Vector<N> &v) const { const Vector<N> operator +(const Vector<N> &v) const {
Vector<N> res; Vector<N> res;
for (unsigned int i = 0; i < N; i++)
res.data[i] = data[i] + v.data[i]; for (unsigned int i = 0; i < N; i++)
return res; res.data[i] = data[i] + v.data[i];
return res;
} }
/** /**
@@ -164,9 +171,11 @@ public:
*/ */
const Vector<N> operator -(const Vector<N> &v) const { const Vector<N> operator -(const Vector<N> &v) const {
Vector<N> res; Vector<N> res;
for (unsigned int i = 0; i < N; i++)
res.data[i] = data[i] - v.data[i]; for (unsigned int i = 0; i < N; i++)
return res; res.data[i] = data[i] - v.data[i];
return res;
} }
/** /**
@@ -174,8 +183,10 @@ public:
*/ */
const Vector<N> operator *(const float num) const { const Vector<N> operator *(const float num) const {
Vector<N> res; Vector<N> res;
for (unsigned int i = 0; i < N; i++)
res.data[i] = data[i] * num; for (unsigned int i = 0; i < N; i++)
res.data[i] = data[i] * num;
return res; return res;
} }
@@ -184,8 +195,10 @@ public:
*/ */
const Vector<N> operator /(const float num) const { const Vector<N> operator /(const float num) const {
Vector<N> res; Vector<N> res;
for (unsigned int i = 0; i < N; i++)
res.data[i] = data[i] / num; for (unsigned int i = 0; i < N; i++)
res.data[i] = data[i] / num;
return res; return res;
} }
@@ -193,8 +206,9 @@ public:
* addition * addition
*/ */
const Vector<N> &operator +=(const Vector<N> &v) { const Vector<N> &operator +=(const Vector<N> &v) {
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++)
data[i] += v.data[i]; data[i] += v.data[i];
return *static_cast<const Vector<N>*>(this); return *static_cast<const Vector<N>*>(this);
} }
@@ -202,8 +216,9 @@ public:
* subtraction * subtraction
*/ */
const Vector<N> &operator -=(const Vector<N> &v) { const Vector<N> &operator -=(const Vector<N> &v) {
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++)
data[i] -= v.data[i]; data[i] -= v.data[i];
return *static_cast<const Vector<N>*>(this); return *static_cast<const Vector<N>*>(this);
} }
@@ -211,8 +226,9 @@ public:
* uniform scaling * uniform scaling
*/ */
const Vector<N> &operator *=(const float num) { const Vector<N> &operator *=(const float num) {
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++)
data[i] *= num; data[i] *= num;
return *static_cast<const Vector<N>*>(this); return *static_cast<const Vector<N>*>(this);
} }
@@ -220,8 +236,9 @@ public:
* uniform scaling * uniform scaling
*/ */
const Vector<N> &operator /=(const float num) { const Vector<N> &operator /=(const float num) {
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++)
data[i] /= num; data[i] /= num;
return *static_cast<const Vector<N>*>(this); return *static_cast<const Vector<N>*>(this);
} }
@@ -230,9 +247,11 @@ public:
*/ */
float operator *(const Vector<N> &v) const { float operator *(const Vector<N> &v) const {
float res = 0.0f; float res = 0.0f;
for (unsigned int i = 0; i < N; i++)
res += data[i] * v.data[i]; for (unsigned int i = 0; i < N; i++)
return res; res += data[i] * v.data[i];
return res;
} }
/** /**
@@ -240,9 +259,11 @@ public:
*/ */
float length_squared() const { float length_squared() const {
float res = 0.0f; float res = 0.0f;
for (unsigned int i = 0; i < N; i++)
res += data[i] * data[i]; for (unsigned int i = 0; i < N; i++)
return res; res += data[i] * data[i];
return res;
} }
/** /**
@@ -250,9 +271,11 @@ public:
*/ */
float length() const { float length() const {
float res = 0.0f; float res = 0.0f;
for (unsigned int i = 0; i < N; i++)
res += data[i] * data[i]; for (unsigned int i = 0; i < N; i++)
return sqrtf(res); res += data[i] * data[i];
return sqrtf(res);
} }
/** /**
@@ -278,14 +301,17 @@ public:
void print(void) { void print(void) {
printf("[ "); printf("[ ");
for (unsigned int i = 0; i < N; i++)
for (unsigned int i = 0; i < N; i++)
printf("%.3f\t", data[i]); printf("%.3f\t", data[i]);
printf("]\n"); printf("]\n");
} }
}; };
template <unsigned int N> template <unsigned int N>
class __EXPORT Vector : public VectorBase<N> { class __EXPORT Vector : public VectorBase<N>
{
public: public:
Vector() : VectorBase<N>() {} Vector() : VectorBase<N>() {}
@@ -303,7 +329,8 @@ public:
}; };
template <> template <>
class __EXPORT Vector<2> : public VectorBase<2> { class __EXPORT Vector<2> : public VectorBase<2>
{
public: public:
Vector() : VectorBase<2>() {} Vector() : VectorBase<2>() {}
@@ -333,12 +360,13 @@ public:
} }
float operator %(const Vector<2> &v) const { float operator %(const Vector<2> &v) const {
return data[0] * v.data[1] - data[1] * v.data[0]; return data[0] * v.data[1] - data[1] * v.data[0];
} }
}; };
template <> template <>
class __EXPORT Vector<3> : public VectorBase<3> { class __EXPORT Vector<3> : public VectorBase<3>
{
public: public:
Vector() : VectorBase<3>() {} Vector() : VectorBase<3>() {}
@@ -365,20 +393,22 @@ public:
const Vector<3> &operator =(const Vector<3> &v) { const Vector<3> &operator =(const Vector<3> &v) {
for (unsigned int i = 0; i < 3; i++) for (unsigned int i = 0; i < 3; i++)
data[i] = v.data[i]; data[i] = v.data[i];
return *this; return *this;
} }
Vector<3> operator %(const Vector<3> &v) const { Vector<3> operator %(const Vector<3> &v) const {
return Vector<3>( return Vector<3>(
data[1] * v.data[2] - data[2] * v.data[1], data[1] * v.data[2] - data[2] * v.data[1],
data[2] * v.data[0] - data[0] * v.data[2], data[2] * v.data[0] - data[0] * v.data[2],
data[0] * v.data[1] - data[1] * v.data[0] data[0] * v.data[1] - data[1] * v.data[0]
); );
} }
}; };
template <> template <>
class __EXPORT Vector<4> : public VectorBase<4> { class __EXPORT Vector<4> : public VectorBase<4>
{
public: public:
Vector() : VectorBase() {} Vector() : VectorBase() {}
@@ -405,6 +435,7 @@ public:
const Vector<4> &operator =(const Vector<4> &v) { const Vector<4> &operator =(const Vector<4> &v) {
for (unsigned int i = 0; i < 4; i++) for (unsigned int i = 0; i < 4; i++)
data[i] = v.data[i]; data[i] = v.data[i];
return *this; return *this;
} }
}; };