xmmMatrix.hpp

Go to the documentation of this file.

/*
 * xmmMatrix.hpp
 *
 * Matrix Utilities
 *
 * Contact:
 * - Jules Francoise <jules.francoise@ircam.fr>
 *
 * This code has been initially authored by Jules Francoise
 * <http://julesfrancoise.com> during his PhD thesis, supervised by Frederic
 * Bevilacqua <href="http://frederic-bevilacqua.net>, in the Sound Music
 * Movement Interaction team <http://ismm.ircam.fr> of the
 * STMS Lab - IRCAM, CNRS, UPMC (2011-2015).
 *
 * Copyright (C) 2015 UPMC, Ircam-Centre Pompidou.
 *
 * This File is part of XMM.
 *
 * XMM 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.
 *
 * XMM 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 XMM.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef xmmMatrix_h
#define xmmMatrix_h

#include <cmath>
#include <exception>
#include <iostream>
#include <stdexcept>
#include <vector>

namespace xmm {
template <typename T>
class Matrix {
  public:
    static const double kEpsilonPseudoInverse() { return 1.0e-9; }

    unsigned int nrows;

    unsigned int ncols;

    std::vector<T> _data;

    typename std::vector<T>::iterator data;

    bool ownData;

    Matrix(bool ownData_ = true) : nrows(0), ncols(0), ownData(ownData_) {}

    Matrix(unsigned int nrows_, bool ownData_ = true) {
        nrows = nrows_;
        ncols = nrows_;
        ownData = ownData_;
        if (ownData) {
            _data.assign(nrows * ncols, T(0.0));
            data = _data.begin();
        }
    }

    Matrix(unsigned int nrows_, unsigned int ncols_, bool ownData_ = true) {
        nrows = nrows_;
        ncols = ncols_;
        ownData = ownData_;
        if (ownData) {
            _data.assign(nrows * ncols, T(0.0));
            data = _data.begin();
        }
    }

    Matrix(unsigned int nrows_, unsigned int ncols_,
           typename std::vector<T>::iterator data_it) {
        nrows = nrows_;
        ncols = ncols_;
        ownData = false;
        data = data_it;
    }

    void resize(unsigned int nrows_, unsigned int ncols_) {
        nrows = nrows_;
        ncols = ncols_;
        _data.resize(nrows * ncols);
    }

    void resize(unsigned int nrows_) {
        if (nrows != ncols) throw std::runtime_error("Matrix is not square");

        nrows = nrows_;
        ncols = nrows;
        _data.resize(nrows * ncols);
    }

    float sum() const {
        float sum_(0.);
        for (unsigned int i = 0; i < nrows * ncols; i++) sum_ += data[i];
        return sum_;
    }

    void print() const {
        for (unsigned int i = 0; i < nrows; i++) {
            for (unsigned int j = 0; j < ncols; j++) {
                std::cout << data[i * ncols + j] << " ";
            }
            std::cout << std::endl;
        }
    }

    Matrix<T> *transpose() const {
        Matrix<T> *out = new Matrix<T>(ncols, nrows);
        for (unsigned int i = 0; i < ncols; i++) {
            for (unsigned int j = 0; j < nrows; j++) {
                out->data[i * nrows + j] = data[j * ncols + i];
            }
        }
        return out;
    }

    Matrix<T> *product(Matrix const *mat) const {
        if (ncols != mat->nrows)
            throw std::runtime_error("Wrong dimensions for matrix product");

        Matrix<T> *out = new Matrix<T>(nrows, mat->ncols);
        for (unsigned int i = 0; i < nrows; i++) {
            for (unsigned int j = 0; j < mat->ncols; j++) {
                out->data[i * mat->ncols + j] = 0.;
                for (unsigned int k = 0; k < ncols; k++) {
                    out->data[i * mat->ncols + j] +=
                        data[i * ncols + k] * mat->data[k * mat->ncols + j];
                }
            }
        }
        return out;
    }

    Matrix<T> *pinv(double *det) const {
        Matrix<T> *inverse = NULL;
        if (nrows == ncols) {
            inverse = gauss_jordan_inverse(det);
            if (inverse) {
                return inverse;
            }
        }

        inverse = new Matrix<T>(ncols, nrows);
        Matrix<T> *transp, *prod, *dst;
        transp = this->transpose();
        if (nrows >= ncols) {
            prod = transp->product(this);
            dst = prod->gauss_jordan_inverse(det);
            inverse = dst->product(transp);
        } else {
            prod = this->product(transp);
            dst = prod->gauss_jordan_inverse(det);
            inverse = transp->product(dst);
        }
        *det = 0;
        delete transp;
        delete prod;
        delete dst;
        return inverse;
    }

    Matrix<T> *gauss_jordan_inverse(double *det) const {
        if (nrows != ncols) {
            throw std::runtime_error(
                "Gauss-Jordan inversion: Can't invert Non-square matrix");
        }
        *det = 1.0f;
        Matrix<T> mat(nrows, ncols * 2);
        Matrix<T> new_mat(nrows, ncols * 2);

        unsigned int n = nrows;

        // Create matrix
        for (unsigned int i = 0; i < n; i++) {
            for (unsigned int j = 0; j < n; j++) {
                mat._data[i * 2 * n + j] = data[i * n + j];
            }
            mat._data[i * 2 * n + n + i] = 1;
        }

        for (unsigned int k = 0; k < n; k++) {
            unsigned int i(k);
            while (std::fabs(mat._data[i * 2 * n + k]) <
                   kEpsilonPseudoInverse()) {
                i++;
                if (i == n) {
                    throw std::runtime_error("Non-invertible matrix");
                }
            }
            *det *= mat._data[i * 2 * n + k];

            // if found > Exchange lines
            if (i != k) {
                mat.swap_lines(i, k);
            }

            new_mat._data = mat._data;

            for (unsigned int j = 0; j < 2 * n; j++) {
                new_mat._data[k * 2 * n + j] /= mat._data[k * 2 * n + k];
            }
            for (i = 0; i < n; i++) {
                if (i != k) {
                    for (unsigned int j = 0; j < 2 * n; j++) {
                        new_mat._data[i * 2 * n + j] -=
                            mat._data[i * 2 * n + k] *
                            new_mat._data[k * 2 * n + j];
                    }
                }
            }
            mat._data = new_mat._data;
        }

        Matrix<T> *dst = new Matrix<T>(nrows, ncols);
        for (unsigned int i = 0; i < n; i++)
            for (unsigned int j = 0; j < n; j++)
                dst->_data[i * n + j] = mat._data[i * 2 * n + n + j];
        return dst;
    }

    void swap_lines(unsigned int i, unsigned int j) {
        T tmp;
        for (unsigned int k = 0; k < ncols; k++) {
            tmp = data[i * ncols + k];
            data[i * ncols + k] = data[j * ncols + k];
            data[j * ncols + k] = tmp;
        }
    }

    void swap_columns(unsigned int i, unsigned int j) {
        T tmp;
        for (unsigned int k = 0; k < nrows; k++) {
            tmp = data[k * ncols + i];
            data[k * ncols + i] = data[k * ncols + j];
            data[k * ncols + j] = tmp;
        }
    }
};
}

#endif