diff options
Diffstat (limited to 'src/lib/mathlib/math/Matrix.hpp')
-rw-r--r-- | src/lib/mathlib/math/Matrix.hpp | 416 |
1 files changed, 395 insertions, 21 deletions
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index f19db15ec..ea0cf4ca1 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -1,6 +1,9 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> + * Pavel Kirienko <pavel.kirienko@gmail.com> + * Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,30 +35,401 @@ ****************************************************************************/ /** - * @file Matrix.h + * @file Matrix.hpp * - * matrix code + * Matrix class */ -#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 <stdio.h> +#include "../CMSIS/Include/arm_math.h" namespace math { -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 + +template <unsigned int M, unsigned int N> +class __EXPORT Matrix; + +// MxN matrix with float elements +template <unsigned int M, unsigned int N> +class __EXPORT 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 = {M, N, &data[0][0]}; + } + + /** + * copyt ctor + */ + MatrixBase(const MatrixBase<M, N> &m) { + arm_mat = {M, N, &data[0][0]}; + memcpy(data, m.data, sizeof(data)); + } + + MatrixBase(const float *d) { + 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]}; + memcpy(data, d, sizeof(data)); + } + + /** + * set data + */ + void set(const float *d) { + memcpy(data, d, sizeof(data)); + } + + /** + * set data + */ + void set(const float d[M][N]) { + memcpy(data, d, sizeof(data)); + } + + /** + * access by index + */ + float &operator()(const unsigned int row, const unsigned int col) { + return data[row][col]; + } + + /** + * access by index + */ + float operator()(const unsigned int row, const unsigned int col) const { + return data[row][col]; + } + + /** + * get rows number + */ + unsigned int get_rows() const { + return M; + } + + /** + * get columns number + */ + unsigned int get_cols() const { + return N; + } + + /** + * test for equality + */ + 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; + + return true; + } + + /** + * test for inequality + */ + 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; + + return false; + } + + /** + * set to value + */ + const Matrix<M, N> &operator =(const Matrix<M, N> &m) { + memcpy(data, m.data, sizeof(data)); + return *static_cast<Matrix<M, N>*>(this); + } + + /** + * negation + */ + Matrix<M, N> operator -(void) const { + 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]; + + return res; + } + + /** + * addition + */ + Matrix<M, N> operator +(const Matrix<M, N> &m) const { + 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]; + + 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]; + + return *static_cast<Matrix<M, N>*>(this); + } + + /** + * subtraction + */ + Matrix<M, N> operator -(const Matrix<M, N> &m) const { + 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]; + + 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]; + + return *static_cast<Matrix<M, N>*>(this); + } + + /** + * uniform scaling + */ + Matrix<M, N> operator *(const float num) const { + 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; + + 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; + + return *static_cast<Matrix<M, N>*>(this); + } + + Matrix<M, N> operator /(const float num) const { + 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; + + 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; + + return *static_cast<Matrix<M, N>*>(this); + } + + /** + * multiplication by another matrix + */ + template <unsigned int P> + Matrix<M, P> operator *(const Matrix<N, P> &m) const { + Matrix<M, P> res; + arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); + 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; + } + + /** + * set zero matrix + */ + void zero(void) { + memset(data, 0, sizeof(data)); + } + + /** + * set identity matrix + */ + void identity(void) { + memset(data, 0, sizeof(data)); + unsigned int n = (M < N) ? M : N; + + 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]); + + printf(" ]\n"); + } + } +}; + +template <unsigned int M, unsigned int N> +class __EXPORT Matrix : public MatrixBase<M, N> +{ +public: + using MatrixBase<M, N>::operator *; + + Matrix() : MatrixBase<M, N>() {} + + Matrix(const Matrix<M, N> &m) : MatrixBase<M, N>(m) {} + + Matrix(const float *d) : MatrixBase<M, N>(d) {} + + Matrix(const float d[M][N]) : MatrixBase<M, N>(d) {} + + /** + * 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<M> 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 __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> +{ +public: + using MatrixBase<3, 3>::operator *; + + Matrix() : MatrixBase<3, 3>() {} + + Matrix(const Matrix<3, 3> &m) : MatrixBase<3, 3>(m) {} + + Matrix(const float *d) : MatrixBase<3, 3>(d) {} + + Matrix(const float d[3][3]) : MatrixBase<3, 3>(d) {} + + /** + * 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(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]); + return res; + } + + /** + * create a rotation matrix from given euler angles + * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf + */ + void from_euler(float roll, float pitch, float yaw) { + float cp = cosf(pitch); + float sp = sinf(pitch); + float sr = sinf(roll); + float cr = cosf(roll); + float sy = sinf(yaw); + float cy = cosf(yaw); + + data[0][0] = cp * cy; + data[0][1] = (sr * sp * cy) - (cr * sy); + data[0][2] = (cr * sp * cy) + (sr * sy); + data[1][0] = cp * sy; + data[1][1] = (sr * sp * sy) + (cr * cy); + data[1][2] = (cr * sp * sy) - (sr * cy); + data[2][0] = -sp; + data[2][1] = sr * cp; + data[2][2] = cr * cp; + } + + /** + * get euler angles from rotation matrix + */ + Vector<3> to_euler(void) const { + Vector<3> euler; + euler.data[1] = asinf(-data[2][0]); + + if (fabsf(euler.data[1] - M_PI_2_F) < 1.0e-3f) { + euler.data[0] = 0.0f; + euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) + euler.data[0]; + + } else if (fabsf(euler.data[1] + M_PI_2_F) < 1.0e-3f) { + euler.data[0] = 0.0f; + euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) - euler.data[0]; + + } else { + euler.data[0] = atan2f(data[2][1], data[2][2]); + euler.data[2] = atan2f(data[1][0], data[0][0]); + } + + return euler; + } +}; + +} + +#endif // MATRIX_HPP |