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

mathlib: fixes and improvements, WIP

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-18 23:01:02 +04:00
parent a83e3cd222
commit e3a5a384d7
4 changed files with 156 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mathlib/math/Matrix.hpp
+27 -11
View File
@@ -34,9 +34,9 @@
****************************************************************************/
/**
* @file Matrix3.hpp
* @file Matrix.hpp
*
* 3x3 Matrix
* Generic Matrix
*/
#ifndef MATRIX_HPP
@@ -203,6 +203,24 @@ public:
return res;
}
/**
* transpose the matrix
*/
Matrix<N, M> transposed(void) const {
Matrix<N, M> res;
arm_mat_trans_f32(&this->arm_mat, &res.arm_mat);
return res;
}
/**
* invert the matrix
*/
Matrix<M, N> inversed(void) const {
Matrix<M, N> res;
arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
return res;
}
/**
* setup the identity matrix
*/
@@ -224,6 +242,8 @@ public:
template <unsigned int M, unsigned int N>
class Matrix : public MatrixBase<M, N> {
public:
using MatrixBase<M, N>::operator *;
/**
* set to value
*/
@@ -235,18 +255,18 @@ public:
/**
* multiplication by a vector
*/
/*
Vector<N> operator *(const Vector<N> &v) const {
Vector<M> res;
arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
return res;
}
*/
};
template <>
class Matrix<3, 3> : public MatrixBase<3, 3> {
public:
using MatrixBase<3, 3>::operator *;
/**
* set to value
*/
@@ -258,15 +278,11 @@ public:
/**
* multiplication by a vector
*/
/*
Vector<3> operator *(const Vector<3> &v) const {
Vector<3> res;
res(0) = data[0][0] * v(0) + data[0][1] * v(1) + data[0][2] * v(2);
res(1) = data[1][0] * v(0) + data[1][1] * v(1) + data[1][2] * v(2);
res(2) = data[2][0] * v(0) + data[2][1] * v(1) + data[2][2] * v(2);
return res;
return Vector<3>(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]);
}
*/
};
}