diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-18 23:01:02 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-18 23:01:02 +0400 |
commit | e3a5a384d7b3678d1cbef63dc28fbe9a8f1de940 (patch) | |
tree | 1f90de01a4f5b1be1eb827981330400c43242013 /src/lib/mathlib/math/Matrix.hpp | |
parent | a83e3cd22276109301678c204e83050483200d6b (diff) | |
download | px4-firmware-e3a5a384d7b3678d1cbef63dc28fbe9a8f1de940.tar.gz px4-firmware-e3a5a384d7b3678d1cbef63dc28fbe9a8f1de940.tar.bz2 px4-firmware-e3a5a384d7b3678d1cbef63dc28fbe9a8f1de940.zip |
mathlib: fixes and improvements, WIP
Diffstat (limited to 'src/lib/mathlib/math/Matrix.hpp')
-rw-r--r-- | src/lib/mathlib/math/Matrix.hpp | 38 |
1 files changed, 27 insertions, 11 deletions
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index f05360a19..1849f58be 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -34,9 +34,9 @@ ****************************************************************************/ /** - * @file Matrix3.hpp + * @file Matrix.hpp * - * 3x3 Matrix + * Generic Matrix */ #ifndef MATRIX_HPP @@ -204,6 +204,24 @@ public: } /** + * 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 */ void identity(void) { @@ -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]); } - */ }; } |