psemek/libs/geom/include/psemek/geom/matrix.hpp

#pragma once

#include <psemek/geom/detail/array.hpp>
#include <psemek/geom/vector.hpp>
#include <psemek/geom/math.hpp>

#include <algorithm>

namespace psemek::geom
{

	template <typename T, std::size_t R, std::size_t C>
	struct matrix
	{
		static constexpr std::size_t rows = R;
		static constexpr std::size_t columns = C;

		using scalar_type = T;

		detail::array<T, R * C>::type coords;

		auto operator[](std::size_t i)
		{
			return coords + C * i;
		}

		auto operator[](std::size_t i) const
		{
			return coords + C * i;
		}

		matrix & operator *= (T const & s);
		matrix & operator /= (T const & s);

		matrix & operator += (matrix const & v);
		matrix & operator -= (matrix const & v);

		static matrix zero();
		static matrix identity();
		static matrix scalar(T const & s);
	};

	template <typename T, std::size_t R, std::size_t C>
	matrix<T, R, C> matrix<T, R, C>::zero()
	{
		matrix<T, R, C> m;
		for (std::size_t i = 0; i < R * C; ++i)
			m.coords[i] = 0;
		return m;
	}

	template <typename T, std::size_t R, std::size_t C>
	matrix<T, R, C> matrix<T, R, C>::identity()
	{
		return scalar(T{1});
	}

	template <typename T, std::size_t R, std::size_t C>
	matrix<T, R, C> matrix<T, R, C>::scalar(T const & s)
	{
		matrix<T, R, C> m = zero();
		for (std::size_t i = 0; i < std::min(R, C); ++i)
			for (std::size_t j = 0; j < C; ++j)
				m[i][i] = s;
		return m;
	}

	template <typename T, std::size_t R, std::size_t C>
	bool operator == (matrix<T, R, C> const & m1, matrix<T, R, C> const & m2)
	{
		return std::equal(m1.coords, m1.coords + R * C, m2.coords);
	}

	template <typename T, std::size_t R, std::size_t C>
	bool operator != (matrix<T, R, C> const & m1, matrix<T, R, C> const & m2)
	{
		return !(m1 == m2);
	}

	template <typename T, std::size_t R, std::size_t C>
	bool operator < (matrix<T, R, C> const & m1, matrix<T, R, C> const & m2)
	{
		return std::lexicographical_compare(m1.coords, m1.coords + R * C, m2.coords, m2.coords + R * C);
	}

	template <typename T, std::size_t R, std::size_t C>
	bool operator > (matrix<T, R, C> const & m1, matrix<T, R, C> const & m2)
	{
		return m2 < m1;
	}

	template <typename T, std::size_t R, std::size_t C>
	bool operator <= (matrix<T, R, C> const & m1, matrix<T, R, C> const & m2)
	{
		return !(m2 < m1);
	}

	template <typename T, std::size_t R, std::size_t C>
	bool operator >= (matrix<T, R, C> const & m1, matrix<T, R, C> const & m2)
	{
		return !(m1 < m2);
	}

	template <typename T1, typename T, std::size_t R, std::size_t C>
	matrix<T1, R, C> cast(matrix<T, R, C> const & m)
	{
		matrix<T1, R, C> r;
		for (std::size_t i = 0; i < R * C; ++i)
			r.coords[i] = static_cast<T1>(m.coords[i]);
		return r;
	}

	template <typename T, std::size_t R, std::size_t C>
	matrix<T, R, C> operator * (matrix<T, R, C> const & m, T const & s)
	{
		matrix<T, R, C> r;
		for (std::size_t i = 0; i < R * C; ++i)
			r.coords[i] = m.coords[i] * s;
		return r;
	}

	template <typename T, std::size_t R, std::size_t C>
	matrix<T, R, C> operator * (T const & s, matrix<T, R, C> const & m)
	{
		matrix<T, R, C> r;
		for (std::size_t i = 0; i < R * C; ++i)
			r.coords[i] = s * m.coords[i];
		return r;
	}

	template <typename T, std::size_t R, std::size_t C>
	matrix<T, R, C> operator / (matrix<T, R, C> const & m, T const & s)
	{
		matrix<T, R, C> r;
		for (std::size_t i = 0; i < R * C; ++i)
			r.coords[i] = m.coords[i] / s;
		return r;
	}

	template <typename T, std::size_t R, std::size_t C>
	matrix<T, R, C> & matrix<T, R, C>::operator *= (T const & s)
	{
		*this = *this * s;
		return *this;
	}

	template <typename T, std::size_t R, std::size_t C>
	matrix<T, R, C> & matrix<T, R, C>::operator /= (T const & s)
	{
		*this = *this / s;
		return *this;
	}

	template <typename T, std::size_t R, std::size_t C>
	matrix<T, R, C> operator - (matrix<T, R, C> const & m)
	{
		matrix<T, R, C> r;
		for (std::size_t i = 0; i < R * C; ++i)
			r.coords[i] = -m.coords[i];
		return r;
	}

