From a83e3cd22276109301678c204e83050483200d6b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 18 Dec 2013 19:33:47 +0400 Subject: New mathlib, WIP --- src/lib/mathlib/math/Matrix.hpp | 253 ++++++++++++++++++++++++++++++++++++---- 1 file changed, 233 insertions(+), 20 deletions(-) (limited to 'src/lib/mathlib/math/Matrix.hpp') 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 + * Anton Babushkin * * 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 - -#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 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 +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) { + 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) { + 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 &operator =(const MatrixBase &m) { + memcpy(data, m.data, sizeof(data)); + return *this; + } + + /** + * negation + */ + MatrixBase operator -(void) const { + MatrixBase 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 operator +(const MatrixBase &m) const { + MatrixBase 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 &operator +=(const MatrixBase &m) { + return *this = *this + m; + } + + /** + * subtraction + */ + MatrixBase operator -(const MatrixBase &m) const { + MatrixBase 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 &operator -=(const MatrixBase &m) { + return *this = *this - m; + } + + /** + * uniform scaling + */ + MatrixBase operator *(const float num) const { + MatrixBase 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 &operator *=(const float num) { + return *this = *this * num; + } + + MatrixBase operator /(const float num) const { + MatrixBase 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 &operator /=(const float num) { + return *this = *this / num; + } + + /** + * multiplication by another matrix + */ + template + Matrix operator *(const Matrix &m) const { + Matrix 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 +class Matrix : public MatrixBase { +public: + /** + * set to value + */ + const Matrix &operator =(const Matrix &m) { + memcpy(this->data, m.data, sizeof(this->data)); + return *this; + } + + /** + * multiplication by a vector + */ + /* + Vector operator *(const Vector &v) const { + Vector 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 -- cgit v1.2.3