SPQR-Team1-2020/lib/Adafruit_BNO055/utility/matrix.h

/*
    Inertial Measurement Unit Maths Library
    Copyright (C) 2013-2014  Samuel Cowen
    www.camelsoftware.com

    Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com)

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef IMUMATH_MATRIX_HPP
#define IMUMATH_MATRIX_HPP

#include <string.h>
#include <stdint.h>

#include "vector.h"

namespace imu
{


template <uint8_t N> class Matrix
{
public:
    Matrix()
    {
        memset(_cell_data, 0, N*N*sizeof(double));
    }

    Matrix(const Matrix &m)
    {
        for (int ij = 0; ij < N*N; ++ij)
        {
            _cell_data[ij] = m._cell_data[ij];
        }
    }

    ~Matrix()
    {
    }

    Matrix& operator=(const Matrix& m)
    {
        for (int ij = 0; ij < N*N; ++ij)
        {
            _cell_data[ij] = m._cell_data[ij];
        }
        return *this;
    }

    Vector<N> row_to_vector(int i) const
    {
        Vector<N> ret;
        for (int j = 0; j < N; j++)
        {
            ret[j] = cell(i, j);
        }
        return ret;
    }

    Vector<N> col_to_vector(int j) const
    {
        Vector<N> ret;
        for (int i = 0; i < N; i++)
        {
            ret[i] = cell(i, j);
        }
        return ret;
    }

    void vector_to_row(const Vector<N>& v, int i)
    {
        for (int j = 0; j < N; j++)
        {
            cell(i, j) = v[j];
        }
    }

    void vector_to_col(const Vector<N>& v, int j)
    {
        for (int i = 0; i < N; i++)
        {
            cell(i, j) = v[i];
        }
    }

    double operator()(int i, int j) const
    {
        return cell(i, j);
    }
    double& operator()(int i, int j)
    {
        return cell(i, j);
    }

    double cell(int i, int j) const
    {
        return _cell_data[i*N+j];
    }
    double& cell(int i, int j)
    {
        return _cell_data[i*N+j];
    }


    Matrix operator+(const Matrix& m) const
    {
        Matrix ret;
        for (int ij = 0; ij < N*N; ++ij)
        {
            ret._cell_data[ij] = _cell_data[ij] + m._cell_data[ij];
        }
        return ret;
    }

    Matrix operator-(const Matrix& m) const
    {
        Matrix ret;
        for (int ij = 0; ij < N*N; ++ij)
        {
            ret._cell_data[ij] = _cell_data[ij] - m._cell_data[ij];
        }
        return ret;
    }

    Matrix operator*(double scalar) const
    {
        Matrix ret;
        for (int ij = 0; ij < N*N; ++ij)
        {
            ret._cell_data[ij] = _cell_data[ij] * scalar;
        }
        return ret;
    }

    Matrix operator*(const Matrix& m) const
    {
        Matrix ret;
        for (int i = 0; i < N; i++)
        {
            Vector<N> row = row_to_vector(i);
            for (int j = 0; j < N; j++)
            {
                ret(i, j) = row.dot(m.col_to_vector(j));
            }
        }
        return ret;
    }

    Matrix transpose() const
    {
        Matrix ret;
        for (int i = 0; i < N; i++)
        {
            for (int j = 0; j < N; j++)
            {
                ret(j, i) = cell(i, j);
            }
        }
        return ret;
    }

    Matrix<N-1> minor_matrix(int row, int col) const
    {
        Matrix<N-1> ret;
        for (int i = 0, im = 0; i < N; i++)
        {
            if (i == row)
                continue;

            for (int j = 0, jm = 0; j < N; j++)
            {
                if (j != col)
                {
                    ret(im, jm++) = cell(i, j);
                }
            }
            im++;
        }
        return ret;
    }

    double determinant() const
    {
        // specialization for N == 1 given below this class
        double det = 0.0, sign = 1.0;
        for (int i = 0; i < N; ++i, sign = -sign)
            det += sign * cell(0, i) * minor_matrix(0, i).determinant();
        return det;
    }

    Matrix invert() const
    {
        Matrix ret;
        double det = determinant();

        for (int i = 0; i < N; i++)
        {
            for (int j = 0; j < N; j++)
            {
                ret(i, j) = minor_matrix(j, i).determinant() / det;
                if ((i+j)%2 == 1)
                    ret(i, j) = -ret(i, j);
            }
        }
        return ret;
    }

    double trace() const
    {
        double tr = 0.0;
        for (int i = 0; i < N; ++i)
            tr += cell(i, i);
        return tr;
    }

private:
    double _cell_data[N*N];
};


template<>
inline double Matrix<1>::determinant() const
{
    return cell(0, 0);
}

};

#endif