	template <typename T, std::size_t R, std::size_t C>
	matrix<T, R, C> operator + (matrix<T, R, C> const & m1, matrix<T, R, C> const & m2)
	{
		matrix<T, R, C> r;
		for (std::size_t i = 0; i < R * C; ++i)
			r.coords[i] = m1.coords[i] + m2.coords[i];
		return r;
	}

	template <typename T, std::size_t R, std::size_t C>
	matrix<T, R, C> operator - (matrix<T, R, C> const & m1, matrix<T, R, C> const & m2)
	{
		matrix<T, R, C> r;
		for (std::size_t i = 0; i < R * C; ++i)
			r.coords[i] = m1.coords[i] - m2.coords[i];
		return r;
	}

	template <typename T, std::size_t R, std::size_t C>
	matrix<T, R, C> & matrix<T, R, C>::operator += (matrix<T, R, C> const & m)
	{
		*this = *this + m;
		return *this;
	}

	template <typename T, std::size_t R, std::size_t C>
	matrix<T, R, C> & matrix<T, R, C>::operator -= (matrix<T, R, C> const & m)
	{
		*this = *this - m;
		return *this;
	}

	template <typename T, std::size_t R, std::size_t C>
	vector<T, R> operator * (matrix<T, R, C> const & m, vector<T, C> const & v)
	{
		vector<T, R> r;
		for (std::size_t i = 0; i < R; ++i)
		{
			r[i] = T{};
			for (std::size_t j = 0; j < C; ++j)
				r[i] += m[i][j] * v[j];
		}
		return r;
	}

	template <typename T, std::size_t R, std::size_t C>
	vector<T, C> operator * (vector<T, R> const & v, matrix<T, R, C> const & m)
	{
		vector<T, C> r;
		for (std::size_t j = 0; j < C; ++j)
		{
			r[j] = T{};
			for (std::size_t i = 0; i < R; ++i)
				r[i] += v[j] * m[i][j];
		}
		return r;
	}

	template <typename T, std::size_t R, std::size_t K, std::size_t C>
	matrix<T, R, C> operator * (matrix<T, R, K> const & m1, matrix<T, K, C> const & m2)
	{
		matrix<T, R, C> r;
		for (std::size_t i = 0; i < R; ++i)
		{
			for (std::size_t j = 0; j < C; ++j)
			{
				r[i][j] = T{};

				for (std::size_t k = 0; k < K; ++k)
					r[i][j] += m1[i][k] * m2[k][j];
			}
		}
		return r;
	}

	template <typename T, std::size_t R, std::size_t C>
	matrix<T, C, R> transpose(matrix<T, R, C> const & m)
	{
		matrix<T, C, R> r;
		for (std::size_t i = 0; i < R; ++i)
			for (std::size_t j = 0; j < C; ++j)
				r[j][i] = m[i][j];
		return r;
	}

	template <typename T, std::size_t N, typename ... Rows>
	auto by_rows(vector<T, N> const & v, Rows const & ... rows)
	{
		vector<T, N> m[] = {v, rows...};

		matrix<T, sizeof...(Rows) + 1, N> result;
		for (std::size_t i = 0; i < result.rows; ++i)
			for (std::size_t j = 0; j < result.columns; ++j)
				result[i][j] = m[i][j];
		return result;
	}

	template <typename T, std::size_t N, typename ... Columns>
	auto by_columns(vector<T, N> const & v, Columns const & ... columns)
	{
		vector<T, N> m[] = {v, columns...};

		matrix<T, N, sizeof...(Columns) + 1> result;
		for (std::size_t i = 0; i < result.rows; ++i)
			for (std::size_t j = 0; j < result.columns; ++j)
				result[i][j] = m[j][i];
		return result;
	}

	template <typename T, std::size_t R, std::size_t C>
	T frobenius_norm_sqr(matrix<T, R, C> const & m)
	{
		T r{0};
		for (std::size_t i = 0; i < R; ++i)
			for (std::size_t j = 0; j < C; ++j)
				r += sqr(m[i][j]);
		return r;
	}

	template <typename T, std::size_t R, std::size_t C>
	T frobenius_norm(matrix<T, R, C> const & m)
	{
		return std::sqrt(frobenius_norm_sqr(m));
	}

	template <typename T, std::size_t R, std::size_t C>
	T trace(matrix<T, R, C> const & m)
	{
		T r{0};
		for (std::size_t i = 0; i < std::min(R, C); ++i)
			r += m[i][i];
		return r;
	}

	template <typename Stream, typename T, std::size_t R, std::size_t C>
	Stream & operator << (Stream & os, matrix<T, R, C> const & m)
	{
		for (std::size_t i = 0; i < R; ++i)
		{
			for (std::size_t j = 0; j < C; ++j)
				os << m[i][j] << ' ';
			os << '\n';
		}
		return os;
	}

}