Task: Gauss-Jordan matrix inversion


C++: #include <algorithm>
#include <cassert>
#include <iomanip>
#include <iostream>
#include <vector>

template <typename scalar_type> class matrix {
public:
    matrix(size_t rows, size_t columns)
        : rows_(rows), columns_(columns), elements_(rows * columns) {}

    matrix(size_t rows, size_t columns, scalar_type value)
        : rows_(rows), columns_(columns), elements_(rows * columns, value) {}

    matrix(size_t rows, size_t columns, std::initializer_list<scalar_type> values)
        : rows_(rows), columns_(columns), elements_(rows * columns) {
        assert(values.size() <= rows_ * columns_);
        std::copy(values.begin(), values.end(), elements_.begin());
    }

    size_t rows() const { return rows_; }
    size_t columns() const { return columns_; }

    scalar_type& operator()(size_t row, size_t column) {
        assert(row < rows_);
        assert(column < columns_);
        return elements_[row * columns_ + column];
    }
    const scalar_type& operator()(size_t row, size_t column) const {
        assert(row < rows_);
        assert(column < columns_);
        return elements_[row * columns_ + column];
    }
private:
    size_t rows_;
    size_t columns_;
    std::vector<scalar_type> elements_;
};

template <typename scalar_type>
matrix<scalar_type> product(const matrix<scalar_type>& a,
                            const matrix<scalar_type>& b) {
    assert(a.columns() == b.rows());
    size_t arows = a.rows();
    size_t bcolumns = b.columns();
    size_t n = a.columns();
    matrix<scalar_type> c(arows, bcolumns);
    for (size_t i = 0; i < arows; ++i) {
        for (size_t j = 0; j < n; ++j) {
            for (size_t k = 0; k < bcolumns; ++k)
                c(i, k) += a(i, j) * b(j, k);
        }
    }
    return c;
}

template <typename scalar_type>
void swap_rows(matrix<scalar_type>& m, size_t i, size_t j) {
    size_t columns = m.columns();
    for (size_t column = 0; column < columns; ++column)
        std::swap(m(i, column), m(j, column));
}


template <typename scalar_type>
void rref(matrix<scalar_type>& m) {
    size_t rows = m.rows();
    size_t columns = m.columns();
    for (size_t row = 0, lead = 0; row < rows && lead < columns; ++row, ++lead) {
        size_t i = row;
        while (m(i, lead) == 0) {
            if (++i == rows) {
                i = row;
                if (++lead == columns)
                    return;
            }
        }
        swap_rows(m, i, row);
        if (m(row, lead) != 0) {
            scalar_type f = m(row, lead);
            for (size_t column = 0; column < columns; ++column)
                m(row, column) /= f;
        }
        for (size_t j = 0; j < rows; ++j) {
            if (j == row)
                continue;
            scalar_type f = m(j, lead);
            for (size_t column = 0; column < columns; ++column)
                m(j, column) -= f * m(row, column);
        }
    }
}

template <typename scalar_type>
matrix<scalar_type> inverse(const matrix<scalar_type>& m) {
    assert(m.rows() == m.columns());
    size_t rows = m.rows();
    matrix<scalar_type> tmp(rows, 2 * rows, 0);
    for (size_t row = 0; row < rows; ++row) {
        for (size_t column = 0; column < rows; ++column)
            tmp(row, column) = m(row, column);
        tmp(row, row + rows) = 1;
    }
    rref(tmp);
    matrix<scalar_type> inv(rows, rows);
    for (size_t row = 0; row < rows; ++row) {
        for (size_t column = 0; column < rows; ++column)
            inv(row, column) = tmp(row, column + rows);
    }
    return inv;
}

template <typename scalar_type>
void print(std::ostream& out, const matrix<scalar_type>& m) {
    size_t rows = m.rows(), columns = m.columns();
    out << std::fixed << std::setprecision(4);
    for (size_t row = 0; row < rows; ++row) {
        for (size_t column = 0; column < columns; ++column) {
            if (column > 0)
                out << ' ';
            out << std::setw(7) << m(row, column);
        }
        out << '\n';
    }
}

int main() {
    matrix<double> m(3, 3, {2, -1, 0, -1, 2, -1, 0, -1, 2});
    std::cout << "Matrix:\n";
    print(std::cout, m);
    auto inv(inverse(m));
    std::cout << "Inverse:\n";
    print(std::cout, inv);
    std::cout << "Product of matrix and inverse:\n";
    print(std::cout, product(m, inv));
    std::cout << "Inverse of inverse:\n";
    print(std::cout, inverse(inv));
    return 0;
}


