lib_math/labo01/Operators.h

#pragma once

/**
 * @file Operators.h
 *
 * @brief Opérateurs arithmétiques pour les matrices et les vecteurs.
 *
 * Nom: William Nolin
 * Code permanent : NOLW76060101
 * Email : william.nolin.1@ens.etsmtl.ca
 *
 */

#include "Matrix.h"
#include "Vector.h"

/**
 * Implémentation de divers opérateurs arithmétiques pour les matrices et les vecteurs.
 */
namespace gti320 {

    /**
     * Multiplication : Matrice * Matrice (générique)
     */
    template<typename _Scalar, int RowsA, int ColsA, int StorageA, int RowsB, int ColsB, int StorageB>
    Matrix<_Scalar, RowsA, ColsB>
    operator*(const Matrix<_Scalar, RowsA, ColsA, StorageA> &A, const Matrix<_Scalar, RowsB, ColsB, StorageB> &B) {
        assert(A.cols() == B.rows());

        auto result = Matrix<_Scalar, Dynamic, Dynamic>(A.rows(), B.cols());

        for (int col = 0; col < B.cols(); col++) {
            for (int row = 0; row < A.rows(); row++) {
                for (int k = 0; k < A.cols(); k++) {
                    result(row, col) += A(row, k) * B(k, col);
                }
            }
        }

        return result;
    }

    /**
     * Multiplication : Matrice (colonne) * Matrice (ligne)
     *
     * Spécialisation de l'opérateur de multiplication pour le cas où les matrices
     * ont un stockage à taille dynamique et où la matrice de gauche utilise un
     * stockage par colonnes et celle de droite un stockage par lignes.
     */
    template<typename _Scalar>
    Matrix<_Scalar, Dynamic, Dynamic> operator*(const Matrix<_Scalar, Dynamic, Dynamic, ColumnStorage> &A,
                                                const Matrix<_Scalar, Dynamic, Dynamic, RowStorage> &B) {
        assert(A.cols() == B.rows());

        auto result = Matrix<_Scalar, Dynamic, Dynamic>(A.rows(), B.cols());

        for (int col = 0; col < A.cols(); col++) {
            for (int row = 0; row < B.rows(); row++) {
                for (int k = 0; k < B.cols(); k++) {
                    result(row, k) += A(row, col) * B(col, k);
                }
            }
        }

        return result;
    }

    /**
     * Multiplication : Matrice (ligne) * Matrice (colonne)
     *
     * Spécialisation de l'opérateur de multiplication pour le cas où les matrices
     * ont un stockage à taille dynamique et où la matrice de gauche utilise un
     * stockage par lignes et celle de droite un stockage par colonnes.
     */
    template<typename _Scalar>
    Matrix<_Scalar, Dynamic, Dynamic> operator*(const Matrix<_Scalar, Dynamic, Dynamic, RowStorage> &A,
                                                const Matrix<_Scalar, Dynamic, Dynamic, ColumnStorage> &B) {
        assert(A.cols() == B.rows());

        auto result = Matrix<_Scalar, Dynamic, Dynamic>(A.rows(), B.cols());

        for (int col = 0; col < B.cols(); col++) {
            for (int row = 0; row < A.rows(); row++) {
                for (int k = 0; k < A.cols(); k++) {
                    result(row, col) += A(row, k) * B(k, col);
                }
            }
        }

        return result;
    }


    /**
     * Addition : Matrice + Matrice (générique)
     */
    template<typename _Scalar, int Rows, int Cols, int StorageA, int StorageB>
    Matrix<_Scalar, Rows, Cols>
    operator+(const Matrix<_Scalar, Rows, Cols, StorageA> &A, const Matrix<_Scalar, Rows, Cols, StorageB> &B) {
        assert(A.rows() == B.rows());
        assert(A.cols() == B.cols());

        auto result = Matrix<_Scalar, Rows, Cols, StorageA>(A.rows(), A.cols());

        for (int row = 0; row < A.rows(); row++) {
            for (int col = 0; col < A.cols(); col++) {
                result(row, col) = A(row, col) + B(row, col);
            }
        }

        return result;
    }

    /**
     * Addition : Matrice (colonne) + Matrice (colonne)
     *
     * Spécialisation de l'opérateur d'addition pour le cas où les deux matrices
     * sont stockées par colonnes.
     */
    template<typename _Scalar>
    Matrix<_Scalar, Dynamic, Dynamic> operator+(const Matrix<_Scalar, Dynamic, Dynamic, ColumnStorage> &A,
                                                const Matrix<_Scalar, Dynamic, Dynamic, ColumnStorage> &B) {
        assert(A.rows() == B.rows());
        assert(A.cols() == B.cols());

        auto result = Matrix<_Scalar, Dynamic, Dynamic, ColumnStorage>(A.rows(), A.cols());

        for (int col = 0; col < A.cols(); col++) {
            for (int row = 0; row < A.rows(); row++) {
                result(row, col) = A(row, col) + B(row, col);
            }
        }

        return result;
    }

