diff options
author | jgoppert <james.goppert@gmail.com> | 2013-01-06 15:33:55 -0500 |
---|---|---|
committer | jgoppert <james.goppert@gmail.com> | 2013-01-06 15:33:55 -0500 |
commit | d9491b20cc5fc8b683eb0f60a50da6b322b55e57 (patch) | |
tree | 48d44accf2ccff88766bca351c78be06bb9fa4a0 /apps/systemlib/math/generic/Matrix.hpp | |
parent | 4f3b17f53b120cd54112097f4217a90863013c1f (diff) | |
download | px4-firmware-d9491b20cc5fc8b683eb0f60a50da6b322b55e57.tar.gz px4-firmware-d9491b20cc5fc8b683eb0f60a50da6b322b55e57.tar.bz2 px4-firmware-d9491b20cc5fc8b683eb0f60a50da6b322b55e57.zip |
Reformat of math library with astyle.
Diffstat (limited to 'apps/systemlib/math/generic/Matrix.hpp')
-rw-r--r-- | apps/systemlib/math/generic/Matrix.hpp | 722 |
1 files changed, 356 insertions, 366 deletions
diff --git a/apps/systemlib/math/generic/Matrix.hpp b/apps/systemlib/math/generic/Matrix.hpp index fd6c8ba51..d10208a1e 100644 --- a/apps/systemlib/math/generic/Matrix.hpp +++ b/apps/systemlib/math/generic/Matrix.hpp @@ -53,395 +53,385 @@ namespace math { -class __EXPORT Matrix { +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) - { + // 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()); + 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) - { + + 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()); + ASSERT(i < getRows()); + ASSERT(j < getCols()); #endif - return getData()[i*getCols() + j]; - } - inline const float & operator()(size_t i, size_t j) const - { + 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()); + 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 - { + 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()); + 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 - { + 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()); + 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 - { + 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()); + 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 - { + 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()); + 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 - { + 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()); + 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 - { + 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()); + 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); - } - } - } + 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); + //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; - } + + // 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; } + 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; + size_t _rows; + size_t _cols; + float *_data; }; } // namespace math |