diff options
Diffstat (limited to 'apps/systemlib/math/generic/Matrix.hpp')
| -rw-r--r-- | apps/systemlib/math/generic/Matrix.hpp | 447 |
1 files changed, 447 insertions, 0 deletions
diff --git a/apps/systemlib/math/generic/Matrix.hpp b/apps/systemlib/math/generic/Matrix.hpp new file mode 100644 index 000000000..fd6c8ba51 --- /dev/null +++ b/apps/systemlib/math/generic/Matrix.hpp @@ -0,0 +1,447 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Matrix.h + * + * matrix code + */ + +#pragma once + + +#include <inttypes.h> +#include <assert.h> +#include <stdlib.h> +#include <string.h> +#include <stdio.h> +#include <math.h> + +#include <systemlib/math/Vector.hpp> +#include <systemlib/math/Matrix.hpp> + +namespace math +{ + +class __EXPORT Matrix { +public: + // constructor + Matrix(size_t rows, size_t cols) : + _rows(rows), + _cols(cols), + _data((float*)calloc(rows*cols,sizeof(float))) + { + } + Matrix(size_t rows, size_t cols, const float * data) : + _rows(rows), + _cols(cols), + _data((float*)malloc(getSize())) + { + memcpy(getData(),data,getSize()); + } + // deconstructor + virtual ~Matrix() + { + delete [] getData(); + } + // copy constructor (deep) + Matrix(const Matrix & right) : + _rows(right.getRows()), + _cols(right.getCols()), + _data((float*)malloc(getSize())) + { + memcpy(getData(),right.getData(), + right.getSize()); + } + // assignment + inline Matrix & operator=(const Matrix & right) + { +#ifdef MATRIX_ASSERT + ASSERT(getRows()==right.getRows()); + ASSERT(getCols()==right.getCols()); +#endif + if (this != &right) + { + memcpy(getData(),right.getData(), + right.getSize()); + } + return *this; + } + // element accessors + inline float & operator()(size_t i, size_t j) + { +#ifdef MATRIX_ASSERT + ASSERT(i<getRows()); + ASSERT(j<getCols()); +#endif + return getData()[i*getCols() + j]; + } + inline const float & operator()(size_t i, size_t j) const + { +#ifdef MATRIX_ASSERT + ASSERT(i<getRows()); + ASSERT(j<getCols()); +#endif + return getData()[i*getCols() + j]; + } + // output + inline void print() const + { + for (size_t i=0; i<getRows(); i++) + { + for (size_t j=0; j<getCols(); j++) + { + float sig; + int exp; + float num = (*this)(i,j); + float2SigExp(num,sig,exp); + printf ("%6.3fe%03.3d,", (double)sig, exp); + } + printf("\n"); + } + } + // boolean ops + inline bool operator==(const Matrix & right) const + { + for (size_t i=0; i<getRows(); i++) + { + for (size_t j=0; j<getCols(); j++) + { + if (fabsf((*this)(i,j)-right(i,j)) > 1e-30f) + return false; + } + } + return true; + } + // scalar ops + inline Matrix operator+(const float & right) const + { + Matrix result(getRows(), getCols()); + for (size_t i=0; i<getRows(); i++) + { + for (size_t j=0; j<getCols(); j++) + { + result(i,j) = (*this)(i,j) + right; + } + } + return result; + } + inline Matrix operator-(const float & right) const + { + Matrix result(getRows(), getCols()); + for (size_t i=0; i<getRows(); i++) + { + for (size_t j=0; j<getCols(); j++) + { + result(i,j) = (*this)(i,j) - right; + } + } + return result; + } + inline Matrix operator*(const float & right) const + { + Matrix result(getRows(), getCols()); + for (size_t i=0; i<getRows(); i++) + { + for (size_t j=0; j<getCols(); j++) + { + result(i,j) = (*this)(i,j) * right; + } + } + return result; + } + inline Matrix operator/(const float & right) const + { + Matrix result(getRows(), getCols()); + for (size_t i=0; i<getRows(); i++) + { + for (size_t j=0; j<getCols(); j++) + { + result(i,j) = (*this)(i,j) / right; + } + } + return result; + } + // vector ops + inline Vector operator*(const Vector & right) const + { +#ifdef MATRIX_ASSERT + ASSERT(getCols()==right.getRows()); +#endif + Vector result(getRows()); + for (size_t i=0; i<getRows(); i++) + { + for (size_t j=0; j<getCols(); j++) + { + result(i) += (*this)(i,j) * right(j); + } + } + return result; + } + // matrix ops + inline Matrix operator+(const Matrix & right) const + { +#ifdef MATRIX_ASSERT + ASSERT(getRows()==right.getRows()); + ASSERT(getCols()==right.getCols()); +#endif + Matrix result(getRows(), getCols()); + for (size_t i=0; i<getRows(); i++) + { + for (size_t j=0; j<getCols(); j++) + { + result(i,j) = (*this)(i,j) + right(i,j); + } + } + return result; + } + inline Matrix operator-(const Matrix & right) const + { +#ifdef MATRIX_ASSERT + ASSERT(getRows()==right.getRows()); + ASSERT(getCols()==right.getCols()); +#endif + Matrix result(getRows(), getCols()); + for (size_t i=0; i<getRows(); i++) + { + for (size_t j=0; j<getCols(); j++) + { + result(i,j) = (*this)(i,j) - right(i,j); + } + } + return result; + } + inline Matrix operator*(const Matrix & right) const + { +#ifdef MATRIX_ASSERT + ASSERT(getCols()==right.getRows()); +#endif + Matrix result(getRows(), right.getCols()); + for (size_t i=0; i<getRows(); i++) + { + for (size_t j=0; j<right.getCols(); j++) + { + for (size_t k=0; k<right.getRows(); k++) + { + result(i,j) += (*this)(i,k) * right(k,j); + } + } + } + return result; + } + inline Matrix operator/(const Matrix & right) const + { +#ifdef MATRIX_ASSERT + ASSERT(right.getRows()==right.getCols()); + ASSERT(getCols()==right.getCols()); +#endif + return (*this)*right.inverse(); + } + // other functions + inline Matrix transpose() const + { + Matrix result(getCols(),getRows()); + for(size_t i=0;i<getRows();i++) { + for(size_t j=0;j<getCols();j++) { + result(j,i) = (*this)(i,j); + } + } + return result; + } + inline void swapRows(size_t a, size_t b) + { + if (a==b) return; + for(size_t j=0;j<getCols();j++) { + float tmp = (*this)(a,j); + (*this)(a,j) = (*this)(b,j); + (*this)(b,j) = tmp; + } + } + inline void swapCols(size_t a, size_t b) + { + if (a==b) return; + for(size_t i=0;i<getRows();i++) { + float tmp = (*this)(i,a); + (*this)(i,a) = (*this)(i,b); + (*this)(i,b) = tmp; + } + } + /** + * inverse based on LU factorization with partial pivotting + */ + Matrix inverse() const + { +#ifdef MATRIX_ASSERT + ASSERT(getRows()==getCols()); +#endif + size_t N = getRows(); + Matrix L = identity(N); + const Matrix & A = (*this); + Matrix U = A; + Matrix P = identity(N); + + //printf("A:\n"); A.print(); + + // for all diagonal elements + for (size_t n=0; n<N; n++) { + + // if diagonal is zero, swap with row below + if (fabsf(U(n,n))<1e-8f) { + //printf("trying pivot for row %d\n",n); + for (size_t i=0; i<N; i++) { + if (i==n) continue; + //printf("\ttrying row %d\n",i); + if (fabsf(U(i,n))>1e-8f) { + //printf("swapped %d\n",i); + U.swapRows(i,n); + P.swapRows(i,n); + } + } + } +#ifdef MATRIX_ASSERT + //printf("A:\n"); A.print(); + //printf("U:\n"); U.print(); + //printf("P:\n"); P.print(); + //fflush(stdout); + ASSERT(fabsf(U(n,n))>1e-8f); +#endif + // failsafe, return zero matrix + if (fabsf(U(n,n))<1e-8f) + { + return Matrix::zero(n); + } + + // for all rows below diagonal + for (size_t i=(n+1); i<N; i++) { + L(i,n) = U(i,n)/U(n,n); + // add i-th row and n-th row + // multiplied by: -a(i,n)/a(n,n) + for (size_t k=n; k<N; k++) { + U(i,k) -= L(i,n) * U(n,k); + } + } + } + + //printf("L:\n"); L.print(); + //printf("U:\n"); U.print(); + + // solve LY=P*I for Y by forward subst + Matrix Y = P; + // for all columns of Y + for (size_t c=0; c<N; c++) { + // for all rows of L + for (size_t i=0; i<N; i++) { + // for all columns of L + for (size_t j=0; j<i; j++) { + // for all existing y + // subtract the component they + // contribute to the solution + Y(i,c) -= L(i,j)*Y(j,c); + } + // divide by the factor + // on current + // term to be solved + // Y(i,c) /= L(i,i); + // but L(i,i) = 1.0 + } + } + + //printf("Y:\n"); Y.print(); + + // solve Ux=y for x by back subst + Matrix X = Y; + // for all columns of X + for (size_t c=0; c<N; c++) { + // for all rows of U + for (size_t k=0; k<N; k++) { + // have to go in reverse order + size_t i = N-1-k; + // for all columns of U + for (size_t j=i+1; j<N; j++) { + // for all existing x + // subtract the component they + // contribute to the solution + X(i,c) -= U(i,j)*X(j,c); + } + // divide by the factor + // on current + // term to be solved + X(i,c) /= U(i,i); + } + } + //printf("X:\n"); X.print(); + return X; + } + inline void setAll(const float & val) + { + for (size_t i=0;i<getRows();i++) { + for (size_t j=0;j<getCols();j++) { + (*this)(i,j) = val; + } + } + } + inline void set(const float * data) + { + memcpy(getData(),data,getSize()); + } + inline size_t getRows() const { return _rows; } + inline size_t getCols() const { return _cols; } + inline static Matrix identity(size_t size) { + Matrix result(size,size); + for (size_t i=0; i<size; i++) { + result(i,i) = 1.0f; + } + return result; + } + inline static Matrix zero(size_t size) { + Matrix result(size,size); + result.setAll(0.0f); + return result; + } + inline static Matrix zero(size_t m, size_t n) { + Matrix result(m,n); + result.setAll(0.0f); + return result; + } +protected: + inline size_t getSize() const { return sizeof(float)*getRows()*getCols(); } + inline float * getData() { return _data; } + inline const float * getData() const { return _data; } + inline void setData(float * data) { _data = data; } +private: + size_t _rows; + size_t _cols; + float * _data; +}; + +} // namespace math |