    /**
     * Addition : Matrice (ligne) + Matrice (ligne)
     *
     * Spécialisation de l'opérateur d'addition pour le cas où les deux matrices
     * sont stockées par lignes.
     */
    template<typename _Scalar>
    Matrix<_Scalar, Dynamic, Dynamic, RowStorage> operator+(const Matrix<_Scalar, Dynamic, Dynamic, RowStorage> &A,
                                                            const Matrix<_Scalar, Dynamic, Dynamic, RowStorage> &B) {
        assert(A.rows() == B.rows());
        assert(A.cols() == B.cols());

        auto result = Matrix<_Scalar, Dynamic, Dynamic, RowStorage>(A.rows(), A.cols());

        for (int row = 0; row < A.rows(); row++) {
            for (int col = 0; col < A.cols(); col++) {
                result(row, col) = A(row, col) + B(row, col);
            }
        }

        return result;
    }

    /**
     * Multiplication  : Scalaire * Matrice (colonne)
     *
     * Spécialisation de l'opérateur de multiplication par un scalaire pour le
     * cas d'une matrice stockée par colonnes.
     */
    template<typename _Scalar, int _Rows, int _Cols>
    Matrix<_Scalar, _Rows, _Cols, ColumnStorage>
    operator*(const _Scalar &a, const Matrix<_Scalar, _Rows, _Cols, ColumnStorage> &A) {
        auto result = Matrix<_Scalar, _Rows, _Cols, ColumnStorage>(A.rows(), A.cols());

        for (int col = 0; col < A.cols(); col++) {
            for (int row = 0; row < A.rows(); row++) {
                result(col, row) = a * A(col, row);
            }
        }

        return result;
    }

    /**
     * Multiplication  : Scalaire * Matrice (ligne)
     *
     * Spécialisation de l'opérateur de multiplication par un scalaire pour le
     * cas d'une matrice stockée par lignes.
     */
    template<typename _Scalar, int _Rows, int _Cols>
    Matrix<_Scalar, _Rows, _Cols, RowStorage>
    operator*(const _Scalar &a, const Matrix<_Scalar, _Rows, _Cols, RowStorage> &A) {
        auto result = Matrix<_Scalar, _Rows, _Cols, RowStorage>(A.rows(), A.cols());

        for (int row = 0; row < A.rows(); row++) {
            for (int col = 0; col < A.cols(); col++) {
                result(col, row) = a * A(col, row);
            }
        }

        return result;
    }

    /**
     * Multiplication : Matrice (ligne) * Vecteur
     *
     * Spécialisation de l'opérateur de multiplication matrice*vecteur pour le
     * cas où la matrice est représentée par lignes.
     */
    template<typename _Scalar, int _Rows, int _Cols>
    Vector<_Scalar, _Rows>
    operator*(const Matrix<_Scalar, _Rows, _Cols, RowStorage> &A, const Vector<_Scalar, _Cols> &v) {
        assert(A.cols() == v.size());

        auto result = Vector<_Scalar, _Rows>(A.rows());

        for (int row = 0; row < A.rows(); row++) {
            _Scalar dotP = 0;

            for (int col = 0; col < A.cols(); col++) {
                dotP += A(col, row) * v(col);
            }

            result(row) = dotP;
        }

        return result;
    }

    /**
     * Multiplication : Matrice (colonne) * Vecteur
     *
     * Spécialisation de l'opérateur de multiplication matrice*vecteur pour le
     * cas où la matrice est représentée par colonnes.
     */
    template<typename _Scalar, int _Rows, int _Cols>
    Vector<_Scalar, _Rows>
    operator*(const Matrix<_Scalar, _Rows, _Cols, ColumnStorage> &A, const Vector<_Scalar, _Cols> &v) {
        assert(A.cols() == v.size());

        auto result = Vector<_Scalar, _Rows>(A.rows());

        for (int col = 0; col < A.cols(); col++) {
            _Scalar v_col = v(col);

            for (int row = 0; row < A.rows(); row++) {
                result(row) += A(row, col) * v_col;
            }
        }

        return result;
    }

    /**
     * Multiplication : Scalaire * Vecteur
     */
    template<typename _Scalar, int _Rows>
    Vector<_Scalar, _Rows> operator*(const _Scalar &a, const Vector<_Scalar, _Rows> &v) {
        auto result = Vector<_Scalar, _Rows>(v.rows());

        for (int row = 0; row < v.rows(); row++) {
            result(row) = a * v(row);
        }

        return result;
    }


