diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-18 19:33:47 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-18 19:33:47 +0400 |
commit | a83e3cd22276109301678c204e83050483200d6b (patch) | |
tree | 95cbb97ea90e6480436db865095dab3e6e7582f6 /src/lib/mathlib/math/Matrix.hpp | |
parent | ea107f41514ac8696ea8b94e93b9231a4802a15b (diff) | |
download | px4-firmware-a83e3cd22276109301678c204e83050483200d6b.tar.gz px4-firmware-a83e3cd22276109301678c204e83050483200d6b.tar.bz2 px4-firmware-a83e3cd22276109301678c204e83050483200d6b.zip |
New mathlib, WIP
Diffstat (limited to 'src/lib/mathlib/math/Matrix.hpp')
-rw-r--r-- | src/lib/mathlib/math/Matrix.hpp | 253 |
1 files changed, 233 insertions, 20 deletions
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index f19db15ec..f05360a19 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -1,6 +1,8 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone <will.perone@gmail.com> + * Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,30 +34,241 @@ ****************************************************************************/ /** - * @file Matrix.h + * @file Matrix3.hpp * - * matrix code + * 3x3 Matrix */ -#pragma once +#ifndef MATRIX_HPP +#define MATRIX_HPP -#include <nuttx/config.h> - -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) -#include "arm/Matrix.hpp" -#else -#include "generic/Matrix.hpp" -#endif +#include "../CMSIS/Include/arm_math.h" namespace math { + +template <unsigned int N, unsigned int M> class Matrix; -int matrixTest(); -int matrixAddTest(); -int matrixSubTest(); -int matrixMultTest(); -int matrixInvTest(); -int matrixDivTest(); -int matrixArmTest(); -bool matrixEqual(const Matrix &a, const Matrix &b, float eps = 1.0e-5f); -} // namespace math + +// MxN matrix with float elements +template <unsigned int N, unsigned int M> +class MatrixBase { +public: + /** + * matrix data[row][col] + */ + float data[M][N]; + + /** + * struct for using arm_math functions + */ + arm_matrix_instance_f32 arm_mat; + + /** + * trivial ctor + * note that this ctor will not initialize elements + */ + MatrixBase() { + //arm_mat_init_f32(&arm_mat, M, N, data); + arm_mat = {M, N, &data[0][0]}; + } + + /** + * access by index + */ + inline float &operator ()(unsigned int row, unsigned int col) { + return data[row][col]; + } + + /** + * access by index + */ + inline const float &operator ()(unsigned int row, unsigned int col) const { + return data[row][col]; + } + + unsigned int getRows() { + return M; + } + + unsigned int getCols() { + return N; + } + + /** + * test for equality + */ + bool operator ==(const MatrixBase<M, N> &m) { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m(i, j)) + return false; + return true; + } + + /** + * test for inequality + */ + bool operator !=(const MatrixBase<M, N> &m) { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m(i, j)) + return true; + return false; + } + + /** + * set to value + */ + const MatrixBase<M, N> &operator =(const MatrixBase<M, N> &m) { + memcpy(data, m.data, sizeof(data)); + return *this; + } + + /** + * negation + */ + MatrixBase<M, N> operator -(void) const { + MatrixBase<M, N> res; + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + res[i][j] = -data[i][j]; + return res; + } + + /** + * addition + */ + MatrixBase<M, N> operator +(const MatrixBase<M, N> &m) const { + MatrixBase<M, N> res; + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + res[i][j] = data[i][j] + m(i, j); + return res; + } + + MatrixBase<M, N> &operator +=(const MatrixBase<M, N> &m) { + return *this = *this + m; + } + + /** + * subtraction + */ + MatrixBase<M, N> operator -(const MatrixBase<M, N> &m) const { + MatrixBase<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] - m(i, j); + return res; + } + + MatrixBase<M, N> &operator -=(const MatrixBase<M, N> &m) { + return *this = *this - m; + } + + /** + * uniform scaling + */ + MatrixBase<M, N> operator *(const float num) const { + MatrixBase<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; + return res; + } + + MatrixBase<M, N> &operator *=(const float num) { + return *this = *this * num; + } + + MatrixBase<M, N> operator /(const float num) const { + MatrixBase<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; + return res; + } + + MatrixBase<M, N> &operator /=(const float num) { + return *this = *this / num; + } + + /** + * multiplication by another matrix + */ + template <unsigned int P> + Matrix<N, P> operator *(const Matrix<M, P> &m) const { + Matrix<N, P> res; + arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); + return res; + } + + /** + * setup the identity matrix + */ + void identity(void) { + memset(data, 0, sizeof(data)); + for (unsigned int i = 0; i < M; i++) + data[i][i] = 1; + } + + void dump(void) { + for (unsigned int i = 0; i < M; i++) { + for (unsigned int j = 0; j < N; j++) + printf("%.3f\t", data[i][j]); + printf("\n"); + } + } +}; + +template <unsigned int M, unsigned int N> +class Matrix : public MatrixBase<M, N> { +public: + /** + * set to value + */ + const Matrix<M, N> &operator =(const Matrix<M, N> &m) { + memcpy(this->data, m.data, sizeof(this->data)); + return *this; + } + + /** + * 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: + /** + * set to value + */ + const Matrix<3, 3> &operator =(const Matrix<3, 3> &m) { + memcpy(this->data, m.data, sizeof(this->data)); + return *this; + } + + /** + * 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; + } + */ +}; + +} + +#endif // MATRIX_HPP |