C#: using System;

namespace Rosetta
{
    internal class Vector
    {
        private double[] b;
        internal readonly int rows;

        internal Vector(int rows)
        {
            this.rows = rows;
            b = new double[rows];
        }

        internal Vector(double[] initArray)
        {
            b = (double[])initArray.Clone();
            rows = b.Length;
        }

        internal Vector Clone()
        {
            Vector v = new Vector(b);
            return v;
        }

        internal double this[int row]
        {
            get { return b[row]; }
            set { b[row] = value; }
        }

        internal void SwapRows(int r1, int r2)
        {
            if (r1 == r2) return;
            double tmp = b[r1];
            b[r1] = b[r2];
            b[r2] = tmp;
        }

        internal double norm(double[] weights)
        {
            double sum = 0;
            for (int i = 0; i < rows; i++)
            {
                double d = b[i] * weights[i];
                sum +=  d*d;
            }
            return Math.Sqrt(sum);
        }

        internal void print()
        {
            for (int i = 0; i < rows; i++)
                Console.WriteLine(b[i]);
            Console.WriteLine();
        }

        public static Vector operator-(Vector lhs, Vector rhs)
        {
            Vector v = new Vector(lhs.rows);
            for (int i = 0; i < lhs.rows; i++)
                v[i] = lhs[i] - rhs[i];
            return v;
        }
    }

    class Matrix
    {
        private double[] b;
        internal readonly int rows, cols;

        internal Matrix(int rows, int cols)
        {
            this.rows = rows;
            this.cols = cols;
            b = new double[rows * cols];            
        }

        internal Matrix(int size)
        {
            this.rows = size;
            this.cols = size;
            b = new double[rows * cols];
            for (int i = 0; i < size; i++)
                this[i, i] = 1;
        }

        internal Matrix(int rows, int cols, double[] initArray)
        {
            this.rows = rows;
            this.cols = cols;
            b = (double[])initArray.Clone();
            if (b.Length != rows * cols) throw new Exception("bad init array");
        }

        internal double this[int row, int col]
        {
            get { return b[row * cols + col]; }
            set { b[row * cols + col] = value; }
        }        
        
        public static Vector operator*(Matrix lhs, Vector rhs)
        {
            if (lhs.cols != rhs.rows) throw new Exception("I can't multiply matrix by vector");
            Vector v = new Vector(lhs.rows);
            for (int i = 0; i < lhs.rows; i++)
            {
                double sum = 0;
                for (int j = 0; j < rhs.rows; j++)
                    sum += lhs[i,j]*rhs[j];
                v[i] = sum;
            }
            return v;
        }

        internal void SwapRows(int r1, int r2)
        {
            if (r1 == r2) return;
            int firstR1 = r1 * cols;
            int firstR2 = r2 * cols;
            for (int i = 0; i < cols; i++)
            {
                double tmp = b[firstR1 + i];
                b[firstR1 + i] = b[firstR2 + i];
                b[firstR2 + i] = tmp;
            }
        }

        
        internal bool InvPartial()
        {
            const double Eps = 1e-12;
            if (rows != cols) throw new Exception("rows != cols for Inv");
            Matrix M = new Matrix(rows); 
            for (int diag = 0; diag < rows; diag++)
            {
                int max_row = diag;
                double max_val = Math.Abs(this[diag, diag]);
                double d;
                for (int row = diag + 1; row < rows; row++)
                    if ((d = Math.Abs(this[row, diag])) > max_val)
                    {
                        max_row = row;
                        max_val = d;
                    }
                if (max_val <= Eps) return false;
                SwapRows(diag, max_row);
                M.SwapRows(diag, max_row);
                double invd = 1 / this[diag, diag];
                for (int col = diag; col < cols; col++)
                {
                    this[diag, col] *= invd;
                }
                for (int col = 0; col < cols; col++)
                {
                    M[diag, col] *= invd;
                }
                for (int row = 0; row < rows; row++)
                {
                    d = this[row, diag];
                    if (row != diag)
                    {
                        for (int col = diag; col < this.cols; col++)
                        {
                            this[row, col] -= d * this[diag, col];
                        }
                        for (int col = 0; col < this.cols; col++)
                        {
                            M[row, col] -= d * M[diag, col];
                        }
                    }
                }
            }
            b = M.b;
            return true;
        }

        internal void print()
        {
            for (int i = 0; i < rows; i++)
            {
                for (int j = 0; j < cols; j++)
                    Console.Write(this[i,j].ToString()+"  ");
                Console.WriteLine();
            }
        }
    }
}