    /**
     * Addition : Vecteur + Vecteur
     */
    template<typename _Scalar, int _RowsA, int _RowsB>
    Vector<_Scalar, _RowsA> operator+(const Vector<_Scalar, _RowsA> &a, const Vector<_Scalar, _RowsB> &b) {
        assert(a.rows() == b.rows());

        auto result = Vector<_Scalar, _RowsA>(a.rows());

        for (int row = 0; row < a.rows(); row++) {
            result(row) = a(row) + b(row);
        }

        return result;
    }

    /**
     * Soustraction : Vecteur - Vecteur
     */
    template<typename _Scalar, int _RowsA, int _RowsB>
    Vector<_Scalar, _RowsA> operator-(const Vector<_Scalar, _RowsA> &a, const Vector<_Scalar, _RowsB> &b) {
        assert(a.rows() == b.rows());

        auto result = Vector<_Scalar, _RowsA>(a.rows());

        for (int row = 0; row < a.rows(); row++) {
            result(row) = a(row) - b(row);
        }

        return result;
    }

    template<typename _Scalar, int _Rows, int _Cols>
    void print_matrix(const Matrix<_Scalar, _Rows, _Cols> &m) {
        std::cout << "+----" << '\n';
        std::cout.precision(2);
        for (int row = 0; row < m.rows(); row++) {
            for (int col = 0; col < m.cols(); col++) {
                std::cout << m(row, col) << "," << "\t";
            }
            std::cout << "\n";
        }
    }

    template<typename _Scalar, int _Rows, int _Cols>
    bool is_identity(const Matrix<_Scalar, _Rows, _Cols> &m) {
        for (int i = 0; i < m.rows(); i++) {
            if (m(i, i) != 1) {
                return false;
            }
        }

        return true;
    }

    template<typename _Scalar, int _Rows, int _Cols>
    bool is_triangle(const Matrix<_Scalar, _Rows, _Cols> &m) {
        int n = m.rows();

        int lower, upper = 0;
        int required_zero = 0;
        for (int i = 1; i < n; i++) {
            required_zero += i;
        }

        for (int i = 0; i < n; i++) {
            for (int j = 0; j < n; j++) {
                if (i < j && m(i, j) == 0) {
                    upper++;
                }

                if (j < i && m(i, j) == 0) {
                    lower++;
                }

                if (lower == n || upper == n) {
                    return true;
                }
            }
        }

        return false;
    }

    // Calcul générique du déterminant
    template<typename _Scalar, int _Rows, int _Cols>
    double det(const Matrix<_Scalar, _Rows, _Cols> &m) {
        assert(m.rows() == m.cols());

        // Cherche d'abord pour un algorithme plus simple
        if (is_identity(m)) {
            return 1;
        }
        if (is_triangle(m)) {
            return det_tri(m);
        }

        int n = m.rows();
        auto l = Matrix<double, _Rows, _Cols>();
        auto u = Matrix<double, _Rows, _Cols>();

        for (int i = 0; i < n; i++) {
            for (int j = 0; j < n; j++) {
                // L
                if (i < j) {
                    l(i, j) = 0;
                } else if (j == i) {
                    l(i, j) = 1;
                } else {
                    l(i, j) = m(i, j) / u(j, j);
                    for (int k = 0; k < j; k++) {
                        l(i, j) -= (u(k, j) * l(i, k)) / u(j, j);
                    }
                }

                // U
                if (j < i) {
                    u(i, j) = 0;
                } else {
                    u(i, j) = m(i, j);
                    for (int k = 0; k < i; k++) {
                        u(i, j) -= l(i, k) * u(k, j);
                    }
                }
            }
        }

        // Det(L) = 1, on peut l'ignorer
        return det_tri(u);
    }

    // Déterminant d'une matrice 1x1
    template<typename _Scalar>
    double det(const Matrix<_Scalar, 1, 1> &m) {
        return m(0, 0);
    }

    // Déterminant d'une matrice 2x2
    template<typename _Scalar>
    double det(const Matrix<_Scalar, 2, 2> &m) {
        _Scalar a = m(0, 0);
        _Scalar b = m(0, 1);
        _Scalar c = m(1, 0);
        _Scalar d = m(1, 1);

        return (a * d) - (b * c);
    }

    // Déterminant d'une matrice triangulaire
    template<typename _Scalar, int _Rows, int _Cols>
    double det_tri(const Matrix<_Scalar, _Rows, _Cols> &m) {
        int n = std::min(m.rows(), m.cols());

        double det = 1;
        for (int i = 0; i < n; i++) {
            det *= m(i, i);
        }

        return det;
    }
}